Diff: main.cpp

Revision:

2:0db4556d847c

Child:

3:67c0e888fb36

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 30 07:26:58 2018 +0000
@@ -0,0 +1,116 @@
+#include <ros.h>;
+#include <sensor_msgs/Joy.h>;
+#include <std_msgs/String.h>;
+#include "mbed.h"
+#include "ikarashiMDC.h"
+DigitalOut led(LED1);
+CAN can(PA_11,PA_12);
+DigitalOut led2(PA_0);
+Ticker ticker;
+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(2,0,SM,&can),
+    ikarashiMDC(2,1,SM,&can),
+    ikarashiMDC(2,2,SM,&can),
+    ikarashiMDC(2,3,SM,&can)
+};
+ikarashiMDC water[]{
+    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 waterPower[4]={0};
+double armPower[4]={0};
+CANMessage msg;
+std_msgs::String debug_msg;
+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 main(){
+    int i;
+    int counter=0;
+    ros::NodeHandle nh;
+    ros::Subscriber<sensor_msgs::Joy> sub("joy", &joy_callback);
+    ros::Publisher pub("chatter", &debug_msg);
+    nh.getHardware()->setBaud(921600);
+    nh.initNode();
+    nh.subscribe(sub);
+    nh.advertise(pub);
+    can.frequency(500000);
+    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();
+        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);
+        }
+        led2 = can.tderror();
+        /*if(can.tderror())
+        {
+            led2 = !led2;
+            counter++;
+            if(counter==10000){
+                //can.reset();
+                counter=0;
+            }
+        }*/
+        wait(0.01);
+    }        
+}
\ No newline at end of file