reserve
Dependencies: mbed MotorDrivers
main.cpp
- Committer:
- sink
- Date:
- 2020-10-07
- Revision:
- 0:2c7c889dbf1a
File content as of revision 0:2c7c889dbf1a:
#include "mbed.h" #include "RoboClaw.h" #include "string" #define SQRT_3 1.73205080757 #define MAX_QPPS1 40960 #define MAX_QPPS2 40960 #define INT_TIME 0.02 #define ADRS1 129 #define ADRS2 130 Ticker timer; RawSerial Slave (p9,p10,115200); RawSerial controller (p13,p14,115200); RoboClaw MD(115200,p28,p27); string recv_str = ""; char c_x = 127, c_y = 127, c_rot = 127, buttons = 0b00000000; void controller_recv(){ char recv_c = controller.getc(); if(recv_str.size() == 4 && recv_c == '\n'){ c_x = recv_str[0]; c_y = recv_str[1]; c_rot = recv_str[2]; buttons = recv_str[3]; recv_str = ""; } else{ recv_str += recv_c; if(recv_str.size() >= 5) recv_str = ""; } } void Roboclaw_send(double V1, double V2, double V3){ MD.SpeedM1(ADRS1,int(V1*MAX_QPPS1)); MD.SpeedM2(ADRS1,int(V2*MAX_QPPS1)); MD.SpeedM1(ADRS2,int(V3*MAX_QPPS2)); } void timer_interrupt(){ int i_x = (int)c_x - 127; int i_y = (int)c_y - 127; int i_rot = (int)c_rot - 127; double V1 = (double)i_x + (double)i_rot; double V2 = -0.5*(double)i_x + SQRT_3*0.5*(double)i_y + (double)i_rot; double V3 = -0.5*(double)i_x - SQRT_3*0.5*(double)i_y + (double)i_rot; Roboclaw_send(V1,V2,V3); Slave.putc(buttons); Slave.putc('\n'); } int main() { timer.attach(&timer_interrupt,INT_TIME); controller.attach(&controller_recv,RawSerial::RxIrq); while(true) {} }