Aditya Mehrotra
/
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?
User | Revision | Line number | New 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 | } |