taka yamanouchi / Mbed 2 deprecated minirobo_under_reserve

reserve

Dependencies:   mbed MotorDrivers

main.cpp@0:2c7c889dbf1a, 2020-10-07 (annotated)

Committer:
sink
Date:
Wed Oct 07 07:50:02 2020 +0000
Revision:
0:2c7c889dbf1a
reserve

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sink 0:2c7c889dbf1a 1 #include "mbed.h"
sink 0:2c7c889dbf1a 2 #include "RoboClaw.h"
sink 0:2c7c889dbf1a 3 #include "string"
sink 0:2c7c889dbf1a 4
sink 0:2c7c889dbf1a 5 #define SQRT_3 1.73205080757
sink 0:2c7c889dbf1a 6 #define MAX_QPPS1 40960
sink 0:2c7c889dbf1a 7 #define MAX_QPPS2 40960
sink 0:2c7c889dbf1a 8 #define INT_TIME 0.02
sink 0:2c7c889dbf1a 9 #define ADRS1 129
sink 0:2c7c889dbf1a 10 #define ADRS2 130
sink 0:2c7c889dbf1a 11
sink 0:2c7c889dbf1a 12 Ticker timer;
sink 0:2c7c889dbf1a 13
sink 0:2c7c889dbf1a 14 RawSerial Slave (p9,p10,115200);
sink 0:2c7c889dbf1a 15 RawSerial controller (p13,p14,115200);
sink 0:2c7c889dbf1a 16
sink 0:2c7c889dbf1a 17 RoboClaw MD(115200,p28,p27);
sink 0:2c7c889dbf1a 18
sink 0:2c7c889dbf1a 19 string recv_str = "";
sink 0:2c7c889dbf1a 20 char c_x = 127, c_y = 127, c_rot = 127, buttons = 0b00000000;
sink 0:2c7c889dbf1a 21
sink 0:2c7c889dbf1a 22
sink 0:2c7c889dbf1a 23
sink 0:2c7c889dbf1a 24 void controller_recv(){
sink 0:2c7c889dbf1a 25 char recv_c = controller.getc();
sink 0:2c7c889dbf1a 26
sink 0:2c7c889dbf1a 27 if(recv_str.size() == 4 && recv_c == '\n'){
sink 0:2c7c889dbf1a 28 c_x = recv_str[0];
sink 0:2c7c889dbf1a 29 c_y = recv_str[1];
sink 0:2c7c889dbf1a 30 c_rot = recv_str[2];
sink 0:2c7c889dbf1a 31 buttons = recv_str[3];
sink 0:2c7c889dbf1a 32 recv_str = "";
sink 0:2c7c889dbf1a 33 }
sink 0:2c7c889dbf1a 34
sink 0:2c7c889dbf1a 35 else{
sink 0:2c7c889dbf1a 36 recv_str += recv_c;
sink 0:2c7c889dbf1a 37 if(recv_str.size() >= 5) recv_str = "";
sink 0:2c7c889dbf1a 38 }
sink 0:2c7c889dbf1a 39 }
sink 0:2c7c889dbf1a 40
sink 0:2c7c889dbf1a 41 void Roboclaw_send(double V1, double V2, double V3){
sink 0:2c7c889dbf1a 42 MD.SpeedM1(ADRS1,int(V1*MAX_QPPS1));
sink 0:2c7c889dbf1a 43 MD.SpeedM2(ADRS1,int(V2*MAX_QPPS1));
sink 0:2c7c889dbf1a 44 MD.SpeedM1(ADRS2,int(V3*MAX_QPPS2));
sink 0:2c7c889dbf1a 45 }
sink 0:2c7c889dbf1a 46
sink 0:2c7c889dbf1a 47 void timer_interrupt(){
sink 0:2c7c889dbf1a 48 int i_x = (int)c_x - 127;
sink 0:2c7c889dbf1a 49 int i_y = (int)c_y - 127;
sink 0:2c7c889dbf1a 50 int i_rot = (int)c_rot - 127;
sink 0:2c7c889dbf1a 51
sink 0:2c7c889dbf1a 52 double V1 = (double)i_x + (double)i_rot;
sink 0:2c7c889dbf1a 53 double V2 = -0.5*(double)i_x + SQRT_3*0.5*(double)i_y + (double)i_rot;
sink 0:2c7c889dbf1a 54 double V3 = -0.5*(double)i_x - SQRT_3*0.5*(double)i_y + (double)i_rot;
sink 0:2c7c889dbf1a 55
sink 0:2c7c889dbf1a 56 Roboclaw_send(V1,V2,V3);
sink 0:2c7c889dbf1a 57 Slave.putc(buttons);
sink 0:2c7c889dbf1a 58 Slave.putc('\n');
sink 0:2c7c889dbf1a 59 }
sink 0:2c7c889dbf1a 60
sink 0:2c7c889dbf1a 61 int main() {
sink 0:2c7c889dbf1a 62
sink 0:2c7c889dbf1a 63 timer.attach(&timer_interrupt,INT_TIME);
sink 0:2c7c889dbf1a 64
sink 0:2c7c889dbf1a 65 controller.attach(&controller_recv,RawSerial::RxIrq);
sink 0:2c7c889dbf1a 66
sink 0:2c7c889dbf1a 67 while(true) {}
sink 0:2c7c889dbf1a 68 }