![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Teleop Demo Code
Diff: main.cpp
- Revision:
- 3:ab8084bfcb53
- Parent:
- 2:7afc501b8283
diff -r 7afc501b8283 -r ab8084bfcb53 main.cpp --- a/main.cpp Mon Sep 27 02:53:32 2021 +0000 +++ b/main.cpp Mon Sep 27 04:13:15 2021 +0000 @@ -1,7 +1,7 @@ #include "mbed.h" #include "crc.h" -#define WAIT_TIME_MS 5 +#define WAIT_TIME_MS 1 #define LEN 100 Serial pc(USBTX, USBRX); @@ -46,7 +46,7 @@ for (int i = 0; i< elements; i++) { uart.putc(packetBuffer[i]); } - wait_ms(10); // fix this!!! + wait_ms(WAIT_TIME_MS); // fix this!!! RTS = 0; // Disable Tx / Enable Rx } @@ -81,11 +81,11 @@ //nextReload = 11; RTS = 1; waitForReceive = 1; - wait_us(100); + wait_us(15); for (int i = 0; i < 14; i++) { uart.putc(tx_buffer[i]); } - wait_us(350); + wait_us(25); RTS = 0; @@ -123,7 +123,7 @@ for (int i = 0; i< elements; i++) { uart.putc(packetBuffer[i]); } - wait_ms(10); // fix this!!! + wait_us(25); // fix this!!! RTS = 0; // Disable Tx / Enable Rx } @@ -156,10 +156,11 @@ for (int i = 0; i< elements; i++) { uart.putc(packetBuffer[i]); } - wait_ms(WAIT_TIME_MS); // fix this!!! + wait_ms(5); // fix this!!! RTS = 0; // Disable Tx / Enable Rx - wait_ms(10); + //wait_ms(10); } + wait_ms(10); } void ledOn(unsigned char ID) { @@ -190,8 +191,8 @@ } wait_ms(WAIT_TIME_MS); // fix this!!! RTS = 0; // Disable Tx / Enable Rx - wait_ms(10); } + wait_ms(10); } void ledOff(unsigned char ID) @@ -221,10 +222,10 @@ for (int i = 0; i< elements; i++) { uart.putc(packetBuffer[i]); } - wait_ms(5); // fix this!!! + wait_ms(WAIT_TIME_MS); // fix this!!! RTS = 0; // Disable Tx / Enable Rx - wait_ms(10); } + wait_ms(10); } @@ -255,10 +256,10 @@ for (int i = 0; i< elements; i++) { uart.putc(packetBuffer[i]); } - wait_ms(WAIT_TIME_MS); // fix this!!! + wait_ms(5); // fix this!!! RTS = 0; // Disable Tx / Enable Rx - wait_ms(10); } + wait_ms(10); } void enCurMode(unsigned char ID) @@ -288,7 +289,7 @@ for (int i = 0; i< elements; i++) { uart.putc(packetBuffer[i]); } - wait_ms(WAIT_TIME_MS); // fix this!!! + wait_ms(5); // fix this!!! RTS = 0; // Disable Tx / Enable Rx wait_ms(10); } @@ -339,11 +340,11 @@ RTS.mode(OpenDrainNoPull); RTS.output(); pc.baud(115200); - uart.baud(57600); + uart.baud(1000000); RTS = 0; wait(1); - //setBaud(0x01, 0x01); + //setBaud(0x01, 0x03); //wait(1); pc.printf("Program is running!\r\n"); @@ -352,6 +353,7 @@ //wait(1); + /* getPosition(0x01); wait(1); @@ -363,27 +365,36 @@ } pc.printf("\n"); pc.printf("done!n"); + */ + /* + ledOn(0x01); + wait(1); + ledOff(0x01); + wait(1); + + pc.printf("done!n"); + */ - /* ledOff(0x01); torqueEnable(0x01,0x00); - enPosMode(0x01); - //enCurMode(0x01); + //enPosMode(0x01); + enCurMode(0x01); wait(1); ledOn(0x01); //setP(0x01,100); torqueEnable(0x01,0x01); wait(1); - //setGoalCurrent(0x01, 0xF0, 0x00); - setGoalPosition(0x01,0x00,0x0F,0x00,0x00); - wait(5); + setGoalCurrent(0x01, 0xF0, 0x00); + //setGoalPosition(0x01,0x00,0x0F,0x00,0x00); + wait(1); ledOff(0x01); torqueEnable(0x01,0x00); //led = 1; - */ + +