Aditya Mehrotra / Mbed OS DMA_Dynamixel_Communications

Teleop Demo Code

main.cpp@2:7afc501b8283, 2021-09-27 (annotated)

Committer:
adimmit
Date:
Mon Sep 27 02:53:32 2021 +0000
Revision:
2:7afc501b8283
Parent:
1:66ff8f8e65f7
Child:
3:ab8084bfcb53
WORKING CURRENT AND COMMUNICATIONS;  57600 BAUD

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 2:7afc501b8283 4 #define WAIT_TIME_MS 5
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 2:7afc501b8283 49 wait_ms(10); // 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;
bwang 0:bcd01ae054eb 84 wait_us(100);
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 2:7afc501b8283 88 wait_us(350);
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 2:7afc501b8283 126 wait_ms(10); // 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 2:7afc501b8283 159 wait_ms(WAIT_TIME_MS); // fix this!!!
ahattori 1:66ff8f8e65f7 160 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 161 wait_ms(10);
bwang 0:bcd01ae054eb 162 }
ahattori 1:66ff8f8e65f7 163 }
ahattori 1:66ff8f8e65f7 164 void ledOn(unsigned char ID)
ahattori 1:66ff8f8e65f7 165 {
ahattori 1:66ff8f8e65f7 166 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 167 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 168 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 169 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 170 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 171 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 172 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 173 packetBuffer[4] = ID; //ID
ahattori 1:66ff8f8e65f7 174 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 175 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 176 packetBuffer[7] = 0x03; //write command
ahattori 1:66ff8f8e65f7 177 packetBuffer[8] = 0x41; //led
ahattori 1:66ff8f8e65f7 178 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 179 packetBuffer[10] = 0x01; //on
ahattori 1:66ff8f8e65f7 180
ahattori 1:66ff8f8e65f7 181 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 182
ahattori 1:66ff8f8e65f7 183 packetBuffer[11] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 184 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 185
ahattori 1:66ff8f8e65f7 186 // Send instruction packet
ahattori 1:66ff8f8e65f7 187 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 188 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 189 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 190 }
adimmit 2:7afc501b8283 191 wait_ms(WAIT_TIME_MS); // fix this!!!
ahattori 1:66ff8f8e65f7 192 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 193 wait_ms(10);
ahattori 1:66ff8f8e65f7 194 }
bwang 0:bcd01ae054eb 195 }
bwang 0:bcd01ae054eb 196
adimmit 2:7afc501b8283 197 void ledOff(unsigned char ID)
adimmit 2:7afc501b8283 198 {
adimmit 2:7afc501b8283 199 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 200 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 201 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 202 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 203 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 204 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 205 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 206 packetBuffer[4] = ID; //ID
adimmit 2:7afc501b8283 207 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 208 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 209 packetBuffer[7] = 0x03; //write command
adimmit 2:7afc501b8283 210 packetBuffer[8] = 0x41; //led
ahattori 1:66ff8f8e65f7 211 packetBuffer[9] = 0x00;
adimmit 2:7afc501b8283 212 packetBuffer[10] = 0x00; //off
ahattori 1:66ff8f8e65f7 213
adimmit 2:7afc501b8283 214 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 215
adimmit 2:7afc501b8283 216 packetBuffer[11] = crc&0x00ff; //CRC1
adimmit 2:7afc501b8283 217 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 218
ahattori 1:66ff8f8e65f7 219 // Send instruction packet
ahattori 1:66ff8f8e65f7 220 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 221 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 222 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 223 }
adimmit 2:7afc501b8283 224 wait_ms(5); // fix this!!!
ahattori 1:66ff8f8e65f7 225 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 226 wait_ms(10);
ahattori 1:66ff8f8e65f7 227 }
ahattori 1:66ff8f8e65f7 228 }
adimmit 2:7afc501b8283 229
adimmit 2:7afc501b8283 230
adimmit 2:7afc501b8283 231 void enPosMode(unsigned char ID)
adimmit 2:7afc501b8283 232 {
adimmit 2:7afc501b8283 233 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 234 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 235 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 236 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 237 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 238 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 239 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 240 packetBuffer[4] = ID; //ID
adimmit 2:7afc501b8283 241 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 242 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 243 packetBuffer[7] = 0x03; //write command
adimmit 2:7afc501b8283 244 packetBuffer[8] = 0x0B; //mode
ahattori 1:66ff8f8e65f7 245 packetBuffer[9] = 0x00;
adimmit 2:7afc501b8283 246 packetBuffer[10] = 0x03; //on
ahattori 1:66ff8f8e65f7 247
adimmit 2:7afc501b8283 248 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 249
adimmit 2:7afc501b8283 250 packetBuffer[11] = crc&0x00ff; //CRC1
adimmit 2:7afc501b8283 251 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 252
ahattori 1:66ff8f8e65f7 253 // Send instruction packet
ahattori 1:66ff8f8e65f7 254 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 255 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 256 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 257 }
adimmit 2:7afc501b8283 258 wait_ms(WAIT_TIME_MS); // fix this!!!
ahattori 1:66ff8f8e65f7 259 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 260 wait_ms(10);
ahattori 1:66ff8f8e65f7 261 }
ahattori 1:66ff8f8e65f7 262 }
adimmit 2:7afc501b8283 263
adimmit 2:7afc501b8283 264 void enCurMode(unsigned char ID)
adimmit 2:7afc501b8283 265 {
adimmit 2:7afc501b8283 266 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 267 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 268 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 269 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 270 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 271 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 272 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 273 packetBuffer[4] = ID; //ID
adimmit 2:7afc501b8283 274 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 275 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 276 packetBuffer[7] = 0x03; //write command
adimmit 2:7afc501b8283 277 packetBuffer[8] = 0x0B; //mode
ahattori 1:66ff8f8e65f7 278 packetBuffer[9] = 0x00;
adimmit 2:7afc501b8283 279 packetBuffer[10] = 0x00; //on
ahattori 1:66ff8f8e65f7 280
adimmit 2:7afc501b8283 281 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 282
adimmit 2:7afc501b8283 283 packetBuffer[11] = crc&0x00ff; //CRC1
adimmit 2:7afc501b8283 284 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 285
ahattori 1:66ff8f8e65f7 286 // Send instruction packet
ahattori 1:66ff8f8e65f7 287 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 288 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 289 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 290 }
adimmit 2:7afc501b8283 291 wait_ms(WAIT_TIME_MS); // fix this!!!
ahattori 1:66ff8f8e65f7 292 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 293 wait_ms(10);
ahattori 1:66ff8f8e65f7 294 }
ahattori 1:66ff8f8e65f7 295 }
adimmit 2:7afc501b8283 296
adimmit 2:7afc501b8283 297
adimmit 2:7afc501b8283 298 void setBaud(unsigned char ID, unsigned char RATE)
adimmit 2:7afc501b8283 299 {
adimmit 2:7afc501b8283 300 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 301 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 302 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 303 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 304 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 305 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 306 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 307 packetBuffer[4] = ID; //ID
adimmit 2:7afc501b8283 308 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 309 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 310 packetBuffer[7] = 0x03; //write command
adimmit 2:7afc501b8283 311 packetBuffer[8] = 0x08; //baud rate
ahattori 1:66ff8f8e65f7 312 packetBuffer[9] = 0x00;
adimmit 2:7afc501b8283 313 packetBuffer[10] = RATE; //0x01 - 57600, 0x02 - 115,200, 0x03 - 1000000
ahattori 1:66ff8f8e65f7 314
adimmit 2:7afc501b8283 315 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 316
adimmit 2:7afc501b8283 317 packetBuffer[11] = crc&0x00ff; //CRC1
adimmit 2:7afc501b8283 318 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 319
ahattori 1:66ff8f8e65f7 320 // Send instruction packet
ahattori 1:66ff8f8e65f7 321 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 322 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 323 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 324 }
adimmit 2:7afc501b8283 325 wait_ms(5); // fix this!!!
ahattori 1:66ff8f8e65f7 326 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 327 wait_ms(10);
ahattori 1:66ff8f8e65f7 328 }
ahattori 1:66ff8f8e65f7 329 }
adimmit 2:7afc501b8283 330
adimmit 2:7afc501b8283 331
adimmit 2:7afc501b8283 332
adimmit 2:7afc501b8283 333
adimmit 2:7afc501b8283 334
bwang 0:bcd01ae054eb 335 int main()
bwang 0:bcd01ae054eb 336 {
ahattori 1:66ff8f8e65f7 337 wait_ms(300);
ahattori 1:66ff8f8e65f7 338 RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN;
adimmit 2:7afc501b8283 339 RTS.mode(OpenDrainNoPull);
adimmit 2:7afc501b8283 340 RTS.output();
bwang 0:bcd01ae054eb 341 pc.baud(115200);
adimmit 2:7afc501b8283 342 uart.baud(57600);
adimmit 2:7afc501b8283 343 RTS = 0;
adimmit 2:7afc501b8283 344 wait(1);
adimmit 2:7afc501b8283 345
adimmit 2:7afc501b8283 346 //setBaud(0x01, 0x01);
adimmit 2:7afc501b8283 347 //wait(1);
adimmit 2:7afc501b8283 348
ahattori 1:66ff8f8e65f7 349 pc.printf("Program is running!\r\n");
bwang 0:bcd01ae054eb 350
adimmit 2:7afc501b8283 351 //torqueEnable(0x01,0x01);
adimmit 2:7afc501b8283 352 //wait(1);
ahattori 1:66ff8f8e65f7 353
bwang 0:bcd01ae054eb 354
adimmit 2:7afc501b8283 355 getPosition(0x01);
adimmit 2:7afc501b8283 356 wait(1);
adimmit 2:7afc501b8283 357
adimmit 2:7afc501b8283 358 //torqueEnable(0x01,0x00);
adimmit 2:7afc501b8283 359 //wait(1);
adimmit 2:7afc501b8283 360
adimmit 2:7afc501b8283 361 for (int i = 0; i<=14; i++) {
adimmit 2:7afc501b8283 362 pc.printf("%hx ", rx_buffer[i]); //USE %HX FOR HEX!!!
adimmit 2:7afc501b8283 363 }
adimmit 2:7afc501b8283 364 pc.printf("\n");
adimmit 2:7afc501b8283 365 pc.printf("done!n");
adimmit 2:7afc501b8283 366
adimmit 2:7afc501b8283 367
adimmit 2:7afc501b8283 368
adimmit 2:7afc501b8283 369
adimmit 2:7afc501b8283 370 /*
adimmit 2:7afc501b8283 371 ledOff(0x01);
adimmit 2:7afc501b8283 372 torqueEnable(0x01,0x00);
adimmit 2:7afc501b8283 373 enPosMode(0x01);
adimmit 2:7afc501b8283 374 //enCurMode(0x01);
adimmit 2:7afc501b8283 375 wait(1);
adimmit 2:7afc501b8283 376 ledOn(0x01);
adimmit 2:7afc501b8283 377 //setP(0x01,100);
ahattori 1:66ff8f8e65f7 378 torqueEnable(0x01,0x01);
ahattori 1:66ff8f8e65f7 379 wait(1);
adimmit 2:7afc501b8283 380 //setGoalCurrent(0x01, 0xF0, 0x00);
adimmit 2:7afc501b8283 381 setGoalPosition(0x01,0x00,0x0F,0x00,0x00);
adimmit 2:7afc501b8283 382 wait(5);
adimmit 2:7afc501b8283 383 ledOff(0x01);
adimmit 2:7afc501b8283 384 torqueEnable(0x01,0x00);
adimmit 2:7afc501b8283 385 //led = 1;
adimmit 2:7afc501b8283 386 */
adimmit 2:7afc501b8283 387
adimmit 2:7afc501b8283 388
adimmit 2:7afc501b8283 389
adimmit 2:7afc501b8283 390
bwang 0:bcd01ae054eb 391 }