nxtOSEK/samples_c/nxte22/nxte22.c

   1 /**\r
   2  *  nxte22.c \r
   3  * \r
   4  * This is a sample program for LATTEBOX NXTe(NXT Extension Kit Product)\r
   5  * and LSC22(22-Axis Servo Kit Product)\r
   6  * <http://www.lattebox.com/product_list_e.php?pts_type=1>\r
   7  * \r
   8  * It enables a NXT to control RC servo motors via I2C.\r
   9  * Maximum number of RC servos to be controlled is:\r
  10  * Number of NXT Sensor ports(4) * Number of NXTe channels (4) * Number of LSC22 channels (22)\r
  11  * = 352 RC Servos might be controlled by using a NXT!\r
  12  * \r
  13  * Special thanks to Yu Yang <sog@lattebox.com>\r
  14  */ \r
  15 \r
  16 #include "ecrobot_interface.h"\r
  17 #include "nxte22.h"\r
  18 \r
  19 const U8 I2C_SC18IS602_F0[2] = {0xF0, 0x0C}; /* Configure SPI Interface - Function ID F0h SPI Mode:3, 461kHz */\r
  20 \r
  21 /* NXTe channel configuration parameters for LSC */\r
  22 const U8 NXTe_CH[MAX_NXTe_CH] = {0x01, 0x02, 0x04, 0x08};\r
  23  \r
  24 /**\r
  25  * init NXTe22. \r
  26  * NOTE that the specified port has to be initialized for I2C (LOWSPEED_9V) before invoking this.\r
  27  * \r
  28  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4 \r
  29  */\r
  30 void init_NXTe22(U8 port_id)\r
  31 {\r
  32         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, I2C_SC18IS602_F0[0], (U8 *)&I2C_SC18IS602_F0[1], 1);\r
  33 }\r
  34 \r
  35 /**\r
  36  * synchronize NXTe and LSC22 connected RC servos\r
  37  * \r
  38  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4 \r
  39  * @param nxte_ch: NXTe channel (0x01/0x02/0x04/0x08)\r
  40  */\r
  41 void sync_NXTe22Servo(U8 port_id, U8 nxte_ch)\r
  42 {\r
  43         U8 snd_buf, rcv_buf;\r
  44         \r
  45         snd_buf = 0x00;\r
  46         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
  47         ecrobot_read_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &rcv_buf, 1);\r
  48         \r
  49         rcv_buf = 0;\r
  50         while(1)\r
  51         {\r
  52                 snd_buf = 0xFF; /* send twice */\r
  53                 ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
  54                 ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
  55                 snd_buf = 0x7F;\r
  56                 ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
  57 \r
  58                 snd_buf = 0x00;\r
  59                 ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
  60                 ecrobot_read_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &rcv_buf, 1);\r
  61                 \r
  62                 if (rcv_buf == 99) break;\r
  63         }\r
  64 }\r
  65 \r
  66 /**\r
  67  * load RC servo channels connected to the NXTe\r
  68  * \r
  69  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4 \r
  70  * @param nxte_ch: NXTe channel (0x01/0x02/0x04/0x08)\r
  71  * @param servo_ch_bit: each bit (0 to 22 bits) represents enable/disable for each servo control\r
  72  */\r
  73 void load_NXTe22Servo(U8 port_id, U8 nxte_ch, U32 servo_ch_bit)\r
  74 {\r
  75         U8 h_byte,l_byte;\r
  76         \r
  77         servo_ch_bit &= ALL_22SERVO_ENABLE; /* check servo channel range */\r
  78 \r
  79         /* send 31 - 16 bits */ \r
  80         h_byte = 0xDF;\r
  81         l_byte = (U8)((servo_ch_bit >> 16) | 0x80); /* 0x80: servo freq is fixed as 20MHz */\r
  82 \r
  83         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &h_byte, 1);\r
  84         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &l_byte, 1);\r
  85         \r
  86         /* send 15 - 0 bits */\r
  87         h_byte = (U8)(servo_ch_bit >> 8);\r
  88         l_byte = (U8)(servo_ch_bit & 0xFF);\r
  89 \r
  90         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &h_byte, 1);\r
  91         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &l_byte, 1);\r
  92 }\r
  93 \r
  94 static void sleep(U8 port_id, U8 nxte_ch)\r
  95 {\r
  96         U8 cmd = 0xFD;\r
  97         \r
  98         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &cmd, 1);\r
  99 }\r
 100 \r
 101 /**\r
 102  * check servo is moving\r
 103  * \r
 104  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4 \r
 105  * @param nxte_ch: NXTe channel (0x01/0x02/0x04/0x08)\r
 106  * @return: each bit (0 to 21 bits) represents moving(1)/not moving(0) of each servo\r
 107  */\r
 108 U32 read_NXTe22Motion(U8 port_id, U8 nxte_ch)\r
 109 {\r
 110         U8 snd_buf;\r
 111         U8 h_byte, m_byte, l_byte;\r
 112 \r
 113         sleep(port_id, nxte_ch);        \r
 114 \r
 115         snd_buf = 0x5D;\r
 116         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
 117 \r
 118         snd_buf = 0x00; \r
 119         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
 120         ecrobot_read_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &h_byte, 1);\r
 121 \r
 122         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
 123         ecrobot_read_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &m_byte, 1);\r
 124 \r
 125         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
 126         ecrobot_read_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &l_byte, 1);\r
 127 \r
 128         return (((U32)(h_byte & 0x7F) << 16) | ((U32)(m_byte & 0xFF) << 8) | (U32)(l_byte & 0xFF)); \r
 129 }\r
 130 \r
 131 /**\r
 132  * set delay to a servo\r
 133  * \r
 134  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4 \r
 135  * @param nxte_ch: NXTe channel (0x01/0x02/0x04/0x08)\r
 136  * @param servo_ch: Servo channel (0 to 21)\r
 137  * @param delay: control delay in msec(?)\r
 138  */\r
 139 void set_NXTe22Delay(U8 port_id, U8 nxte_ch, U8 servo_ch, U8 delay)\r
 140 {\r
 141         U8 h_byte,l_byte;\r
 142         \r
 143         h_byte = (0xE0 | servo_ch);\r
 144         l_byte = (U8)(delay & 0x0F);\r
 145 \r
 146         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &h_byte, 1);\r
 147         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &l_byte, 1);\r
 148 }\r
 149 \r
 150 /**\r
 151  * set angle value of a servo\r
 152  * \r
 153  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4 \r
 154  * @param nxte_ch: NXTe channel (0x01/0x02/0x04/0x08)\r
 155  * @param servo_ch: Servo channel (0 to 21)\r
 156  * @param angle: servo angle count\r
 157  */\r
 158 void set_NXTe22Angle(U8 port_id, U8 nxte_ch, U8 servo_ch, S16 angle)\r
 159 {\r
 160         U8 h_byte,l_byte;\r
 161         \r
 162         /* limit check */\r
 163         if (angle >= MAX_ANGLE)\r
 164         {\r
 165                 angle = MAX_ANGLE;\r
 166         }\r
 167         else if (angle <= MIN_ANGLE)\r
 168         {\r
 169                 angle = MIN_ANGLE;\r
 170         }\r
 171         \r
 172         h_byte = (U8)(0xC0 | servo_ch);\r
 173         l_byte = (U8)(0xF8 | (angle >> 8));\r
 174 \r
 175         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &h_byte, 1);\r
 176         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &l_byte, 1);\r
 177         \r
 178         h_byte = (U8)(angle & 0xFF);\r
 179         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &h_byte, 1);\r
 180 }\r
 181 \r
 182 /**\r
 183  * get angle value of a servo\r
 184  * \r
 185  * @param port_id: NXT_PORT_S1/NXT_PORT_S2/NXT_PORT_S3/NXT_PORT_S4 \r
 186  * @param nxte_ch: NXTe channel (0x01/0x02/0x04/0x08)\r
 187  * @param servo_ch: Servo channel (0 to 21)\r
 188  * @param angle: servo angle count\r
 189  */\r
 190 S16 get_NXTe22Angle(U8 port_id, U8 nxte_ch, U8 servo_ch)\r
 191 {\r
 192         U8 snd_buf;\r
 193         U8 h_byte, l_byte;\r
 194 \r
 195         sleep(port_id, nxte_ch);        \r
 196 \r
 197         h_byte = (U8)(servo_ch | 0x40);\r
 198         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &h_byte, 1);\r
 199 \r
 200         snd_buf = 0x00;\r
 201         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
 202         ecrobot_read_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &h_byte, 1);\r
 203         \r
 204         ecrobot_send_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &snd_buf, 1);\r
 205         ecrobot_read_i2c(port_id, NXTe_I2C_ADDR, nxte_ch, &l_byte, 1);\r
 206         \r
 207         return (((S16)(h_byte & 0x07 ) << 8) + (S16)l_byte);\r
 208 }\r