Wataru Shibazaki / Mbed 2 deprecated minirobo_under_reserve

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MotorDrivers

main.cpp@1:57524647d891, 2020-10-11 (annotated)

Committer:
shibazakiwataru
Date:
Sun Oct 11 08:43:51 2020 +0000
Revision:
1:57524647d891
Parent:
0:2c7c889dbf1a 
yamanouti_program_ashi

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
shibazakiwataru 1:57524647d891 6 #define MAX_QPPS1 40960*0.1
shibazakiwataru 1:57524647d891 7 #define MAX_QPPS2 40960*0.1
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
shibazakiwataru 1:57524647d891 14 RawSerial Slave (p28,p27,115200);
shibazakiwataru 1:57524647d891 15 RawSerial controller (p9,p10,115200);
shibazakiwataru 1:57524647d891 16 RawSerial pc(USBTX,USBRX,115200);
sink 0:2c7c889dbf1a 17
shibazakiwataru 1:57524647d891 18 RoboClaw MD(115200,p13,p14);
sink 0:2c7c889dbf1a 19
sink 0:2c7c889dbf1a 20 string recv_str = "";
sink 0:2c7c889dbf1a 21 char c_x = 127, c_y = 127, c_rot = 127, buttons = 0b00000000;
sink 0:2c7c889dbf1a 22
sink 0:2c7c889dbf1a 23
sink 0:2c7c889dbf1a 24
sink 0:2c7c889dbf1a 25 void controller_recv(){
sink 0:2c7c889dbf1a 26 char recv_c = controller.getc();
sink 0:2c7c889dbf1a 27
sink 0:2c7c889dbf1a 28 if(recv_str.size() == 4 && recv_c == '\n'){
shibazakiwataru 1:57524647d891 29 c_y = recv_str[0];
shibazakiwataru 1:57524647d891 30 c_x = recv_str[1];
sink 0:2c7c889dbf1a 31 c_rot = recv_str[2];
sink 0:2c7c889dbf1a 32 buttons = recv_str[3];
sink 0:2c7c889dbf1a 33 recv_str = "";
sink 0:2c7c889dbf1a 34 }
sink 0:2c7c889dbf1a 35
sink 0:2c7c889dbf1a 36 else{
sink 0:2c7c889dbf1a 37 recv_str += recv_c;
sink 0:2c7c889dbf1a 38 if(recv_str.size() >= 5) recv_str = "";
sink 0:2c7c889dbf1a 39 }
sink 0:2c7c889dbf1a 40 }
sink 0:2c7c889dbf1a 41
sink 0:2c7c889dbf1a 42 void Roboclaw_send(double V1, double V2, double V3){
sink 0:2c7c889dbf1a 43 MD.SpeedM1(ADRS1,int(V1*MAX_QPPS1));
sink 0:2c7c889dbf1a 44 MD.SpeedM2(ADRS1,int(V2*MAX_QPPS1));
sink 0:2c7c889dbf1a 45 MD.SpeedM1(ADRS2,int(V3*MAX_QPPS2));
sink 0:2c7c889dbf1a 46 }
sink 0:2c7c889dbf1a 47
sink 0:2c7c889dbf1a 48 void timer_interrupt(){
sink 0:2c7c889dbf1a 49 int i_x = (int)c_x - 127;
sink 0:2c7c889dbf1a 50 int i_y = (int)c_y - 127;
sink 0:2c7c889dbf1a 51 int i_rot = (int)c_rot - 127;
sink 0:2c7c889dbf1a 52
sink 0:2c7c889dbf1a 53 double V1 = (double)i_x + (double)i_rot;
sink 0:2c7c889dbf1a 54 double V2 = -0.5*(double)i_x + SQRT_3*0.5*(double)i_y + (double)i_rot;
sink 0:2c7c889dbf1a 55 double V3 = -0.5*(double)i_x - SQRT_3*0.5*(double)i_y + (double)i_rot;
sink 0:2c7c889dbf1a 56
shibazakiwataru 1:57524647d891 57 Roboclaw_send(V3,V2,V1);
sink 0:2c7c889dbf1a 58 Slave.putc(buttons);
sink 0:2c7c889dbf1a 59 Slave.putc('\n');
shibazakiwataru 1:57524647d891 60 pc.printf("%d,%d,%d\n",(int)i_x,(int)i_y,(int)i_rot);
sink 0:2c7c889dbf1a 61 }
sink 0:2c7c889dbf1a 62
sink 0:2c7c889dbf1a 63 int main() {
sink 0:2c7c889dbf1a 64
sink 0:2c7c889dbf1a 65 timer.attach(&timer_interrupt,INT_TIME);
sink 0:2c7c889dbf1a 66
sink 0:2c7c889dbf1a 67 controller.attach(&controller_recv,RawSerial::RxIrq);
sink 0:2c7c889dbf1a 68
sink 0:2c7c889dbf1a 69 while(true) {}
sink 0:2c7c889dbf1a 70 }