main.cpp

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