Aditya Mehrotra / Mbed OS DMA_Dynamixel_Communications

Teleop Demo Code

main.cpp@1:66ff8f8e65f7, 2021-04-17 (annotated)

Committer:
ahattori
Date:
Sat Apr 17 17:49:36 2021 +0000
Revision:
1:66ff8f8e65f7
Parent:
0:bcd01ae054eb
Child:
2:7afc501b8283
Initial?

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
ahattori 1:66ff8f8e65f7 4 Serial pc(USBTX, USBRX);
bwang 0:bcd01ae054eb 5
ahattori 1:66ff8f8e65f7 6 Serial uart(PA_0, PA_1);
bwang 0:bcd01ae054eb 7 DigitalOut RTS(D12);
bwang 0:bcd01ae054eb 8
ahattori 1:66ff8f8e65f7 9 //DigitalOut led(LED1);
ahattori 1:66ff8f8e65f7 10
bwang 0:bcd01ae054eb 11 DigitalOut dbg(D2);
bwang 0:bcd01ae054eb 12 DigitalOut flip(D3);
bwang 0:bcd01ae054eb 13
ahattori 1:66ff8f8e65f7 14 AnalogIn GainPot(A2);
ahattori 1:66ff8f8e65f7 15 DigitalIn Calibrate(D7);
ahattori 1:66ff8f8e65f7 16 DigitalIn GainUpdate(D6);
ahattori 1:66ff8f8e65f7 17
ahattori 1:66ff8f8e65f7 18 DigitalOut led(D8);
ahattori 1:66ff8f8e65f7 19
ahattori 1:66ff8f8e65f7 20 volatile uint8_t waitForReceive = 0;
ahattori 1:66ff8f8e65f7 21 volatile uint8_t nextReload = 15;
ahattori 1:66ff8f8e65f7 22
bwang 0:bcd01ae054eb 23 uint8_t rx_buffer[100];
bwang 0:bcd01ae054eb 24
ahattori 1:66ff8f8e65f7 25 extern "C" void DMA1_Stream2_IRQHandler() {
ahattori 1:66ff8f8e65f7 26 DMA1->LIFCR |= (1 << 5); // clear the transfer interrupt flag
ahattori 1:66ff8f8e65f7 27 // there is some sort of DMA error generated on each transfer
ahattori 1:66ff8f8e65f7 28 // it needs to be cleared before we can re-enable
ahattori 1:66ff8f8e65f7 29 DMA1->LIFCR = 0xffffffff; // clear other flags which are confusing and bad
ahattori 1:66ff8f8e65f7 30
ahattori 1:66ff8f8e65f7 31 DMA1_Stream2->CR &= ~1; // disable interrupt
ahattori 1:66ff8f8e65f7 32 // start the next transfer
ahattori 1:66ff8f8e65f7 33 DMA1_Stream2->NDTR = nextReload;
ahattori 1:66ff8f8e65f7 34 DMA1_Stream2->M0AR = (uint32_t)rx_buffer;
ahattori 1:66ff8f8e65f7 35 DMA1_Stream2->CR |= 1; // enable
ahattori 1:66ff8f8e65f7 36 flip = 0;
ahattori 1:66ff8f8e65f7 37 waitForReceive = 0;
ahattori 1:66ff8f8e65f7 38 }
ahattori 1:66ff8f8e65f7 39 void setGoalPosition(unsigned char ID, unsigned char p1, unsigned char p2, unsigned char p3, unsigned char p4){
ahattori 1:66ff8f8e65f7 40 uint8_t elements = 16;
ahattori 1:66ff8f8e65f7 41 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 42 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 43 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 44 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 45 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 46 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 47 packetBuffer[4] = ID; //ID
ahattori 1:66ff8f8e65f7 48 packetBuffer[5] = 0x09; //length
ahattori 1:66ff8f8e65f7 49 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 50 packetBuffer[7] = 0x03; //write command
ahattori 1:66ff8f8e65f7 51 packetBuffer[8] = 0x74; //Position
ahattori 1:66ff8f8e65f7 52 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 53 packetBuffer[10] = p1;
ahattori 1:66ff8f8e65f7 54 packetBuffer[11] = p2;
ahattori 1:66ff8f8e65f7 55 packetBuffer[12] = p3;
ahattori 1:66ff8f8e65f7 56 packetBuffer[13] = p4;
ahattori 1:66ff8f8e65f7 57
ahattori 1:66ff8f8e65f7 58 unsigned short crc = update_crc(0,packetBuffer,14);
ahattori 1:66ff8f8e65f7 59
ahattori 1:66ff8f8e65f7 60 packetBuffer[14] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 61 packetBuffer[15] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 62
ahattori 1:66ff8f8e65f7 63 // Send instruction packet
ahattori 1:66ff8f8e65f7 64 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 65 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 66 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 67 }
ahattori 1:66ff8f8e65f7 68 wait_us(200); // fix this!!!
ahattori 1:66ff8f8e65f7 69 RTS = 0; // Disable Tx / Enable Rx
bwang 0:bcd01ae054eb 70 }
ahattori 1:66ff8f8e65f7 71
bwang 0:bcd01ae054eb 72 }
ahattori 1:66ff8f8e65f7 73 void write_get_position(uint8_t id, uint8_t id2) {
ahattori 1:66ff8f8e65f7 74 for(int i = 0; i < 100; i++) rx_buffer[i] = 0;
ahattori 1:66ff8f8e65f7 75
bwang 0:bcd01ae054eb 76 uint8_t tx_buffer[14];
ahattori 1:66ff8f8e65f7 77 // rx_count = 0;
bwang 0:bcd01ae054eb 78
bwang 0:bcd01ae054eb 79 tx_buffer[0] = 0xff;
bwang 0:bcd01ae054eb 80 tx_buffer[1] = 0xff;
bwang 0:bcd01ae054eb 81 tx_buffer[2] = 0xfd;
bwang 0:bcd01ae054eb 82 tx_buffer[3] = 0x00;
bwang 0:bcd01ae054eb 83 tx_buffer[4] = id; //ID
bwang 0:bcd01ae054eb 84 tx_buffer[5] = 0x07; //length
bwang 0:bcd01ae054eb 85 tx_buffer[6] = 0x00; //length2
bwang 0:bcd01ae054eb 86 tx_buffer[7] = 0x02; //read command
bwang 0:bcd01ae054eb 87 tx_buffer[8] = 0x84; //position identifier
bwang 0:bcd01ae054eb 88 tx_buffer[9] = 0x00;
bwang 0:bcd01ae054eb 89 tx_buffer[10] = 0x04;
bwang 0:bcd01ae054eb 90 tx_buffer[11] = 0x00;
bwang 0:bcd01ae054eb 91 uint16_t crc = update_crc(0, tx_buffer, 12);
bwang 0:bcd01ae054eb 92 tx_buffer[12] = crc & 0xff;
bwang 0:bcd01ae054eb 93 tx_buffer[13] = (crc >> 8) & 0xff;
bwang 0:bcd01ae054eb 94
ahattori 1:66ff8f8e65f7 95 // printf("0x%x 0x%x\r\n", tx_buffer[12], tx_buffer[13]);
ahattori 1:66ff8f8e65f7 96 nextReload = 11;
bwang 0:bcd01ae054eb 97 RTS = 1;
ahattori 1:66ff8f8e65f7 98 waitForReceive = 1;
bwang 0:bcd01ae054eb 99 wait_us(100);
bwang 0:bcd01ae054eb 100 for (int i = 0; i < 14; i++) {
bwang 0:bcd01ae054eb 101 uart.putc(tx_buffer[i]);
bwang 0:bcd01ae054eb 102 }
bwang 0:bcd01ae054eb 103 wait_us(200);
bwang 0:bcd01ae054eb 104 RTS = 0;
ahattori 1:66ff8f8e65f7 105
bwang 0:bcd01ae054eb 106
bwang 0:bcd01ae054eb 107 flip = 1;
ahattori 1:66ff8f8e65f7 108 while (waitForReceive == 1) {
bwang 0:bcd01ae054eb 109 }
ahattori 1:66ff8f8e65f7 110
ahattori 1:66ff8f8e65f7 111 // for (int i = 0; i < 15; i++) {
ahattori 1:66ff8f8e65f7 112 // pc.printf("0x%x ", rx_buffer[i]);
ahattori 1:66ff8f8e65f7 113 // }
ahattori 1:66ff8f8e65f7 114 // pc.printf("\r\n");
ahattori 1:66ff8f8e65f7 115
ahattori 1:66ff8f8e65f7 116 nextReload = 15;
ahattori 1:66ff8f8e65f7 117 //pc.printf("NDTR before send2: %d\r\n", DMA1_Stream2->NDTR);
ahattori 1:66ff8f8e65f7 118 waitForReceive = 1;
ahattori 1:66ff8f8e65f7 119 setGoalPosition(id2, rx_buffer[9], rx_buffer[10], rx_buffer[11], rx_buffer[12]);
ahattori 1:66ff8f8e65f7 120
ahattori 1:66ff8f8e65f7 121 while (waitForReceive == 1) {
ahattori 1:66ff8f8e65f7 122 //wait_ms(100);
ahattori 1:66ff8f8e65f7 123 //printf("NDTR: %d wfr: %d\r\n", DMA1_Stream2->NDTR, waitForReceive);
ahattori 1:66ff8f8e65f7 124 }
bwang 0:bcd01ae054eb 125
ahattori 1:66ff8f8e65f7 126
ahattori 1:66ff8f8e65f7 127 }
ahattori 1:66ff8f8e65f7 128
ahattori 1:66ff8f8e65f7 129 void torqueEnable(unsigned char ID, unsigned char enable){
ahattori 1:66ff8f8e65f7 130
ahattori 1:66ff8f8e65f7 131 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 132 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 133 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 134 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 135 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 136 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 137 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 138 packetBuffer[4] = ID; //ID
ahattori 1:66ff8f8e65f7 139 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 140 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 141 packetBuffer[7] = 0x03; //write command
ahattori 1:66ff8f8e65f7 142 packetBuffer[8] = 0x40; //torque enable
ahattori 1:66ff8f8e65f7 143 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 144 packetBuffer[10] = enable;
ahattori 1:66ff8f8e65f7 145
ahattori 1:66ff8f8e65f7 146 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 147
ahattori 1:66ff8f8e65f7 148 packetBuffer[11] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 149 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 150
ahattori 1:66ff8f8e65f7 151 // Send instruction packet
ahattori 1:66ff8f8e65f7 152 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 153 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 154 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 155 }
ahattori 1:66ff8f8e65f7 156 wait_us(200); // fix this!!!
ahattori 1:66ff8f8e65f7 157 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 158 wait_ms(10);
bwang 0:bcd01ae054eb 159 }
ahattori 1:66ff8f8e65f7 160 }
ahattori 1:66ff8f8e65f7 161 void ledOn(unsigned char ID)
ahattori 1:66ff8f8e65f7 162 {
ahattori 1:66ff8f8e65f7 163 uint8_t elements = 13;
ahattori 1:66ff8f8e65f7 164 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 165 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 166 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 167 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 168 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 169 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 170 packetBuffer[4] = ID; //ID
ahattori 1:66ff8f8e65f7 171 packetBuffer[5] = 0x06; //length
ahattori 1:66ff8f8e65f7 172 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 173 packetBuffer[7] = 0x03; //write command
ahattori 1:66ff8f8e65f7 174 packetBuffer[8] = 0x41; //led
ahattori 1:66ff8f8e65f7 175 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 176 packetBuffer[10] = 0x01; //on
ahattori 1:66ff8f8e65f7 177
ahattori 1:66ff8f8e65f7 178 unsigned short crc = update_crc(0,packetBuffer,11);
ahattori 1:66ff8f8e65f7 179
ahattori 1:66ff8f8e65f7 180 packetBuffer[11] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 181 packetBuffer[12] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 182
ahattori 1:66ff8f8e65f7 183 // Send instruction packet
ahattori 1:66ff8f8e65f7 184 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 185 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 186 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 187 }
ahattori 1:66ff8f8e65f7 188 wait_ms(1); // fix this!!!
ahattori 1:66ff8f8e65f7 189 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 190 wait_ms(10);
ahattori 1:66ff8f8e65f7 191 }
bwang 0:bcd01ae054eb 192 }
bwang 0:bcd01ae054eb 193
ahattori 1:66ff8f8e65f7 194 void setD(unsigned char ID, uint16_t kd) {
ahattori 1:66ff8f8e65f7 195
ahattori 1:66ff8f8e65f7 196 uint8_t kd2 = kd >> 8;
ahattori 1:66ff8f8e65f7 197 uint8_t kd1 = kd & 0xff;
ahattori 1:66ff8f8e65f7 198 uint8_t elements = 14;
ahattori 1:66ff8f8e65f7 199 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 200 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 201 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 202 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 203 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 204 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 205 packetBuffer[4] = ID; //ID
ahattori 1:66ff8f8e65f7 206 packetBuffer[5] = 0x07; //length
ahattori 1:66ff8f8e65f7 207 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 208 packetBuffer[7] = 0x03; //write command
ahattori 1:66ff8f8e65f7 209 packetBuffer[8] = 0x50; //Kp
ahattori 1:66ff8f8e65f7 210 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 211 packetBuffer[10] = kd1;
ahattori 1:66ff8f8e65f7 212 packetBuffer[11] = kd2;
ahattori 1:66ff8f8e65f7 213
ahattori 1:66ff8f8e65f7 214 unsigned short crc = update_crc(0,packetBuffer,12);
ahattori 1:66ff8f8e65f7 215
ahattori 1:66ff8f8e65f7 216 packetBuffer[12] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 217 packetBuffer[13] = (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 }
ahattori 1:66ff8f8e65f7 224 wait_ms(1); // 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 }
ahattori 1:66ff8f8e65f7 229 void setP(unsigned char ID, uint16_t kp) {
ahattori 1:66ff8f8e65f7 230 uint8_t kp2 = kp >> 8;
ahattori 1:66ff8f8e65f7 231 uint8_t kp1 = kp & 0xff;
ahattori 1:66ff8f8e65f7 232 uint8_t elements = 14;
ahattori 1:66ff8f8e65f7 233 // printf("0x%x 0x%x\r\n", kp1, kp2);
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
ahattori 1:66ff8f8e65f7 241 packetBuffer[5] = 0x07; //length
ahattori 1:66ff8f8e65f7 242 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 243 packetBuffer[7] = 0x03; //write command
ahattori 1:66ff8f8e65f7 244 packetBuffer[8] = 0x54; //Kp
ahattori 1:66ff8f8e65f7 245 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 246 packetBuffer[10] = kp1;
ahattori 1:66ff8f8e65f7 247 packetBuffer[11] = kp2;
ahattori 1:66ff8f8e65f7 248
ahattori 1:66ff8f8e65f7 249 unsigned short crc = update_crc(0,packetBuffer,12);
ahattori 1:66ff8f8e65f7 250
ahattori 1:66ff8f8e65f7 251 packetBuffer[12] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 252 packetBuffer[13] = (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 }
ahattori 1:66ff8f8e65f7 259 wait_ms(1); // fix this!!!
ahattori 1:66ff8f8e65f7 260 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 261 wait_ms(10);
ahattori 1:66ff8f8e65f7 262 }
ahattori 1:66ff8f8e65f7 263 }
ahattori 1:66ff8f8e65f7 264 void setFFA(unsigned char ID, uint16_t k) {
ahattori 1:66ff8f8e65f7 265 uint8_t kp2 = k >> 8;
ahattori 1:66ff8f8e65f7 266 uint8_t kp1 = k & 0xff;
ahattori 1:66ff8f8e65f7 267 uint8_t elements = 14;
ahattori 1:66ff8f8e65f7 268 // printf("0x%x 0x%x\r\n", kp1, kp2);
ahattori 1:66ff8f8e65f7 269 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 270 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 271 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 272 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 273 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 274 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 275 packetBuffer[4] = ID; //ID
ahattori 1:66ff8f8e65f7 276 packetBuffer[5] = 0x07; //length
ahattori 1:66ff8f8e65f7 277 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 278 packetBuffer[7] = 0x03; //write command
ahattori 1:66ff8f8e65f7 279 packetBuffer[8] = 0x58; //Feed forward acceleration
ahattori 1:66ff8f8e65f7 280 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 281 packetBuffer[10] = kp1;
ahattori 1:66ff8f8e65f7 282 packetBuffer[11] = kp2;
ahattori 1:66ff8f8e65f7 283
ahattori 1:66ff8f8e65f7 284 unsigned short crc = update_crc(0,packetBuffer,12);
ahattori 1:66ff8f8e65f7 285
ahattori 1:66ff8f8e65f7 286 packetBuffer[12] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 287 packetBuffer[13] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 288
ahattori 1:66ff8f8e65f7 289 // Send instruction packet
ahattori 1:66ff8f8e65f7 290 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 291 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 292 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 293 }
ahattori 1:66ff8f8e65f7 294 wait_ms(1); // fix this!!!
ahattori 1:66ff8f8e65f7 295 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 296 wait_ms(10);
ahattori 1:66ff8f8e65f7 297 }
ahattori 1:66ff8f8e65f7 298 }
ahattori 1:66ff8f8e65f7 299 void setFFV(unsigned char ID, uint16_t k) {
ahattori 1:66ff8f8e65f7 300 uint8_t kp2 = k >> 8;
ahattori 1:66ff8f8e65f7 301 uint8_t kp1 = k & 0xff;
ahattori 1:66ff8f8e65f7 302 uint8_t elements = 14;
ahattori 1:66ff8f8e65f7 303 // printf("0x%x 0x%x\r\n", kp1, kp2);
ahattori 1:66ff8f8e65f7 304 unsigned char packetBuffer[elements];
ahattori 1:66ff8f8e65f7 305 if (uart.writeable() ) {
ahattori 1:66ff8f8e65f7 306 packetBuffer[0] = 0xff;
ahattori 1:66ff8f8e65f7 307 packetBuffer[1] = 0xff;
ahattori 1:66ff8f8e65f7 308 packetBuffer[2] = 0xfd;
ahattori 1:66ff8f8e65f7 309 packetBuffer[3] = 0x00;
ahattori 1:66ff8f8e65f7 310 packetBuffer[4] = ID; //ID
ahattori 1:66ff8f8e65f7 311 packetBuffer[5] = 0x07; //length
ahattori 1:66ff8f8e65f7 312 packetBuffer[6] = 0x00; //length2
ahattori 1:66ff8f8e65f7 313 packetBuffer[7] = 0x03; //write command
ahattori 1:66ff8f8e65f7 314 packetBuffer[8] = 0x5A; //Feed forward velocity
ahattori 1:66ff8f8e65f7 315 packetBuffer[9] = 0x00;
ahattori 1:66ff8f8e65f7 316 packetBuffer[10] = kp1;
ahattori 1:66ff8f8e65f7 317 packetBuffer[11] = kp2;
ahattori 1:66ff8f8e65f7 318
ahattori 1:66ff8f8e65f7 319 unsigned short crc = update_crc(0,packetBuffer,12);
ahattori 1:66ff8f8e65f7 320
ahattori 1:66ff8f8e65f7 321 packetBuffer[12] = crc&0x00ff; //CRC1
ahattori 1:66ff8f8e65f7 322 packetBuffer[13] = (crc>>8)&0x00ff; //CRC2
ahattori 1:66ff8f8e65f7 323
ahattori 1:66ff8f8e65f7 324 // Send instruction packet
ahattori 1:66ff8f8e65f7 325 RTS = 1; // Enable Tx / Disable Rx
ahattori 1:66ff8f8e65f7 326 for (int i = 0; i< elements; i++) {
ahattori 1:66ff8f8e65f7 327 uart.putc(packetBuffer[i]);
ahattori 1:66ff8f8e65f7 328 }
ahattori 1:66ff8f8e65f7 329 wait_ms(1); // fix this!!!
ahattori 1:66ff8f8e65f7 330 RTS = 0; // Disable Tx / Enable Rx
ahattori 1:66ff8f8e65f7 331 wait_ms(10);
ahattori 1:66ff8f8e65f7 332 }
ahattori 1:66ff8f8e65f7 333 }
bwang 0:bcd01ae054eb 334 int main()
bwang 0:bcd01ae054eb 335 {
ahattori 1:66ff8f8e65f7 336 wait_ms(300);
ahattori 1:66ff8f8e65f7 337 RCC->AHB1ENR |= RCC_AHB1ENR_DMA1EN;
bwang 0:bcd01ae054eb 338 pc.baud(115200);
ahattori 1:66ff8f8e65f7 339 uart.baud(1000000);
ahattori 1:66ff8f8e65f7 340 // uart.attach(rxCallback);
ahattori 1:66ff8f8e65f7 341 UART4->CR3 |= USART_CR3_DMAR;
ahattori 1:66ff8f8e65f7 342 DMA1_Stream2->PAR = (uint32_t)&UART4->DR;
ahattori 1:66ff8f8e65f7 343 DMA1_Stream2->M0AR = (uint32_t)rx_buffer;
ahattori 1:66ff8f8e65f7 344 DMA1_Stream2->CR |= (4<<25);
ahattori 1:66ff8f8e65f7 345 DMA1_Stream2->NDTR = 16;
ahattori 1:66ff8f8e65f7 346 // DMA1_Stream2->CR |= DMA_SxCR_PFCTRL;
ahattori 1:66ff8f8e65f7 347 DMA1_Stream2->CR |= DMA_SxCR_MINC;
ahattori 1:66ff8f8e65f7 348 NVIC_EnableIRQ(DMA1_Stream2_IRQn);
ahattori 1:66ff8f8e65f7 349 DMA1_Stream2->CR |= DMA_SxCR_TCIE;
ahattori 1:66ff8f8e65f7 350 pc.printf("Enabling DMA\r\n");
ahattori 1:66ff8f8e65f7 351 //DMA1_Stream2->CR |= DMA_SxCR_EN;
ahattori 1:66ff8f8e65f7 352 pc.printf("DMA Enabled\r\n");
ahattori 1:66ff8f8e65f7 353 pc.printf("Program is running!\r\n");
bwang 0:bcd01ae054eb 354
ahattori 1:66ff8f8e65f7 355
ahattori 1:66ff8f8e65f7 356
ahattori 1:66ff8f8e65f7 357 ledOn(0x01);
ahattori 1:66ff8f8e65f7 358 setD(0x01, 120);
ahattori 1:66ff8f8e65f7 359 setP(0x01,200);
ahattori 1:66ff8f8e65f7 360 setD(0x02, 120);
ahattori 1:66ff8f8e65f7 361 setP(0x02, 200);
bwang 0:bcd01ae054eb 362
ahattori 1:66ff8f8e65f7 363 setD(0x03, 0);
ahattori 1:66ff8f8e65f7 364 setD(0x04, 0);
ahattori 1:66ff8f8e65f7 365 setD(0x05, 0);
ahattori 1:66ff8f8e65f7 366 setD(0x06, 0);
ahattori 1:66ff8f8e65f7 367 setP(0x03, 200);
ahattori 1:66ff8f8e65f7 368 setP(0x04, 200);
ahattori 1:66ff8f8e65f7 369 setP(0x05, 200);
ahattori 1:66ff8f8e65f7 370 setP(0x06, 200);
ahattori 1:66ff8f8e65f7 371 // setFFA(0x03, 800);
ahattori 1:66ff8f8e65f7 372 // setFFA(0x05, 800);
ahattori 1:66ff8f8e65f7 373 //setGoalPosition(0x04, 0xD0, 0x07, 0x00, 0x00);
ahattori 1:66ff8f8e65f7 374 // wait_ms(10);
ahattori 1:66ff8f8e65f7 375 torqueEnable(0x02,0x01);
ahattori 1:66ff8f8e65f7 376 torqueEnable(0x01,0x01);
ahattori 1:66ff8f8e65f7 377 torqueEnable(0x03,0x01);
ahattori 1:66ff8f8e65f7 378 torqueEnable(0x04,0x01);
ahattori 1:66ff8f8e65f7 379 torqueEnable(0x05,0x01);
ahattori 1:66ff8f8e65f7 380 torqueEnable(0x06,0x01);
ahattori 1:66ff8f8e65f7 381 wait(1);
ahattori 1:66ff8f8e65f7 382 led = 1;
ahattori 1:66ff8f8e65f7 383 while(uart.readable()) {uart.getc();}
ahattori 1:66ff8f8e65f7 384 DMA1_Stream2->CR |= DMA_SxCR_EN;
bwang 0:bcd01ae054eb 385 for (;;) {
ahattori 1:66ff8f8e65f7 386
bwang 0:bcd01ae054eb 387 dbg = 1;
ahattori 1:66ff8f8e65f7 388 // ledOn(0x03);
ahattori 1:66ff8f8e65f7 389 write_get_position(0x01,0x02);
ahattori 1:66ff8f8e65f7 390 write_get_position(0x02,0x01);
ahattori 1:66ff8f8e65f7 391 write_get_position(0x03,0x04);
ahattori 1:66ff8f8e65f7 392 write_get_position(0x04,0x03);
ahattori 1:66ff8f8e65f7 393 write_get_position(0x05,0x06);
ahattori 1:66ff8f8e65f7 394 write_get_position(0x06,0x05);
ahattori 1:66ff8f8e65f7 395 //wait_ms(10);
bwang 0:bcd01ae054eb 396 dbg = 0;
bwang 0:bcd01ae054eb 397 }
bwang 0:bcd01ae054eb 398 }