File content as of revision 1:e15fbee169e7:

#include "mbed.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;
int modeflag=0;
double wheelSpeed[4]={0};
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)
{
    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(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
        */
    }
}
double cropped_speed(double speed)
{
    return std::min(1.0,std::max(-1.0,speed));
}
void setWheelSpeed()
{
    static int mode=0,toggle=0;
    if((joybuttons[7]==1)&&(toggle==0)&&(mode==0))
    {
        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;
        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;
    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(){
    bool flag=false;
    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(125000);
    while(1)
    {
        nh.spinOnce();
        setWheelSpeed();
        setWaterSpeed();
        setArmSpeed();
        if(can.tderror())
        {   
            led2 = !led2;
            if(!flag){
                flag=true;
                timeout.attach(&reset,TIMEOUT);
            }
        }else{
            flag=false;
            timeout.detach();
        }
        wait(0.01);
    }        
}