Diff: main.cpp

Revision:

1:e15fbee169e7

Parent:

0:e5287b509fe9

--- a/main.cpp	Tue Dec 04 06:36:29 2018 +0000
+++ b/main.cpp	Tue Dec 04 09:00:44 2018 +0000
@@ -1,5 +1,4 @@
 #include "mbed.h"
-//#include "ikarashiMDC.h"
 #include "algorithm"
 #include <ros.h>
 #include <sensor_msgs/Joy.h>
@@ -9,7 +8,7 @@
 CAN can(PA_11,PA_12);
 DigitalOut led2(PA_0);
 Timeout timeout;
-
+int modeflag=0;
 double wheelSpeed[4]={0};
 double joyaxes[8];
 double joybuttons[11];
@@ -67,22 +66,29 @@
 }
 void setWheelSpeed()
 {
-
-    if(joybuttons[9]==1)
+    static int mode=0,toggle=0;
+    if((joybuttons[7]==1)&&(toggle==0)&&(mode==0))
     {
-        wheelSpeed[0]=joyaxes[0];
-        wheelSpeed[2]=joyaxes[1];
-    }else {
+        toggle=1;
+        mode=1;
+    }else if((joybuttons[7]==1)&&(toggle==0)&&(mode==1)){
+        toggle=1;
+        mode=0;
+    }else{
+        toggle=0;
+    }    
+    /*normal mode*/
+    if(mode==0)
+    {
         wheelSpeed[0]=joyaxes[1]*-1;
+        wheelSpeed[1]=joyaxes[4];
         wheelSpeed[2]=joyaxes[1]*-1;
-    }
-    if(joybuttons[10]==1)
-    {
-        wheelSpeed[1]=joyaxes[3]*-1;
-        wheelSpeed[3]=joyaxes[4]*-1;
-    }else{    
-        wheelSpeed[1]=joyaxes[4];
         wheelSpeed[3]=joyaxes[4];
+    }else{
+        wheelSpeed[0]=joyaxes[1]*-1;
+        wheelSpeed[1]=joyaxes[1];
+        wheelSpeed[2]=joyaxes[3]*0.3;
+        wheelSpeed[3]=joyaxes[3]*0.3;
     }
     msg.id = 1;
     msg.len = 4;