V1.cpp

00001 #include "mbed.h"
00002 #include "use_can_stm.h"
00003 #include "use_can_stm.cpp"
00004 #define CanStm_RD PA11
00005 #define CanStm_XD PA12
00006 
00007 Serial PC(USBTX,USBRX,115200);
00008 float V1,V2,V3,V4,lx,ly;
00009 float cw,ccw;//モータードライバーにPWMする値
00010 Ticker purppo;//CANの受け取り
00011 CanStm Can1;
00012 
00013 void read();
00014 void v1d();
00015 void V_data();
00016 PwmOut P1cw(D1);
00017 PwmOut P1ccw(D9);
00018 
00019 union V1_data{float fv1;char cv1[4];};
00020 V1_data Data1;
00021 
00022 union V2_data{float fv2;char cv2[4];};
00023 V2_data Data2;                              //CANでfloat受け取るためのユニオン
00024 
00025 
00026 
00027 
00028 int main()
00029 {  
00030     purppo.attach(&V_data,0.01f);
00031     
00032     Can1.init(CAN_500KHZ);
00033     P1cw.period(0.00005f);
00034     P1ccw.period(0.00005f);
00035 
00036     Can1.attach(0x110,&v1d,RX_FIFO0);
00037    
00038    
00039     while(1) {
00040              
00041               ly=Data2.fv2;
00042               lx=Data1.fv1;
00043              
00044              
00045              
00046              
00047              P1ccw.write(ccw);//Ｖ1の逆転出力
00048              P1cw.write(cw);   //Ｖ1の正転出力
00049            
00050               V1=((-lx)+(ly))/2.0f;
00051               V2=((-lx)+(-ly))/2.0f;
00052               V3=((lx)+(-ly))/2.0f;
00053               V4=((lx)+(ly))/2.0f;//各Ⅴ成分の計算
00054              
00055   
00056          
00057          PC.printf("lx=%f,ly=%f,V1=%f,V2=%f,V3=%f,V4=%f\n\r",lx,ly,V1,V2,V3,V4);
00058     }
00059 }
00060 
00061     
00062 void v1d() {
00063               Can1.receive();
00064              
00065               Data1.cv1[0]=Can1.getData(RX_DATA_0);
00066               Data1.cv1[1]=Can1.getData(RX_DATA_1);
00067               Data1.cv1[2]=Can1.getData(RX_DATA_2);
00068               Data1.cv1[3]=Can1.getData(RX_DATA_3);//LXのデータ受け取り
00069               
00070               Data2.cv2[0]=Can1.getData(RX_DATA_4);
00071               Data2.cv2[1]=Can1.getData(RX_DATA_5);
00072               Data2.cv2[2]=Can1.getData(RX_DATA_6);
00073               Data2.cv2[3]=Can1.getData(RX_DATA_7);//LYのデータ受け取り
00074              
00075     }
00076     void V_data(){
00077                     if(V1>0){
00078                  cw=(fabs(V1));ccw=0;//cwにPWM出力
00079                  }
00080              else if(V1<0){
00081                  ccw=(fabs(V1));cw=0;//ccwにPWM出力
00082                  }
00083             else if(V1==0){cw=0;ccw=0;}
00084         
00085         
00086         
00087         }
00088 
00089 
00090 
00091 
00092 
00093