main.cpp

00001 #include "mbed.h"
00002 #include "algorithm"
00003 #include <ros.h>
00004 #include <sensor_msgs/Joy.h>
00005 #include <std_msgs/String.h>
00006 #define TIMEOUT 0.4
00007 DigitalOut led(LED1);
00008 CAN can(PA_11,PA_12);
00009 DigitalOut led2(PA_0);
00010 Timeout timeout;
00011 int modeflag=0;
00012 double wheelSpeed[4]={0};
00013 double joyaxes[8];
00014 double joybuttons[11];
00015 double waterPower[4]={0};
00016 double armPower[4]={0};
00017 CANMessage msg;
00018 std_msgs::String debug_msg;
00019 void reset(){
00020     //NVIC_SystemReset();
00021 }
00022 int Button(int a,int b){
00023     if((a==1)&&(b==0)) return 1;
00024     else if((a==0)&&(b==1)) return -1;
00025     else return 0;
00026 }
00027 void joy_callback(const sensor_msgs::Joy &joy)
00028 {
00029     int i;
00030     led = !led;
00031     for(i=0;i<8;i++)
00032     {
00033         joyaxes[i]=joy.axes[i];
00034         /*
00035         0 Left/Right Axis stick left
00036         1 Up/Down Axis stick left
00037         2 LT
00038         3 Left/Right Axis stick right
00039         4 Up/Down Axis stick right
00040         5 RT
00041         6 cross key left/right
00042         7 cross key up/down
00043         */
00044     }    
00045     for(i=0;i<11;i++)
00046     {
00047         joybuttons[i]=joy.buttons[i];
00048         /*
00049         0 A
00050         1 B
00051         2 X
00052         3 Y
00053         4 LB
00054         5 RB
00055         6 back
00056         7 start
00057         8 power
00058         9 Button stick left
00059         10 Button stick right
00060         */
00061     }
00062 }
00063 double cropped_speed(double speed)
00064 {
00065     return std::min(1.0,std::max(-1.0,speed));
00066 }
00067 void setWheelSpeed()
00068 {
00069     static int mode=0,toggle=0;
00070     if((joybuttons[7]==1)&&(toggle==0)&&(mode==0))
00071     {
00072         toggle=1;
00073         mode=1;
00074     }else if((joybuttons[7]==1)&&(toggle==0)&&(mode==1)){
00075         toggle=1;
00076         mode=0;
00077     }else{
00078         toggle=0;
00079     }    
00080     /*normal mode*/
00081     if(mode==0)
00082     {
00083         wheelSpeed[0]=joyaxes[1]*-1;
00084         wheelSpeed[1]=joyaxes[4];
00085         wheelSpeed[2]=joyaxes[1]*-1;
00086         wheelSpeed[3]=joyaxes[4];
00087     }else{
00088         wheelSpeed[0]=joyaxes[1]*-1;
00089         wheelSpeed[1]=joyaxes[1];
00090         wheelSpeed[2]=joyaxes[3]*0.3;
00091         wheelSpeed[3]=joyaxes[3]*0.3;
00092     }
00093     msg.id = 1;
00094     msg.len = 4;
00095     for(int i=0;i<4;i++)
00096     {
00097         msg.data[i] = ((cropped_speed(wheelSpeed[i])+1.0)/2.0)*253;
00098     }
00099     can.write(msg);
00100 }
00101 void setWaterSpeed()
00102 {
00103     waterPower[0] = Button(joybuttons[0],joybuttons[1]);
00104     waterPower[1] = Button(joybuttons[2],joybuttons[3]);
00105     waterPower[2] = Button(joybuttons[4],joybuttons[5]);
00106     msg.id = 2;
00107     msg.len = 4;
00108     for(int i=0;i<4;i++)
00109     {
00110         msg.data[i] = ((cropped_speed(waterPower[i])+1.0)/2.0)*253;
00111     }
00112     can.write(msg);
00113 }
00114 void setArmSpeed()
00115 {
00116     armPower[0]=joyaxes[6];
00117     armPower[1]=joyaxes[7];
00118     msg.id = 3;
00119     msg.len = 4;
00120     for(int i=0;i<4;i++)
00121     {
00122         msg.data[i] = ((cropped_speed(armPower[i])+1.0)/2.0)*253;
00123     }
00124     can.write(msg);
00125 }
00126 int main(){
00127     bool flag=false;
00128     ros::NodeHandle nh;
00129     ros::Subscriber<sensor_msgs::Joy> sub("joy", &joy_callback);
00130     ros::Publisher pub("chatter", &debug_msg);
00131     nh.getHardware()->setBaud(921600);
00132     nh.initNode();
00133     nh.subscribe(sub);
00134     nh.advertise(pub);
00135     can.frequency(125000);
00136     while(1)
00137     {
00138         nh.spinOnce();
00139         setWheelSpeed();
00140         setWaterSpeed();
00141         setArmSpeed();
00142         if(can.tderror())
00143         {   
00144             led2 = !led2;
00145             if(!flag){
00146                 flag=true;
00147                 timeout.attach(&reset,TIMEOUT);
00148             }
00149         }else{
00150             flag=false;
00151             timeout.detach();
00152         }
00153         wait(0.01);
00154     }        
00155 }