Aditya Mehrotra
/
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?
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 | 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 | } |