Wataru Shibazaki / Mbed 2 deprecated minirobo_under_reserve

yamanouti_program_ashi

Dependencies:   mbed MotorDrivers

Files at this revision

API Documentation at this revision

Revision 1:57524647d891, committed 2020-10-11

Comitter:
shibazakiwataru
Date:
Sun Oct 11 08:43:51 2020 +0000
Parent:
0:2c7c889dbf1a
Commit message:
yamanouti_program_ashi

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
diff -r 2c7c889dbf1a -r 57524647d891 main.cpp
--- a/main.cpp	Wed Oct 07 07:50:02 2020 +0000
+++ b/main.cpp	Sun Oct 11 08:43:51 2020 +0000
@@ -3,18 +3,19 @@
 #include "string"
 
 #define SQRT_3 1.73205080757
-#define MAX_QPPS1 40960
-#define MAX_QPPS2 40960
+#define MAX_QPPS1 40960*0.1
+#define MAX_QPPS2 40960*0.1
 #define INT_TIME 0.02
 #define ADRS1 129
 #define ADRS2 130
 
 Ticker timer;
 
-RawSerial Slave (p9,p10,115200);
-RawSerial controller (p13,p14,115200);
+RawSerial Slave (p28,p27,115200);
+RawSerial controller (p9,p10,115200);
+RawSerial pc(USBTX,USBRX,115200);
 
-RoboClaw MD(115200,p28,p27);
+RoboClaw MD(115200,p13,p14);
 
 string recv_str = "";
 char c_x = 127, c_y = 127, c_rot  = 127, buttons = 0b00000000;
@@ -25,8 +26,8 @@
     char recv_c = controller.getc();
     
     if(recv_str.size() == 4 && recv_c == '\n'){
-        c_x = recv_str[0];
-        c_y = recv_str[1];
+        c_y = recv_str[0];
+        c_x = recv_str[1];
         c_rot = recv_str[2];
         buttons = recv_str[3];
         recv_str = "";
@@ -53,9 +54,10 @@
     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);
+    Roboclaw_send(V3,V2,V1);
     Slave.putc(buttons);
     Slave.putc('\n');
+    pc.printf("%d,%d,%d\n",(int)i_x,(int)i_y,(int)i_rot);
 }
 
 int main() {