AECL 601 / 4WOK_8dir_1m_PI_p2pcontrol

123

dynamixel.cpp@0:e3a3ccc3b0f8, 2017-09-12 (annotated)

Committer:
peter16688
Date:
Tue Sep 12 13:25:29 2017 +0000
Revision:
0:e3a3ccc3b0f8
4W_8dir_1m_p2pcontrol

Who changed what in which revision?

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