![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
123
Dependencies: 3W_8Dir_p2pcontrol mbed
Fork of DXL_SDK_For_F446RE by
dynamixel.cpp@4:44560391db99, 2017-02-08 (annotated)
- Committer:
- stanley1228
- Date:
- Wed Feb 08 15:49:35 2017 +0800
- Revision:
- 4:44560391db99
- Parent:
- 3:e2e9064c668d
- Child:
- 5:edccfcb47ab8
add ROM parameter setting to limit torque=55,multiturn offset=-1024,multiturn mode
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
stanley1228 | 0:bf4774b25188 | 1 | #include "dxl_hal.h" |
stanley1228 | 0:bf4774b25188 | 2 | #include "dynamixel.h" |
stanley1228 | 0:bf4774b25188 | 3 | |
stanley1228 | 0:bf4774b25188 | 4 | #define ID (2) |
stanley1228 | 0:bf4774b25188 | 5 | #define LENGTH (3) |
stanley1228 | 0:bf4774b25188 | 6 | #define INSTRUCTION (4) |
stanley1228 | 0:bf4774b25188 | 7 | #define ERRBIT (4) |
stanley1228 | 0:bf4774b25188 | 8 | #define PARAMETER (5) |
stanley1228 | 0:bf4774b25188 | 9 | #define DEFAULT_BAUDNUMBER (1) |
stanley1228 | 0:bf4774b25188 | 10 | |
stanley1228 | 0:bf4774b25188 | 11 | unsigned char gbInstructionPacket[MAXNUM_TXPARAM+10] = {0}; |
stanley1228 | 0:bf4774b25188 | 12 | unsigned char gbStatusPacket[MAXNUM_RXPARAM+10] = {0}; |
stanley1228 | 0:bf4774b25188 | 13 | unsigned char gbRxPacketLength = 0; |
stanley1228 | 0:bf4774b25188 | 14 | unsigned char gbRxGetLength = 0; |
stanley1228 | 0:bf4774b25188 | 15 | int gbCommStatus = COMM_RXSUCCESS; |
stanley1228 | 0:bf4774b25188 | 16 | int giBusUsing = 0; |
stanley1228 | 0:bf4774b25188 | 17 | |
stanley1228 | 0:bf4774b25188 | 18 | #include "mbed.h" |
stanley1228 | 0:bf4774b25188 | 19 | #include "Serial.h" |
stanley1228 | 0:bf4774b25188 | 20 | extern Serial pc;//stanley |
stanley1228 | 0:bf4774b25188 | 21 | |
stanley1228 | 0:bf4774b25188 | 22 | |
stanley1228 | 0:bf4774b25188 | 23 | //DigitalOut gTest(D4); |
stanley1228 | 0:bf4774b25188 | 24 | |
stanley1228 | 0:bf4774b25188 | 25 | int dxl_initialize( int devIndex, int baudnum ) |
stanley1228 | 0:bf4774b25188 | 26 | { |
stanley1228 | 0:bf4774b25188 | 27 | float baudrate; |
stanley1228 | 0:bf4774b25188 | 28 | baudrate = 2000000.0f / (float)(baudnum + 1); |
stanley1228 | 0:bf4774b25188 | 29 | |
stanley1228 | 0:bf4774b25188 | 30 | //gTest=0;//stanley |
stanley1228 | 0:bf4774b25188 | 31 | |
stanley1228 | 0:bf4774b25188 | 32 | if( dxl_hal_open(devIndex, baudrate) == 0 ) |
stanley1228 | 0:bf4774b25188 | 33 | return 0; |
stanley1228 | 0:bf4774b25188 | 34 | |
stanley1228 | 0:bf4774b25188 | 35 | gbCommStatus = COMM_RXSUCCESS; |
stanley1228 | 0:bf4774b25188 | 36 | giBusUsing = 0; |
stanley1228 | 0:bf4774b25188 | 37 | return 1; |
stanley1228 | 0:bf4774b25188 | 38 | } |
stanley1228 | 0:bf4774b25188 | 39 | |
stanley1228 | 0:bf4774b25188 | 40 | void dxl_terminate() |
stanley1228 | 0:bf4774b25188 | 41 | { |
stanley1228 | 0:bf4774b25188 | 42 | dxl_hal_close(); |
stanley1228 | 0:bf4774b25188 | 43 | } |
stanley1228 | 0:bf4774b25188 | 44 | |
stanley1228 | 0:bf4774b25188 | 45 | void dxl_tx_packet() |
stanley1228 | 0:bf4774b25188 | 46 | { |
stanley1228 | 0:bf4774b25188 | 47 | unsigned char i; |
stanley1228 | 0:bf4774b25188 | 48 | unsigned char TxNumByte, RealTxNumByte; |
stanley1228 | 0:bf4774b25188 | 49 | unsigned char checksum = 0; |
stanley1228 | 0:bf4774b25188 | 50 | |
stanley1228 | 0:bf4774b25188 | 51 | if( giBusUsing == 1 ) |
stanley1228 | 0:bf4774b25188 | 52 | return; |
stanley1228 | 0:bf4774b25188 | 53 | |
stanley1228 | 0:bf4774b25188 | 54 | giBusUsing = 1; |
stanley1228 | 0:bf4774b25188 | 55 | |
stanley1228 | 0:bf4774b25188 | 56 | if( gbInstructionPacket[LENGTH] > (MAXNUM_TXPARAM+2) ) |
stanley1228 | 0:bf4774b25188 | 57 | { |
stanley1228 | 0:bf4774b25188 | 58 | gbCommStatus = COMM_TXERROR; |
stanley1228 | 0:bf4774b25188 | 59 | giBusUsing = 0; |
stanley1228 | 0:bf4774b25188 | 60 | return; |
stanley1228 | 0:bf4774b25188 | 61 | } |
stanley1228 | 0:bf4774b25188 | 62 | |
stanley1228 | 0:bf4774b25188 | 63 | if( gbInstructionPacket[INSTRUCTION] != INST_PING |
stanley1228 | 0:bf4774b25188 | 64 | && gbInstructionPacket[INSTRUCTION] != INST_READ |
stanley1228 | 0:bf4774b25188 | 65 | && gbInstructionPacket[INSTRUCTION] != INST_WRITE |
stanley1228 | 0:bf4774b25188 | 66 | && gbInstructionPacket[INSTRUCTION] != INST_REG_WRITE |
stanley1228 | 0:bf4774b25188 | 67 | && gbInstructionPacket[INSTRUCTION] != INST_ACTION |
stanley1228 | 0:bf4774b25188 | 68 | && gbInstructionPacket[INSTRUCTION] != INST_RESET |
stanley1228 | 0:bf4774b25188 | 69 | && gbInstructionPacket[INSTRUCTION] != INST_SYNC_WRITE ) |
stanley1228 | 0:bf4774b25188 | 70 | { |
stanley1228 | 0:bf4774b25188 | 71 | gbCommStatus = COMM_TXERROR; |
stanley1228 | 0:bf4774b25188 | 72 | giBusUsing = 0; |
stanley1228 | 0:bf4774b25188 | 73 | return; |
stanley1228 | 0:bf4774b25188 | 74 | } |
stanley1228 | 0:bf4774b25188 | 75 | |
stanley1228 | 0:bf4774b25188 | 76 | gbInstructionPacket[0] = 0xff; |
stanley1228 | 0:bf4774b25188 | 77 | gbInstructionPacket[1] = 0xff; |
stanley1228 | 0:bf4774b25188 | 78 | for( i=0; i<(gbInstructionPacket[LENGTH]+1); i++ ) |
stanley1228 | 0:bf4774b25188 | 79 | checksum += gbInstructionPacket[i+2]; |
stanley1228 | 0:bf4774b25188 | 80 | gbInstructionPacket[gbInstructionPacket[LENGTH]+3] = ~checksum; |
stanley1228 | 0:bf4774b25188 | 81 | |
stanley1228 | 0:bf4774b25188 | 82 | if( gbCommStatus == COMM_RXTIMEOUT || gbCommStatus == COMM_RXCORRUPT ) |
stanley1228 | 0:bf4774b25188 | 83 | dxl_hal_clear(); |
stanley1228 | 0:bf4774b25188 | 84 | |
stanley1228 | 0:bf4774b25188 | 85 | TxNumByte = gbInstructionPacket[LENGTH] + 4; |
stanley1228 | 0:bf4774b25188 | 86 | RealTxNumByte = dxl_hal_tx( (unsigned char*)gbInstructionPacket, TxNumByte ); |
stanley1228 | 0:bf4774b25188 | 87 | |
stanley1228 | 0:bf4774b25188 | 88 | if( TxNumByte != RealTxNumByte ) |
stanley1228 | 0:bf4774b25188 | 89 | { |
stanley1228 | 0:bf4774b25188 | 90 | gbCommStatus = COMM_TXFAIL; |
stanley1228 | 0:bf4774b25188 | 91 | giBusUsing = 0; |
stanley1228 | 0:bf4774b25188 | 92 | return; |
stanley1228 | 0:bf4774b25188 | 93 | } |
stanley1228 | 0:bf4774b25188 | 94 | |
stanley1228 | 0:bf4774b25188 | 95 | if( gbInstructionPacket[INSTRUCTION] == INST_READ ) |
stanley1228 | 0:bf4774b25188 | 96 | dxl_hal_set_timeout( gbInstructionPacket[PARAMETER+1] + 6 ); |
stanley1228 | 0:bf4774b25188 | 97 | else |
stanley1228 | 0:bf4774b25188 | 98 | dxl_hal_set_timeout( 6 ); |
stanley1228 | 0:bf4774b25188 | 99 | |
stanley1228 | 0:bf4774b25188 | 100 | gbCommStatus = COMM_TXSUCCESS; |
stanley1228 | 0:bf4774b25188 | 101 | } |
stanley1228 | 0:bf4774b25188 | 102 | |
stanley1228 | 0:bf4774b25188 | 103 | void dxl_rx_packet() |
stanley1228 | 0:bf4774b25188 | 104 | { |
stanley1228 | 0:bf4774b25188 | 105 | unsigned char i, j, nRead; |
stanley1228 | 0:bf4774b25188 | 106 | unsigned char checksum = 0; |
stanley1228 | 0:bf4774b25188 | 107 | |
stanley1228 | 0:bf4774b25188 | 108 | if( giBusUsing == 0 ) |
stanley1228 | 0:bf4774b25188 | 109 | return; |
stanley1228 | 0:bf4774b25188 | 110 | |
stanley1228 | 0:bf4774b25188 | 111 | if( gbInstructionPacket[ID] == BROADCAST_ID ) |
stanley1228 | 0:bf4774b25188 | 112 | { |
stanley1228 | 0:bf4774b25188 | 113 | gbCommStatus = COMM_RXSUCCESS; |
stanley1228 | 0:bf4774b25188 | 114 | giBusUsing = 0; |
stanley1228 | 0:bf4774b25188 | 115 | return; |
stanley1228 | 0:bf4774b25188 | 116 | } |
stanley1228 | 0:bf4774b25188 | 117 | |
stanley1228 | 0:bf4774b25188 | 118 | if( gbCommStatus == COMM_TXSUCCESS ) |
stanley1228 | 0:bf4774b25188 | 119 | { |
stanley1228 | 0:bf4774b25188 | 120 | gbRxGetLength = 0; |
stanley1228 | 0:bf4774b25188 | 121 | gbRxPacketLength = 6; |
stanley1228 | 0:bf4774b25188 | 122 | } |
stanley1228 | 0:bf4774b25188 | 123 | |
stanley1228 | 0:bf4774b25188 | 124 | nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); |
stanley1228 | 0:bf4774b25188 | 125 | |
stanley1228 | 0:bf4774b25188 | 126 | gbRxGetLength += nRead; |
stanley1228 | 0:bf4774b25188 | 127 | if( gbRxGetLength < gbRxPacketLength ) |
stanley1228 | 0:bf4774b25188 | 128 | { |
stanley1228 | 0:bf4774b25188 | 129 | if( dxl_hal_timeout() == 1 ) |
stanley1228 | 0:bf4774b25188 | 130 | { |
stanley1228 | 0:bf4774b25188 | 131 | if(gbRxGetLength == 0) |
stanley1228 | 0:bf4774b25188 | 132 | gbCommStatus = COMM_RXTIMEOUT; |
stanley1228 | 0:bf4774b25188 | 133 | else |
stanley1228 | 0:bf4774b25188 | 134 | gbCommStatus = COMM_RXCORRUPT; |
stanley1228 | 0:bf4774b25188 | 135 | giBusUsing = 0; |
stanley1228 | 0:bf4774b25188 | 136 | return; |
stanley1228 | 0:bf4774b25188 | 137 | } |
stanley1228 | 0:bf4774b25188 | 138 | } |
stanley1228 | 0:bf4774b25188 | 139 | //pc.printf("133\n");//stanley |
stanley1228 | 0:bf4774b25188 | 140 | // Find packet header |
stanley1228 | 0:bf4774b25188 | 141 | for( i=0; i<(gbRxGetLength-1); i++ ) |
stanley1228 | 0:bf4774b25188 | 142 | { |
stanley1228 | 0:bf4774b25188 | 143 | if( gbStatusPacket[i] == 0xff && gbStatusPacket[i+1] == 0xff ) |
stanley1228 | 0:bf4774b25188 | 144 | { |
stanley1228 | 0:bf4774b25188 | 145 | break; |
stanley1228 | 0:bf4774b25188 | 146 | } |
stanley1228 | 0:bf4774b25188 | 147 | else if( i == gbRxGetLength-2 && gbStatusPacket[gbRxGetLength-1] == 0xff ) |
stanley1228 | 0:bf4774b25188 | 148 | { |
stanley1228 | 0:bf4774b25188 | 149 | break; |
stanley1228 | 0:bf4774b25188 | 150 | } |
stanley1228 | 0:bf4774b25188 | 151 | } |
stanley1228 | 0:bf4774b25188 | 152 | if( i > 0 ) |
stanley1228 | 0:bf4774b25188 | 153 | { |
stanley1228 | 0:bf4774b25188 | 154 | for( j=0; j<(gbRxGetLength-i); j++ ) |
stanley1228 | 0:bf4774b25188 | 155 | gbStatusPacket[j] = gbStatusPacket[j + i]; |
stanley1228 | 0:bf4774b25188 | 156 | |
stanley1228 | 0:bf4774b25188 | 157 | gbRxGetLength -= i; |
stanley1228 | 0:bf4774b25188 | 158 | } |
stanley1228 | 0:bf4774b25188 | 159 | |
stanley1228 | 0:bf4774b25188 | 160 | if( gbRxGetLength < gbRxPacketLength ) |
stanley1228 | 0:bf4774b25188 | 161 | { |
stanley1228 | 0:bf4774b25188 | 162 | gbCommStatus = COMM_RXWAITING; |
stanley1228 | 0:bf4774b25188 | 163 | return; |
stanley1228 | 0:bf4774b25188 | 164 | } |
stanley1228 | 0:bf4774b25188 | 165 | |
stanley1228 | 0:bf4774b25188 | 166 | // Check id pairing |
stanley1228 | 0:bf4774b25188 | 167 | if( gbInstructionPacket[ID] != gbStatusPacket[ID]) |
stanley1228 | 0:bf4774b25188 | 168 | { |
stanley1228 | 0:bf4774b25188 | 169 | gbCommStatus = COMM_RXCORRUPT; |
stanley1228 | 0:bf4774b25188 | 170 | giBusUsing = 0; |
stanley1228 | 0:bf4774b25188 | 171 | return; |
stanley1228 | 0:bf4774b25188 | 172 | } |
stanley1228 | 0:bf4774b25188 | 173 | |
stanley1228 | 0:bf4774b25188 | 174 | gbRxPacketLength = gbStatusPacket[LENGTH] + 4; |
stanley1228 | 0:bf4774b25188 | 175 | if( gbRxGetLength < gbRxPacketLength ) |
stanley1228 | 0:bf4774b25188 | 176 | { |
stanley1228 | 0:bf4774b25188 | 177 | nRead = dxl_hal_rx( (unsigned char*)&gbStatusPacket[gbRxGetLength], gbRxPacketLength - gbRxGetLength ); |
stanley1228 | 0:bf4774b25188 | 178 | gbRxGetLength += nRead; |
stanley1228 | 0:bf4774b25188 | 179 | if( gbRxGetLength < gbRxPacketLength ) |
stanley1228 | 0:bf4774b25188 | 180 | { |
stanley1228 | 0:bf4774b25188 | 181 | gbCommStatus = COMM_RXWAITING; |
stanley1228 | 0:bf4774b25188 | 182 | return; |
stanley1228 | 0:bf4774b25188 | 183 | } |
stanley1228 | 0:bf4774b25188 | 184 | } |
stanley1228 | 0:bf4774b25188 | 185 | |
stanley1228 | 0:bf4774b25188 | 186 | // Check checksum |
stanley1228 | 0:bf4774b25188 | 187 | for( i=0; i<(gbStatusPacket[LENGTH]+1); i++ ) |
stanley1228 | 0:bf4774b25188 | 188 | checksum += gbStatusPacket[i+2]; |
stanley1228 | 0:bf4774b25188 | 189 | checksum = ~checksum; |
stanley1228 | 0:bf4774b25188 | 190 | |
stanley1228 | 0:bf4774b25188 | 191 | if( gbStatusPacket[gbStatusPacket[LENGTH]+3] != checksum ) |
stanley1228 | 0:bf4774b25188 | 192 | { |
stanley1228 | 0:bf4774b25188 | 193 | gbCommStatus = COMM_RXCORRUPT; |
stanley1228 | 0:bf4774b25188 | 194 | giBusUsing = 0; |
stanley1228 | 0:bf4774b25188 | 195 | return; |
stanley1228 | 0:bf4774b25188 | 196 | } |
stanley1228 | 0:bf4774b25188 | 197 | |
stanley1228 | 0:bf4774b25188 | 198 | gbCommStatus = COMM_RXSUCCESS; |
stanley1228 | 0:bf4774b25188 | 199 | giBusUsing = 0; |
stanley1228 | 0:bf4774b25188 | 200 | } |
stanley1228 | 0:bf4774b25188 | 201 | |
stanley1228 | 0:bf4774b25188 | 202 | void dxl_txrx_packet() |
stanley1228 | 0:bf4774b25188 | 203 | { |
stanley1228 | 0:bf4774b25188 | 204 | |
stanley1228 | 0:bf4774b25188 | 205 | //pc.printf("before dxl_tx_packet\n"); |
stanley1228 | 0:bf4774b25188 | 206 | dxl_tx_packet(); |
stanley1228 | 0:bf4774b25188 | 207 | |
stanley1228 | 0:bf4774b25188 | 208 | //pc.printf("after dxl_tx_packet\n"); |
stanley1228 | 0:bf4774b25188 | 209 | if( gbCommStatus != COMM_TXSUCCESS ) |
stanley1228 | 0:bf4774b25188 | 210 | return; |
stanley1228 | 0:bf4774b25188 | 211 | |
stanley1228 | 0:bf4774b25188 | 212 | |
stanley1228 | 0:bf4774b25188 | 213 | |
stanley1228 | 0:bf4774b25188 | 214 | do{ |
stanley1228 | 0:bf4774b25188 | 215 | //pc.printf("before dxl_rx_packet\n"); |
stanley1228 | 0:bf4774b25188 | 216 | dxl_rx_packet(); |
stanley1228 | 0:bf4774b25188 | 217 | //pc.printf("after dxl_rx_packet\n"); |
stanley1228 | 0:bf4774b25188 | 218 | }while( gbCommStatus == COMM_RXWAITING ); |
stanley1228 | 0:bf4774b25188 | 219 | } |
stanley1228 | 0:bf4774b25188 | 220 | |
stanley1228 | 0:bf4774b25188 | 221 | int dxl_get_result() |
stanley1228 | 0:bf4774b25188 | 222 | { |
stanley1228 | 0:bf4774b25188 | 223 | return gbCommStatus; |
stanley1228 | 0:bf4774b25188 | 224 | } |
stanley1228 | 0:bf4774b25188 | 225 | |
stanley1228 | 0:bf4774b25188 | 226 | void dxl_set_txpacket_id( int id ) |
stanley1228 | 0:bf4774b25188 | 227 | { |
stanley1228 | 0:bf4774b25188 | 228 | gbInstructionPacket[ID] = (unsigned char)id; |
stanley1228 | 0:bf4774b25188 | 229 | } |
stanley1228 | 0:bf4774b25188 | 230 | |
stanley1228 | 0:bf4774b25188 | 231 | void dxl_set_txpacket_instruction( int instruction ) |
stanley1228 | 0:bf4774b25188 | 232 | { |
stanley1228 | 0:bf4774b25188 | 233 | gbInstructionPacket[INSTRUCTION] = (unsigned char)instruction; |
stanley1228 | 0:bf4774b25188 | 234 | } |
stanley1228 | 0:bf4774b25188 | 235 | |
stanley1228 | 0:bf4774b25188 | 236 | void dxl_set_txpacket_parameter( int index, int value ) |
stanley1228 | 0:bf4774b25188 | 237 | { |
stanley1228 | 0:bf4774b25188 | 238 | gbInstructionPacket[PARAMETER+index] = (unsigned char)value; |
stanley1228 | 0:bf4774b25188 | 239 | } |
stanley1228 | 0:bf4774b25188 | 240 | |
stanley1228 | 0:bf4774b25188 | 241 | void dxl_set_txpacket_length( int length ) |
stanley1228 | 0:bf4774b25188 | 242 | { |
stanley1228 | 0:bf4774b25188 | 243 | gbInstructionPacket[LENGTH] = (unsigned char)length; |
stanley1228 | 0:bf4774b25188 | 244 | } |
stanley1228 | 0:bf4774b25188 | 245 | |
stanley1228 | 0:bf4774b25188 | 246 | int dxl_get_rxpacket_error( int errbit ) |
stanley1228 | 0:bf4774b25188 | 247 | { |
stanley1228 | 0:bf4774b25188 | 248 | if( gbStatusPacket[ERRBIT] & (unsigned char)errbit ) |
stanley1228 | 0:bf4774b25188 | 249 | return 1; |
stanley1228 | 0:bf4774b25188 | 250 | |
stanley1228 | 0:bf4774b25188 | 251 | return 0; |
stanley1228 | 0:bf4774b25188 | 252 | } |
stanley1228 | 0:bf4774b25188 | 253 | |
stanley1228 | 0:bf4774b25188 | 254 | int dxl_get_rxpacket_length() |
stanley1228 | 0:bf4774b25188 | 255 | { |
stanley1228 | 0:bf4774b25188 | 256 | return (int)gbStatusPacket[LENGTH]; |
stanley1228 | 0:bf4774b25188 | 257 | } |
stanley1228 | 0:bf4774b25188 | 258 | |
stanley1228 | 0:bf4774b25188 | 259 | int dxl_get_rxpacket_parameter( int index ) |
stanley1228 | 0:bf4774b25188 | 260 | { |
stanley1228 | 0:bf4774b25188 | 261 | return (int)gbStatusPacket[PARAMETER+index]; |
stanley1228 | 0:bf4774b25188 | 262 | } |
stanley1228 | 0:bf4774b25188 | 263 | |
stanley1228 | 0:bf4774b25188 | 264 | int dxl_makeword( int lowbyte, int highbyte ) |
stanley1228 | 0:bf4774b25188 | 265 | { |
stanley1228 | 0:bf4774b25188 | 266 | unsigned short word; |
stanley1228 | 0:bf4774b25188 | 267 | |
stanley1228 | 0:bf4774b25188 | 268 | word = highbyte; |
stanley1228 | 0:bf4774b25188 | 269 | word = word << 8; |
stanley1228 | 0:bf4774b25188 | 270 | word = word + lowbyte; |
stanley1228 | 0:bf4774b25188 | 271 | return (int)word; |
stanley1228 | 0:bf4774b25188 | 272 | } |
stanley1228 | 0:bf4774b25188 | 273 | |
stanley1228 | 0:bf4774b25188 | 274 | int dxl_get_lowbyte( int word ) |
stanley1228 | 0:bf4774b25188 | 275 | { |
stanley1228 | 0:bf4774b25188 | 276 | unsigned short temp; |
stanley1228 | 0:bf4774b25188 | 277 | |
stanley1228 | 0:bf4774b25188 | 278 | temp = word & 0xff; |
stanley1228 | 0:bf4774b25188 | 279 | return (int)temp; |
stanley1228 | 0:bf4774b25188 | 280 | } |
stanley1228 | 0:bf4774b25188 | 281 | |
stanley1228 | 0:bf4774b25188 | 282 | int dxl_get_highbyte( int word ) |
stanley1228 | 0:bf4774b25188 | 283 | { |
stanley1228 | 0:bf4774b25188 | 284 | unsigned short temp; |
stanley1228 | 0:bf4774b25188 | 285 | |
stanley1228 | 0:bf4774b25188 | 286 | temp = word & 0xff00; |
stanley1228 | 0:bf4774b25188 | 287 | temp = temp >> 8; |
stanley1228 | 0:bf4774b25188 | 288 | return (int)temp; |
stanley1228 | 0:bf4774b25188 | 289 | } |
stanley1228 | 0:bf4774b25188 | 290 | |
stanley1228 | 0:bf4774b25188 | 291 | void dxl_ping( int id ) |
stanley1228 | 0:bf4774b25188 | 292 | { |
stanley1228 | 0:bf4774b25188 | 293 | while(giBusUsing); |
stanley1228 | 0:bf4774b25188 | 294 | |
stanley1228 | 0:bf4774b25188 | 295 | gbInstructionPacket[ID] = (unsigned char)id; |
stanley1228 | 0:bf4774b25188 | 296 | gbInstructionPacket[INSTRUCTION] = INST_PING; |
stanley1228 | 0:bf4774b25188 | 297 | gbInstructionPacket[LENGTH] = 2; |
stanley1228 | 0:bf4774b25188 | 298 | |
stanley1228 | 0:bf4774b25188 | 299 | dxl_txrx_packet(); |
stanley1228 | 0:bf4774b25188 | 300 | } |
stanley1228 | 0:bf4774b25188 | 301 | |
stanley1228 | 0:bf4774b25188 | 302 | int dxl_read_byte( int id, int address ) |
stanley1228 | 0:bf4774b25188 | 303 | { |
stanley1228 | 0:bf4774b25188 | 304 | while(giBusUsing); |
stanley1228 | 0:bf4774b25188 | 305 | |
stanley1228 | 0:bf4774b25188 | 306 | gbInstructionPacket[ID] = (unsigned char)id; |
stanley1228 | 0:bf4774b25188 | 307 | gbInstructionPacket[INSTRUCTION] = INST_READ; |
stanley1228 | 0:bf4774b25188 | 308 | gbInstructionPacket[PARAMETER] = (unsigned char)address; |
stanley1228 | 0:bf4774b25188 | 309 | gbInstructionPacket[PARAMETER+1] = 1; |
stanley1228 | 0:bf4774b25188 | 310 | gbInstructionPacket[LENGTH] = 4; |
stanley1228 | 0:bf4774b25188 | 311 | |
stanley1228 | 0:bf4774b25188 | 312 | dxl_txrx_packet(); |
stanley1228 | 0:bf4774b25188 | 313 | |
stanley1228 | 0:bf4774b25188 | 314 | return (int)gbStatusPacket[PARAMETER]; |
stanley1228 | 0:bf4774b25188 | 315 | } |
stanley1228 | 0:bf4774b25188 | 316 | |
stanley1228 | 0:bf4774b25188 | 317 | void dxl_write_byte( int id, int address, int value ) |
stanley1228 | 0:bf4774b25188 | 318 | { |
stanley1228 | 0:bf4774b25188 | 319 | //pc.printf("id=%d,address=%x,value=%d\n",id,address,value);//stanley |
stanley1228 | 0:bf4774b25188 | 320 | while(giBusUsing); |
stanley1228 | 0:bf4774b25188 | 321 | |
stanley1228 | 0:bf4774b25188 | 322 | //pc.printf("after giBusUsing\n",id,address,value);//stanley |
stanley1228 | 0:bf4774b25188 | 323 | |
stanley1228 | 0:bf4774b25188 | 324 | gbInstructionPacket[ID] = (unsigned char)id; |
stanley1228 | 0:bf4774b25188 | 325 | gbInstructionPacket[INSTRUCTION] = INST_WRITE; |
stanley1228 | 0:bf4774b25188 | 326 | gbInstructionPacket[PARAMETER] = (unsigned char)address; |
stanley1228 | 0:bf4774b25188 | 327 | gbInstructionPacket[PARAMETER+1] = (unsigned char)value; |
stanley1228 | 0:bf4774b25188 | 328 | gbInstructionPacket[LENGTH] = 4; |
stanley1228 | 0:bf4774b25188 | 329 | |
stanley1228 | 0:bf4774b25188 | 330 | //pc.printf("before dxl_txrx_packet\n",id,address,value);//stanley |
stanley1228 | 0:bf4774b25188 | 331 | |
stanley1228 | 0:bf4774b25188 | 332 | dxl_txrx_packet(); |
stanley1228 | 0:bf4774b25188 | 333 | |
stanley1228 | 0:bf4774b25188 | 334 | //pc.printf("after dxl_txrx_packet\n",id,address,value);//stanley |
stanley1228 | 0:bf4774b25188 | 335 | } |
stanley1228 | 0:bf4774b25188 | 336 | |
stanley1228 | 4:44560391db99 | 337 | int dxl_read_word( short int id, short int address ) |
stanley1228 | 0:bf4774b25188 | 338 | { |
stanley1228 | 0:bf4774b25188 | 339 | while(giBusUsing); |
stanley1228 | 0:bf4774b25188 | 340 | |
stanley1228 | 0:bf4774b25188 | 341 | gbInstructionPacket[ID] = (unsigned char)id; |
stanley1228 | 0:bf4774b25188 | 342 | gbInstructionPacket[INSTRUCTION] = INST_READ; |
stanley1228 | 0:bf4774b25188 | 343 | gbInstructionPacket[PARAMETER] = (unsigned char)address; |
stanley1228 | 0:bf4774b25188 | 344 | gbInstructionPacket[PARAMETER+1] = 2; |
stanley1228 | 0:bf4774b25188 | 345 | gbInstructionPacket[LENGTH] = 4; |
stanley1228 | 0:bf4774b25188 | 346 | |
stanley1228 | 0:bf4774b25188 | 347 | dxl_txrx_packet(); |
stanley1228 | 0:bf4774b25188 | 348 | |
stanley1228 | 0:bf4774b25188 | 349 | return dxl_makeword((int)gbStatusPacket[PARAMETER], (int)gbStatusPacket[PARAMETER+1]); |
stanley1228 | 0:bf4774b25188 | 350 | } |
stanley1228 | 0:bf4774b25188 | 351 | |
stanley1228 | 4:44560391db99 | 352 | void dxl_write_word( short int id, short int address, short int value ) |
stanley1228 | 0:bf4774b25188 | 353 | { |
stanley1228 | 0:bf4774b25188 | 354 | while(giBusUsing); |
stanley1228 | 0:bf4774b25188 | 355 | |
stanley1228 | 0:bf4774b25188 | 356 | gbInstructionPacket[ID] = (unsigned char)id; |
stanley1228 | 0:bf4774b25188 | 357 | gbInstructionPacket[INSTRUCTION] = INST_WRITE; |
stanley1228 | 0:bf4774b25188 | 358 | gbInstructionPacket[PARAMETER] = (unsigned char)address; |
stanley1228 | 0:bf4774b25188 | 359 | gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(value); |
stanley1228 | 0:bf4774b25188 | 360 | gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(value); |
stanley1228 | 0:bf4774b25188 | 361 | gbInstructionPacket[LENGTH] = 5; |
stanley1228 | 0:bf4774b25188 | 362 | |
stanley1228 | 0:bf4774b25188 | 363 | dxl_txrx_packet(); |
stanley1228 | 0:bf4774b25188 | 364 | } |
stanley1228 | 0:bf4774b25188 | 365 | |
stanley1228 | 3:e2e9064c668d | 366 | int syncWrite_u16base(short int start_addr, short int data_length, short int *param, short int param_length) // WORD(16bit) syncwrite() for DXL stanley |
stanley1228 | 0:bf4774b25188 | 367 | { |
stanley1228 | 0:bf4774b25188 | 368 | while(giBusUsing); |
stanley1228 | 0:bf4774b25188 | 369 | |
stanley1228 | 0:bf4774b25188 | 370 | gbInstructionPacket[ID] = (unsigned char)BROADCAST_ID; |
stanley1228 | 0:bf4774b25188 | 371 | gbInstructionPacket[INSTRUCTION] = INST_SYNC_WRITE; |
stanley1228 | 0:bf4774b25188 | 372 | gbInstructionPacket[PARAMETER] = (unsigned char)start_addr; |
stanley1228 | 0:bf4774b25188 | 373 | gbInstructionPacket[PARAMETER+1] = (unsigned char)data_length*2; |
stanley1228 | 0:bf4774b25188 | 374 | |
stanley1228 | 0:bf4774b25188 | 375 | |
stanley1228 | 0:bf4774b25188 | 376 | int slaveNum=param_length/(data_length+1); |
stanley1228 | 0:bf4774b25188 | 377 | int OneRawByte=(1+data_length*2);//ID(1byte) + worddata*len(2byte*len) |
stanley1228 | 0:bf4774b25188 | 378 | |
stanley1228 | 0:bf4774b25188 | 379 | |
stanley1228 | 0:bf4774b25188 | 380 | int i=0; //offset of slave number(number of row) |
stanley1228 | 0:bf4774b25188 | 381 | int j=0; //offset of data in raw |
stanley1228 | 0:bf4774b25188 | 382 | int k=1;//offset of int *param |
stanley1228 | 0:bf4774b25188 | 383 | int index=0; |
stanley1228 | 0:bf4774b25188 | 384 | |
stanley1228 | 0:bf4774b25188 | 385 | for( i=0; i<slaveNum; i++ ) |
stanley1228 | 0:bf4774b25188 | 386 | { |
stanley1228 | 0:bf4774b25188 | 387 | index=PARAMETER+OneRawByte*i+2; |
stanley1228 | 0:bf4774b25188 | 388 | gbInstructionPacket[index] = (unsigned char)param[i*(data_length+1)];//ID |
stanley1228 | 0:bf4774b25188 | 389 | k=1; |
stanley1228 | 0:bf4774b25188 | 390 | |
stanley1228 | 0:bf4774b25188 | 391 | for(j=1;j<OneRawByte;j+=2) |
stanley1228 | 0:bf4774b25188 | 392 | { |
stanley1228 | 0:bf4774b25188 | 393 | gbInstructionPacket[index+j]= (unsigned char)(param[i*(data_length+1)+k]&0xff); //DATA L |
stanley1228 | 0:bf4774b25188 | 394 | gbInstructionPacket[index+j+1]= (unsigned char)(param[i*(data_length+1)+k]>>8); //DATA H |
stanley1228 | 0:bf4774b25188 | 395 | k++; |
stanley1228 | 0:bf4774b25188 | 396 | } |
stanley1228 | 0:bf4774b25188 | 397 | } |
stanley1228 | 3:e2e9064c668d | 398 | |
stanley1228 | 3:e2e9064c668d | 399 | gbInstructionPacket[LENGTH] = OneRawByte*slaveNum+4; |
stanley1228 | 0:bf4774b25188 | 400 | |
stanley1228 | 3:e2e9064c668d | 401 | //for(int i=0;i<50;i++) |
stanley1228 | 3:e2e9064c668d | 402 | // pc.printf("gbInstructionPacket[%d]=%x\n",i,gbInstructionPacket[i]);//stanley test |
stanley1228 | 3:e2e9064c668d | 403 | |
stanley1228 | 3:e2e9064c668d | 404 | |
stanley1228 | 0:bf4774b25188 | 405 | |
stanley1228 | 0:bf4774b25188 | 406 | dxl_txrx_packet(); |
stanley1228 | 0:bf4774b25188 | 407 | return 0; |
stanley1228 | 0:bf4774b25188 | 408 | } |
stanley1228 | 0:bf4774b25188 | 409 | |
stanley1228 | 0:bf4774b25188 | 410 | void setPosition(int ServoID, int Position, int Speed)//stanley |
stanley1228 | 0:bf4774b25188 | 411 | { |
stanley1228 | 0:bf4774b25188 | 412 | while(giBusUsing); |
stanley1228 | 0:bf4774b25188 | 413 | |
stanley1228 | 0:bf4774b25188 | 414 | gbInstructionPacket[ID] = (unsigned char)ServoID; |
stanley1228 | 0:bf4774b25188 | 415 | gbInstructionPacket[INSTRUCTION] = INST_WRITE; |
stanley1228 | 0:bf4774b25188 | 416 | gbInstructionPacket[PARAMETER] = (unsigned char)30; |
stanley1228 | 0:bf4774b25188 | 417 | gbInstructionPacket[PARAMETER+1] = (unsigned char)dxl_get_lowbyte(Position); |
stanley1228 | 0:bf4774b25188 | 418 | gbInstructionPacket[PARAMETER+2] = (unsigned char)dxl_get_highbyte(Position); |
stanley1228 | 0:bf4774b25188 | 419 | gbInstructionPacket[PARAMETER+3] = (unsigned char)dxl_get_lowbyte(Speed); |
stanley1228 | 0:bf4774b25188 | 420 | gbInstructionPacket[PARAMETER+4] = (unsigned char)dxl_get_highbyte(Speed); |
stanley1228 | 0:bf4774b25188 | 421 | |
stanley1228 | 0:bf4774b25188 | 422 | gbInstructionPacket[LENGTH] = 7; |
stanley1228 | 0:bf4774b25188 | 423 | |
stanley1228 | 0:bf4774b25188 | 424 | dxl_txrx_packet(); |
stanley1228 | 0:bf4774b25188 | 425 | |
stanley1228 | 0:bf4774b25188 | 426 | } |