Aditya Mehrotra / Mbed OS DMA_Dynamixel_Communications

Teleop Demo Code

main.cpp@3:ab8084bfcb53, 2021-09-27 (annotated)

Committer:
adimmit
Date:
Mon Sep 27 04:13:15 2021 +0000
Revision:
3:ab8084bfcb53
Parent:
2:7afc501b8283 
1Mbaud;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bwang 0:bcd01ae054eb 1 #include "mbed.h"
bwang 0:bcd01ae054eb 2 #include "crc.h"
bwang 0:bcd01ae054eb 3
adimmit 3:ab8084bfcb53 4 #define WAIT_TIME_MS 1
adimmit 2:7afc501b8283 5 #define LEN 100
adimmit 2:7afc501b8283 6
ahattori 1:66ff8f8e65f7 7 Serial pc(USBTX, USBRX);
bwang 0:bcd01ae054eb 8
adimmit 2:7afc501b8283 9 RawSerial uart(D1, D0);
adimmit 2:7afc501b8283 10 DigitalInOut RTS(D3);
ahattori 1:66ff8f8e65f7 11
adimmit 2:7afc501b8283 12 DigitalOut dbg(LED1);
adimmit 2:7afc501b8283 13 DigitalOut flip(D2);
ahattori 1:66ff8f8e65f7 14
ahattori 1:66ff8f8e65f7 15 volatile uint8_t waitForReceive = 0;
ahattori 1:66ff8f8e65f7 16 volatile uint8_t nextReload = 15;
ahattori 1:66ff8f8e65f7 17
adimmit 2:7afc501b8283 18 uint8_t rx_buffer[LEN];
bwang 0:bcd01ae054eb 19
ahattori 1:66ff8f8e65f7 20 void setGoalPosition(unsigned char ID, unsigned char p1, unsigned char p2, unsigned char p3, unsigned char p4){
ahattori 1:66ff8f8e65f7 21 uint8_t elements = 16;
ahattori 1:66ff8f8e65f7 22 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 23 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 24 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 25 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 26 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 27 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 28 packetBuffer[4] = ID; //ID
ahattori 1:66ff8f8e65f7 29 packetBuffer[5] = 0x09; //length
ahattori 1:66ff8f8e65f7 30 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 31 packetBuffer[7] = 0x03; //write command
ahattori 1:66ff8f8e65f7 32 packetBuffer[8] = 0x74; //Position
ahattori 1:66ff8f8e65f7 33 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 34 packetBuffer[10] = p1;
ahattori 1:66ff8f8e65f7 35 packetBuffer[11] = p2;
ahattori 1:66ff8f8e65f7 36 packetBuffer[12] = p3;
ahattori 1:66ff8f8e65f7 37 packetBuffer[13] = p4;
ahattori 1:66ff8f8e65f7 38
ahattori 1:66ff8f8e65f7 39 unsigned short crc = update_crc(0,packetBuffer,14);
ahattori 1:66ff8f8e65f7 40
ahattori 1:66ff8f8e65f7 41 packetBuffer[14] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 42 packetBuffer[15] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 43
ahattori 1:66ff8f8e65f7 44 // Send instruction packet
ahattori 1:66ff8f8e65f7 45 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 46 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 47 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 48 }
adimmit 3:ab8084bfcb53 49 wait_ms(WAIT_TIME_MS); // fix this!!!
ahattori 1:66ff8f8e65f7 50 RTS = 0; // Disable Tx / Enable Rx
bwang 0:bcd01ae054eb 51 }
ahattori 1:66ff8f8e65f7 52
bwang 0:bcd01ae054eb 53 }
adimmit 2:7afc501b8283 54
adimmit 2:7afc501b8283 55 void getPosition(uint8_t id/*, uint8_t id2*/) {
ahattori 1:66ff8f8e65f7 56 for(int i = 0; i < 100; i++) rx_buffer[i] = 0;
ahattori 1:66ff8f8e65f7 57
adimmit 2:7afc501b8283 58 uint8_t elements = 14;
adimmit 2:7afc501b8283 59 unsigned char tx_buffer[elements];
ahattori 1:66ff8f8e65f7 60 // rx_count = 0;
adimmit 2:7afc501b8283 61
bwang 0:bcd01ae054eb 62 tx_buffer[0] = 0xff;
bwang 0:bcd01ae054eb 63 tx_buffer[1] = 0xff;
bwang 0:bcd01ae054eb 64 tx_buffer[2] = 0xfd;
bwang 0:bcd01ae054eb 65 tx_buffer[3] = 0x00;
bwang 0:bcd01ae054eb 66 tx_buffer[4] = id; //ID
bwang 0:bcd01ae054eb 67 tx_buffer[5] = 0x07; //length
bwang 0:bcd01ae054eb 68 tx_buffer[6] = 0x00; //length2
bwang 0:bcd01ae054eb 69 tx_buffer[7] = 0x02; //read command
bwang 0:bcd01ae054eb 70 tx_buffer[8] = 0x84; //position identifier
adimmit 2:7afc501b8283 71 tx_buffer[9] = 0x00; //SECOND BYTE OF IDENTIFIER (command)
adimmit 2:7afc501b8283 72 tx_buffer[10] = 0x04; //p1 --> means we want 4 bytes back
adimmit 2:7afc501b8283 73 tx_buffer[11] = 0x00; //p2
adimmit 2:7afc501b8283 74
adimmit 2:7afc501b8283 75 unsigned short crc = update_crc(0, tx_buffer, 12);
adimmit 2:7afc501b8283 76
bwang 0:bcd01ae054eb 77 tx_buffer[12] = crc & 0xff;
bwang 0:bcd01ae054eb 78 tx_buffer[13] = (crc >> 8) & 0xff;
bwang 0:bcd01ae054eb 79
ahattori 1:66ff8f8e65f7 80 // printf("0x%x 0x%x\r\n", tx_buffer[12], tx_buffer[13]);
adimmit 2:7afc501b8283 81 //nextReload = 11;
bwang 0:bcd01ae054eb 82 RTS = 1;
ahattori 1:66ff8f8e65f7 83 waitForReceive = 1;
adimmit 3:ab8084bfcb53 84 wait_us(15);
bwang 0:bcd01ae054eb 85 for (int i = 0; i < 14; i++) {
bwang 0:bcd01ae054eb 86 uart.putc(tx_buffer[i]);
bwang 0:bcd01ae054eb 87 }
adimmit 3:ab8084bfcb53 88 wait_us(25);
bwang 0:bcd01ae054eb 89 RTS = 0;
adimmit 2:7afc501b8283 90
bwang 0:bcd01ae054eb 91
adimmit 2:7afc501b8283 92 for (int i = 0; i<=14; i++){
adimmit 2:7afc501b8283 93 rx_buffer[i] = uart.getc();
bwang 0:bcd01ae054eb 94 }
adimmit 2:7afc501b8283 95
adimmit 2:7afc501b8283 96 }
adimmit 2:7afc501b8283 97
adimmit 2:7afc501b8283 98
adimmit 2:7afc501b8283 99 void setGoalCurrent(unsigned char ID, unsigned char p1, unsigned char p2){
adimmit 2:7afc501b8283 100 uint8_t elements = 14;
adimmit 2:7afc501b8283 101 unsigned char packetBuffer[elements];
adimmit 2:7afc501b8283 102 if (uart.writeable() ) {
adimmit 2:7afc501b8283 103 packetBuffer[0] = 0xff;
adimmit 2:7afc501b8283 104 packetBuffer[1] = 0xff;
adimmit 2:7afc501b8283 105 packetBuffer[2] = 0xfd;
adimmit 2:7afc501b8283 106 packetBuffer[3] = 0x00;
adimmit 2:7afc501b8283 107 packetBuffer[4] = ID; //ID
adimmit 2:7afc501b8283 108 packetBuffer[5] = 0x07; //length
adimmit 2:7afc501b8283 109 packetBuffer[6] = 0x00; //length2
adimmit 2:7afc501b8283 110 packetBuffer[7] = 0x03; //write command
adimmit 2:7afc501b8283 111 packetBuffer[8] = 0x66; //Position
adimmit 2:7afc501b8283 112 packetBuffer[9] = 0x00;
adimmit 2:7afc501b8283 113 packetBuffer[10] = p1;
adimmit 2:7afc501b8283 114 packetBuffer[11] = p2;
ahattori 1:66ff8f8e65f7 115
adimmit 2:7afc501b8283 116 unsigned short crc = update_crc(0,packetBuffer,12);
adimmit 2:7afc501b8283 117
adimmit 2:7afc501b8283 118 packetBuffer[12] = crc&0x00ff; //CRC1
adimmit 2:7afc501b8283 119 packetBuffer[13] = (crc>>8)&0x00ff; //CRC2
adimmit 2:7afc501b8283 120
adimmit 2:7afc501b8283 121 // Send instruction packet
adimmit 2:7afc501b8283 122 RTS = 1; // Enable Tx / Disable Rx
adimmit 2:7afc501b8283 123 for (int i = 0; i< elements; i++) {
adimmit 2:7afc501b8283 124 uart.putc(packetBuffer[i]);
adimmit 2:7afc501b8283 125 }
adimmit 3:ab8084bfcb53 126 wait_us(25); // fix this!!!
adimmit 2:7afc501b8283 127 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 128 }
ahattori 1:66ff8f8e65f7 129
ahattori 1:66ff8f8e65f7 130 }
ahattori 1:66ff8f8e65f7 131
ahattori 1:66ff8f8e65f7 132 void torqueEnable(unsigned char ID, unsigned char enable){
ahattori 1:66ff8f8e65f7 133
ahattori 1:66ff8f8e65f7 134 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 135 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 136 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 137 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 138 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 139 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 140 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 141 packetBuffer[4] = ID; //ID
ahattori 1:66ff8f8e65f7 142 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 143 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 144 packetBuffer[7] = 0x03; //write command
adimmit 2:7afc501b8283 145 packetBuffer[8] = 0x40; //enable current mode
ahattori 1:66ff8f8e65f7 146 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 147 packetBuffer[10] = enable;
ahattori 1:66ff8f8e65f7 148
ahattori 1:66ff8f8e65f7 149 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 150
ahattori 1:66ff8f8e65f7 151 packetBuffer[11] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 152 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 153
ahattori 1:66ff8f8e65f7 154 // Send instruction packet
ahattori 1:66ff8f8e65f7 155 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 156 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 157 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 158 }
adimmit 3:ab8084bfcb53 159 wait_ms(5); // fix this!!!
ahattori 1:66ff8f8e65f7 160 RTS = 0; // Disable Tx / Enable Rx
adimmit 3:ab8084bfcb53 161 //wait_ms(10);
bwang 0:bcd01ae054eb 162 }
adimmit 3:ab8084bfcb53 163 wait_ms(10);
ahattori 1:66ff8f8e65f7 164 }
ahattori 1:66ff8f8e65f7 165 void ledOn(unsigned char ID)
ahattori 1:66ff8f8e65f7 166 {
ahattori 1:66ff8f8e65f7 167 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 168 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 169 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 170 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 171 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 172 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 173 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 174 packetBuffer[4] = ID; //ID
ahattori 1:66ff8f8e65f7 175 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 176 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 177 packetBuffer[7] = 0x03; //write command
ahattori 1:66ff8f8e65f7 178 packetBuffer[8] = 0x41; //led
ahattori 1:66ff8f8e65f7 179 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 180 packetBuffer[10] = 0x01; //on
ahattori 1:66ff8f8e65f7 181
ahattori 1:66ff8f8e65f7 182 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 183
ahattori 1:66ff8f8e65f7 184 packetBuffer[11] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 185 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 186
ahattori 1:66ff8f8e65f7 187 // Send instruction packet
ahattori 1:66ff8f8e65f7 188 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 189 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 190 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 191 }
adimmit 2:7afc501b8283 192 wait_ms(WAIT_TIME_MS); // fix this!!!
ahattori 1:66ff8f8e65f7 193 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 194 }
adimmit 3:ab8084bfcb53 195 wait_ms(10);
bwang 0:bcd01ae054eb 196 }
bwang 0:bcd01ae054eb 197
adimmit 2:7afc501b8283 198 void ledOff(unsigned char ID)
adimmit 2:7afc501b8283 199 {
adimmit 2:7afc501b8283 200 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 201 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 202 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 203 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 204 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 205 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 206 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 207 packetBuffer[4] = ID; //ID
adimmit 2:7afc501b8283 208 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 209 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 210 packetBuffer[7] = 0x03; //write command
adimmit 2:7afc501b8283 211 packetBuffer[8] = 0x41; //led
ahattori 1:66ff8f8e65f7 212 packetBuffer[9] = 0x00;
adimmit 2:7afc501b8283 213 packetBuffer[10] = 0x00; //off
ahattori 1:66ff8f8e65f7 214
adimmit 2:7afc501b8283 215 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 216
adimmit 2:7afc501b8283 217 packetBuffer[11] = crc&0x00ff; //CRC1
adimmit 2:7afc501b8283 218 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 219
ahattori 1:66ff8f8e65f7 220 // Send instruction packet
ahattori 1:66ff8f8e65f7 221 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 222 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 223 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 224 }
adimmit 3:ab8084bfcb53 225 wait_ms(WAIT_TIME_MS); // fix this!!!
ahattori 1:66ff8f8e65f7 226 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 227 }
adimmit 3:ab8084bfcb53 228 wait_ms(10);
ahattori 1:66ff8f8e65f7 229 }
adimmit 2:7afc501b8283 230
adimmit 2:7afc501b8283 231
adimmit 2:7afc501b8283 232 void enPosMode(unsigned char ID)
adimmit 2:7afc501b8283 233 {
adimmit 2:7afc501b8283 234 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 235 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 236 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 237 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 238 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 239 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 240 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 241 packetBuffer[4] = ID; //ID
adimmit 2:7afc501b8283 242 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 243 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 244 packetBuffer[7] = 0x03; //write command
adimmit 2:7afc501b8283 245 packetBuffer[8] = 0x0B; //mode
ahattori 1:66ff8f8e65f7 246 packetBuffer[9] = 0x00;
adimmit 2:7afc501b8283 247 packetBuffer[10] = 0x03; //on
ahattori 1:66ff8f8e65f7 248
adimmit 2:7afc501b8283 249 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 250
adimmit 2:7afc501b8283 251 packetBuffer[11] = crc&0x00ff; //CRC1
adimmit 2:7afc501b8283 252 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 253
ahattori 1:66ff8f8e65f7 254 // Send instruction packet
ahattori 1:66ff8f8e65f7 255 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 256 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 257 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 258 }
adimmit 3:ab8084bfcb53 259 wait_ms(5); // fix this!!!
ahattori 1:66ff8f8e65f7 260 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 261 }
adimmit 3:ab8084bfcb53 262 wait_ms(10);
ahattori 1:66ff8f8e65f7 263 }
adimmit 2:7afc501b8283 264
adimmit 2:7afc501b8283 265 void enCurMode(unsigned char ID)
adimmit 2:7afc501b8283 266 {
adimmit 2:7afc501b8283 267 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 268 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 269 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 270 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 271 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 272 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 273 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 274 packetBuffer[4] = ID; //ID
adimmit 2:7afc501b8283 275 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 276 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 277 packetBuffer[7] = 0x03; //write command
adimmit 2:7afc501b8283 278 packetBuffer[8] = 0x0B; //mode
ahattori 1:66ff8f8e65f7 279 packetBuffer[9] = 0x00;
adimmit 2:7afc501b8283 280 packetBuffer[10] = 0x00; //on
ahattori 1:66ff8f8e65f7 281
adimmit 2:7afc501b8283 282 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 283
adimmit 2:7afc501b8283 284 packetBuffer[11] = crc&0x00ff; //CRC1
adimmit 2:7afc501b8283 285 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 286
ahattori 1:66ff8f8e65f7 287 // Send instruction packet
ahattori 1:66ff8f8e65f7 288 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 289 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 290 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 291 }
adimmit 3:ab8084bfcb53 292 wait_ms(5); // fix this!!!
ahattori 1:66ff8f8e65f7 293 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 294 wait_ms(10);
ahattori 1:66ff8f8e65f7 295 }
ahattori 1:66ff8f8e65f7 296 }
adimmit 2:7afc501b8283 297
adimmit 2:7afc501b8283 298
adimmit 2:7afc501b8283 299 void setBaud(unsigned char ID, unsigned char RATE)
adimmit 2:7afc501b8283 300 {
adimmit 2:7afc501b8283 301 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 302 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 303 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 304 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 305 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 306 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 307 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 308 packetBuffer[4] = ID; //ID
adimmit 2:7afc501b8283 309 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 310 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 311 packetBuffer[7] = 0x03; //write command
adimmit 2:7afc501b8283 312 packetBuffer[8] = 0x08; //baud rate
ahattori 1:66ff8f8e65f7 313 packetBuffer[9] = 0x00;
adimmit 2:7afc501b8283 314 packetBuffer[10] = RATE; //0x01 - 57600, 0x02 - 115,200, 0x03 - 1000000
ahattori 1:66ff8f8e65f7 315
adimmit 2:7afc501b8283 316 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 317
adimmit 2:7afc501b8283 318 packetBuffer[11] = crc&0x00ff; //CRC1
adimmit 2:7afc501b8283 319 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 320
ahattori 1:66ff8f8e65f7 321 // Send instruction packet
ahattori 1:66ff8f8e65f7 322 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 323 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 324 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 325 }
adimmit 2:7afc501b8283 326 wait_ms(5); // fix this!!!
ahattori 1:66ff8f8e65f7 327 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 328 wait_ms(10);
ahattori 1:66ff8f8e65f7 329 }
ahattori 1:66ff8f8e65f7 330 }
adimmit 2:7afc501b8283 331
adimmit 2:7afc501b8283 332
adimmit 2:7afc501b8283 333
adimmit 2:7afc501b8283 334
adimmit 2:7afc501b8283 335
bwang 0:bcd01ae054eb 336 int main()
bwang 0:bcd01ae054eb 337 {
ahattori 1:66ff8f8e65f7 338 wait_ms(300);
ahattori 1:66ff8f8e65f7 339 RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN;
adimmit 2:7afc501b8283 340 RTS.mode(OpenDrainNoPull);
adimmit 2:7afc501b8283 341 RTS.output();
bwang 0:bcd01ae054eb 342 pc.baud(115200);
adimmit 3:ab8084bfcb53 343 uart.baud(1000000);
adimmit 2:7afc501b8283 344 RTS = 0;
adimmit 2:7afc501b8283 345 wait(1);
adimmit 2:7afc501b8283 346
adimmit 3:ab8084bfcb53 347 //setBaud(0x01, 0x03);
adimmit 2:7afc501b8283 348 //wait(1);
adimmit 2:7afc501b8283 349
ahattori 1:66ff8f8e65f7 350 pc.printf("Program is running!\r\n");
bwang 0:bcd01ae054eb 351
adimmit 2:7afc501b8283 352 //torqueEnable(0x01,0x01);
adimmit 2:7afc501b8283 353 //wait(1);
ahattori 1:66ff8f8e65f7 354
bwang 0:bcd01ae054eb 355
adimmit 3:ab8084bfcb53 356 /*
adimmit 2:7afc501b8283 357 getPosition(0x01);
adimmit 2:7afc501b8283 358 wait(1);
adimmit 2:7afc501b8283 359
adimmit 2:7afc501b8283 360 //torqueEnable(0x01,0x00);
adimmit 2:7afc501b8283 361 //wait(1);
adimmit 2:7afc501b8283 362
adimmit 2:7afc501b8283 363 for (int i = 0; i<=14; i++) {
adimmit 2:7afc501b8283 364 pc.printf("%hx ", rx_buffer[i]); //USE %HX FOR HEX!!!
adimmit 2:7afc501b8283 365 }
adimmit 2:7afc501b8283 366 pc.printf("\n");
adimmit 2:7afc501b8283 367 pc.printf("done!n");
adimmit 3:ab8084bfcb53 368 */
adimmit 2:7afc501b8283 369
adimmit 3:ab8084bfcb53 370 /*
adimmit 3:ab8084bfcb53 371 ledOn(0x01);
adimmit 3:ab8084bfcb53 372 wait(1);
adimmit 3:ab8084bfcb53 373 ledOff(0x01);
adimmit 3:ab8084bfcb53 374 wait(1);
adimmit 3:ab8084bfcb53 375
adimmit 3:ab8084bfcb53 376 pc.printf("done!n");
adimmit 3:ab8084bfcb53 377 */
adimmit 2:7afc501b8283 378
adimmit 2:7afc501b8283 379
adimmit 2:7afc501b8283 380
adimmit 2:7afc501b8283 381 ledOff(0x01);
adimmit 2:7afc501b8283 382 torqueEnable(0x01,0x00);
adimmit 3:ab8084bfcb53 383 //enPosMode(0x01);
adimmit 3:ab8084bfcb53 384 enCurMode(0x01);
adimmit 2:7afc501b8283 385 wait(1);
adimmit 2:7afc501b8283 386 ledOn(0x01);
adimmit 2:7afc501b8283 387 //setP(0x01,100);
ahattori 1:66ff8f8e65f7 388 torqueEnable(0x01,0x01);
ahattori 1:66ff8f8e65f7 389 wait(1);
adimmit 3:ab8084bfcb53 390 setGoalCurrent(0x01, 0xF0, 0x00);
adimmit 3:ab8084bfcb53 391 //setGoalPosition(0x01,0x00,0x0F,0x00,0x00);
adimmit 3:ab8084bfcb53 392 wait(1);
adimmit 2:7afc501b8283 393 ledOff(0x01);
adimmit 2:7afc501b8283 394 torqueEnable(0x01,0x00);
adimmit 2:7afc501b8283 395 //led = 1;
adimmit 3:ab8084bfcb53 396
adimmit 3:ab8084bfcb53 397
adimmit 2:7afc501b8283 398
adimmit 2:7afc501b8283 399
adimmit 2:7afc501b8283 400
adimmit 2:7afc501b8283 401
bwang 0:bcd01ae054eb 402 }