File content as of revision 2:0db4556d847c:

#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);
    }        
}