Diff: main.cpp

Revision:

4:bcdd99711326

Parent:

3:67c0e888fb36

--- a/main.cpp	Sat Dec 01 11:49:43 2018 +0000
+++ b/main.cpp	Tue Dec 04 00:14:32 2018 +0000
@@ -1,13 +1,15 @@
-#include <ros.h>;
-#include <sensor_msgs/Joy.h>;
-#include <std_msgs/String.h>;
 #include "mbed.h"
-#include "ikarashiMDC.h"
+//#include "ikarashiMDC.h"
+#include "algorithm"
+#include <ros.h>
+#include <sensor_msgs/Joy.h>
+#include <std_msgs/String.h>
 #define TIMEOUT 0.4
 DigitalOut led(LED1);
 CAN can(PA_11,PA_12);
 DigitalOut led2(PA_0);
 Timeout timeout;
+/*
 ikarashiMDC wheel[]{
     ikarashiMDC(1,0,SM,&can),
     ikarashiMDC(1,1,SM,&can),
@@ -25,7 +27,7 @@
     ikarashiMDC(3,1,SM,&can),
     ikarashiMDC(3,2,SM,&can),
     ikarashiMDC(3,3,SM,&can)
-};
+};*/
 double wheelSpeed[4]={0};
 double joyaxes[8];
 double joybuttons[11];
@@ -59,7 +61,7 @@
         7 cross key up/down
         */
     }    
-    for(int i=0;i<11;i++)
+    for(i=0;i<11;i++)
     {
         joybuttons[i]=joy.buttons[i];
         /*
@@ -77,9 +79,18 @@
         */
     }
 }
+double cropped_speed(double speed)
+{
+    return std::min(1.0,std::max(-1.0,speed));
+}
 void setWheelSpeed()
 {
-    if(joybuttons[9])
+    /*
+    wheelSpeed[0]=joyaxes[0]*-1;
+    wheelSpeed[1]=joyaxes[0]*-1;
+    wheelSpeed[2]=joyaxes[0]*-1;
+    wheelSpeed[3]=joyaxes[0]*-1;
+    */if(joybuttons[9]==1)
     {
         wheelSpeed[0]=joyaxes[0];
         wheelSpeed[2]=joyaxes[1];
@@ -87,25 +98,46 @@
         wheelSpeed[0]=joyaxes[0];
         wheelSpeed[2]=joyaxes[0];
     }
-    if(joybuttons[10])
+    if(joybuttons[10]==1)
     {
-        wheelSpeed[1]=joyaxes[1]*-1;
-        wheelSpeed[3]=joyaxes[3]*-1;
+        wheelSpeed[1]=joyaxes[3]*-1;
+        wheelSpeed[3]=joyaxes[4]*-1;
     }else{    
-        wheelSpeed[1]=joyaxes[1]*-1;
-        wheelSpeed[3]=joyaxes[1]*-1;
+        wheelSpeed[1]=joyaxes[4]*-1;
+        wheelSpeed[3]=joyaxes[4]*-1;
     }
+    msg.id = 1;
+    msg.len = 4;
+    for(int i=0;i<4;i++)
+    {
+        msg.data[i] = ((cropped_speed(wheelSpeed[i])+1.0)/2.0)*253;
+    }
+    can.write(msg);
 }
 void setWaterSpeed()
 {
     waterPower[0] = Button(joybuttons[0],joybuttons[1]);
     waterPower[1] = Button(joybuttons[2],joybuttons[3]);
     waterPower[2] = Button(joybuttons[4],joybuttons[5]);
+    msg.id = 2;
+    msg.len = 4;
+    for(int i=0;i<4;i++)
+    {
+        msg.data[i] = ((cropped_speed(waterPower[i])+1.0)/2.0)*253;
+    }
+    can.write(msg);
 }
 void setArmSpeed()
 {
     armPower[0]=joyaxes[6];
     armPower[1]=joyaxes[7];
+    msg.id = 3;
+    msg.len = 4;
+    for(int i=0;i<4;i++)
+    {
+        msg.data[i] = ((cropped_speed(armPower[i])+1.0)/2.0)*253;
+    }
+    can.write(msg);
 }
 int main(){
     int i;
@@ -118,27 +150,18 @@
     nh.subscribe(sub);
     nh.advertise(pub);
     can.frequency(125000);
-    for(i=0;i<4;i++)
+    /*for(i=0;i<4;i++)
     {
         wheel[i].braking = true;
         water[i].braking = true;
         arm[i].braking = true;
-    }
+    }*/
     while(1)
     {
         nh.spinOnce();
         setWheelSpeed();
         setWaterSpeed();
         setArmSpeed();
-        for(i=0;i<4;i++)
-        {
-            wheel[i].setSpeed(wheelSpeed[i]);
-            wait(0.001);
-            water[i].setSpeed(waterPower[i]);
-            wait(0.001);
-            arm[i].setSpeed(armPower[i]);
-            wait(0.001);
-        }
         if(can.tderror())
         {   
             led2 = !led2;