Diff: main.cpp

Revision:

3:67c0e888fb36

Parent:

2:0db4556d847c

Child:

4:bcdd99711326

--- a/main.cpp	Fri Nov 30 07:26:58 2018 +0000
+++ b/main.cpp	Sat Dec 01 11:49:43 2018 +0000
@@ -3,77 +3,113 @@
 #include <std_msgs/String.h>;
 #include "mbed.h"
 #include "ikarashiMDC.h"
+#define TIMEOUT 0.4
 DigitalOut led(LED1);
 CAN can(PA_11,PA_12);
 DigitalOut led2(PA_0);
-Ticker ticker;
+Timeout timeout;
 ikarashiMDC wheel[]{
     ikarashiMDC(1,0,SM,&can),
     ikarashiMDC(1,1,SM,&can),
     ikarashiMDC(1,2,SM,&can),
     ikarashiMDC(1,3,SM,&can)
 };
-ikarashiMDC arm[]{
+ikarashiMDC water[]{
     ikarashiMDC(2,0,SM,&can),
     ikarashiMDC(2,1,SM,&can),
     ikarashiMDC(2,2,SM,&can),
     ikarashiMDC(2,3,SM,&can)
 };
-ikarashiMDC water[]{
+ikarashiMDC arm[]{
     ikarashiMDC(3,0,SM,&can),
     ikarashiMDC(3,1,SM,&can),
     ikarashiMDC(3,2,SM,&can),
     ikarashiMDC(3,3,SM,&can)
 };
 double wheelSpeed[4]={0};
-double joyaxes[4];
+double joyaxes[8];
+double joybuttons[11];
 double waterPower[4]={0};
 double armPower[4]={0};
 CANMessage msg;
 std_msgs::String debug_msg;
+void reset(){
+    NVIC_SystemReset();
+}
+int Button(int a,int b){
+    if((a==1)&&(b==0)) return 1;
+    else if((a==0)&&(b==1)) return -1;
+    else return 0;
+}
 void joy_callback(const sensor_msgs::Joy &joy)
 {
-    led = ! led;
-    wheelSpeed[0]=joy.axes[1]*-1;
-    wheelSpeed[1]=joy.axes[4];
-    wheelSpeed[2]=joy.axes[1]*-1;
-    wheelSpeed[3]=joy.axes[4];
-    armPower[0]=joy.axes[6];
-    armPower[1]=joy.axes[7];        
-    if((joy.buttons[0]==1)&&(joy.buttons[3]==0)){
-        waterPower[0]=1;
-    }else if((joy.buttons[0]==0)&&(joy.buttons[3]==1)){
-        waterPower[0]=-1;
-    }else if((joy.buttons[0]==0)&&(joy.buttons[3]==0)){
-        waterPower[0]=0;
-    }else {
-        waterPower[0]=0;
-    }
-    
-    if((joy.buttons[1]==1)&&(joy.buttons[2]==0)){
-        waterPower[1]=1;
-    }else if((joy.buttons[1]==0)&&(joy.buttons[2]==1)){
-        waterPower[1]=-1;
-    }else if((joy.buttons[1]==0)&&(joy.buttons[2]==0)){
-        waterPower[1]=0;
-    }else {
-        waterPower[1]=0;
-    }
-    
-    if((joy.buttons[4]==1)&&(joy.buttons[5]==0)){
-        waterPower[2]=1;
-    }else if((joy.buttons[4]==0)&&(joy.buttons[5]==1)){
-        waterPower[2]=-1;
-    }else if ((joy.buttons[4]==0)&&(joy.buttons[5]==1)){
-        waterPower[2]=0;
-    }else {
-        waterPower[2]=0;
+    int i;
+    led = !led;
+    for(i=0;i<8;i++)
+    {
+        joyaxes[i]=joy.axes[i];
+        /*
+        0 Left/Right Axis stick left
+        1 Up/Down Axis stick left
+        2 LT
+        3 Left/Right Axis stick right
+        4 Up/Down Axis stick right
+        5 RT
+        6 cross key left/right
+        7 cross key up/down
+        */
+    }    
+    for(int i=0;i<11;i++)
+    {
+        joybuttons[i]=joy.buttons[i];
+        /*
+        0 A
+        1 B
+        2 X
+        3 Y
+        4 LB
+        5 RB
+        6 back
+        7 start
+        8 power
+        9 Button stick left
+        10 Button stick right
+        */
     }
 }
-
+void setWheelSpeed()
+{
+    if(joybuttons[9])
+    {
+        wheelSpeed[0]=joyaxes[0];
+        wheelSpeed[2]=joyaxes[1];
+    }else {
+        wheelSpeed[0]=joyaxes[0];
+        wheelSpeed[2]=joyaxes[0];
+    }
+    if(joybuttons[10])
+    {
+        wheelSpeed[1]=joyaxes[1]*-1;
+        wheelSpeed[3]=joyaxes[3]*-1;
+    }else{    
+        wheelSpeed[1]=joyaxes[1]*-1;
+        wheelSpeed[3]=joyaxes[1]*-1;
+    }
+}
+void setWaterSpeed()
+{
+    waterPower[0] = Button(joybuttons[0],joybuttons[1]);
+    waterPower[1] = Button(joybuttons[2],joybuttons[3]);
+    waterPower[2] = Button(joybuttons[4],joybuttons[5]);
+}
+void setArmSpeed()
+{
+    armPower[0]=joyaxes[6];
+    armPower[1]=joyaxes[7];
+}
 int main(){
     int i;
-    int counter=0;
+    bool flag=false;
     ros::NodeHandle nh;
     ros::Subscriber<sensor_msgs::Joy> sub("joy", &joy_callback);
     ros::Publisher pub("chatter", &debug_msg);
@@ -81,36 +117,39 @@
     nh.initNode();
     nh.subscribe(sub);
     nh.advertise(pub);
-    can.frequency(500000);
+    can.frequency(125000);
     for(i=0;i<4;i++)
     {
         wheel[i].braking = true;
         water[i].braking = true;
         arm[i].braking = true;
     }
-    wait(0.5);
     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);
+            water[i].setSpeed(waterPower[i]);
+            wait(0.001);
             arm[i].setSpeed(armPower[i]);
             wait(0.001);
         }
-        led2 = can.tderror();
-        /*if(can.tderror())
-        {
+        if(can.tderror())
+        {   
             led2 = !led2;
-            counter++;
-            if(counter==10000){
-                //can.reset();
-                counter=0;
+            if(!flag){
+                flag=true;
+                timeout.attach(&reset,TIMEOUT);
             }
-        }*/
+        }else{
+            flag=false;
+            timeout.detach();
+        }
         wait(0.01);
     }        
 }
\ No newline at end of file