main.cpp

00001 //送信
00002 #include "mbed.h"
00003 #include "sbus.h"
00004 #include "can_network_motordriver_definition.h"
00005 
00006 
00007 CAN pinName(p30,p29);//(rd,td)
00008 CAN can(p9,p10);
00009 
00010 CANMessage bluemd;//青色md
00011 CANMessage servo;//サーボ
00012 CANMessage load;//装填ステッピング
00013 
00014 
00015 Serial pc(USBTX,USBRX,115200);
00016 Sbus sbus(p13,p14);
00017 //DigitalOut fan(p23);//p23
00018 PwmOut fan(p23);
00019 Ticker get_date;
00020 Ticker controlCycle;
00021 int a,b,c,d,e,f,g,h;
00022 float rx,ry,lx,ly,vr,V1,V2,V3,V4;
00023 Ticker timer;
00024 
00025 union vr_data{float fvr;char cvr[4];};
00026 vr_data VRer;
00027 
00028 union ry_data{float fry;char cry[4];};
00029 ry_data Data2;
00030 
00031 union rx_data{float frx;char crx[4];};
00032 rx_data RX;
00033 
00034 
00035 void puropo();   
00036 void send(); 
00037 void mainLoop();
00038  
00039 
00040 
00041 int main() {
00042     timer.attach(&send,0.05);//canのタイマ割込み
00043     get_date.attach(&puropo,0.01f);//プロポのタイマ割込み
00044     sbus.setup(Sbus::VR,Sbus::SW_D,Sbus::SW_C,Sbus::SW_B,Sbus::SW_E,Sbus::SW_A);
00045     pinName.frequency(250000);
00046     fan.period(0.00005f);
00047     fan.pulsewidth(0.020);
00048         
00049     can.frequency(500000);
00050    controlCycle.attach(&mainLoop,0.01f);
00051     
00052     bluemd.id = 0x103;
00053     for(int i=0;i<8;i++)  bluemd.data[i] = (int)i;
00054     bluemd.len = 8;                                 
00055     bluemd.type = CANData;                           
00056     bluemd.format = CANStandard;
00057     
00058     servo.id = 0x202;
00059     for(int i=0;i<8;i++)  servo.data[i] = (int)i;
00060     servo.len = 8;                                 
00061     servo.type = CANData;                           
00062     servo.format = CANStandard;
00063     
00064     load.id = 0x302;
00065     for(int i=0;i<8;i++)  load.data[i] = (int)i;
00066     load.len = 8;                                 
00067     load.type = CANData;                           
00068     load.format = CANStandard;
00069     
00070 while(1) {
00071         //fan = 1;
00072         //fan.write(0.8);
00073              if(d == 2&& a == 2){  
00074       /*V1=((-lx)+(ly))/2.0f;
00075       V2=((-lx)+(-ly))/2.0f;
00076       V3=((lx)+(-ly))/2.0f;
00077       V4=((lx)+(ly))/2.0f;*/
00078       V1=((-lx)+(ly))/5.0f;
00079       V2=((-lx)+(-ly))/5.0f;
00080       V3=((lx)+(-ly))/5.0f;
00081       V4=((lx)+(ly))/5.0f;
00082        }else if(d == 2){
00083                     V2 = 0.1;
00084                     V1 = 0.1;
00085                     V3 = 0.1;
00086                     V4 = 0.1;
00087                         }else if(a == 2){
00088                             V2 = -0.1;
00089                             V1 = -0.1;
00090                             V3 = -0.1;
00091                             V4 = -0.1;
00092                                         }
00093                                     
00094                                         
00095             //pc.printf("%f",Data2.frx);
00096             pc.printf("A = %d' F= %d'H = %d'D = %d'e = %d'E = %d'vr = %f",a,b,c,d,e,f,vr);
00097             //pc.printf("%f",Data1.flx);
00098             pc.printf("\n\r");
00099     }
00100 }
00101 
00102 void puropo(){
00103     
00104             a=sbus.getSwitchValue(0);
00105             b=sbus.getSwitchValue(1); 
00106             c=sbus.getSwitchValue(2);
00107             d=sbus.getSwitchValue(3);
00108             e=sbus.getSwitchValue(4);
00109             f=sbus.getSwitchValue(5);
00110 
00111             
00112             
00113    
00114             RX.frx = sbus.getStickValue(0);
00115             Data2.fry=sbus.getStickValue(1);
00116             lx=sbus.getStickValue(2);
00117             ly=sbus.getStickValue(3);
00118             vr = sbus.getVolumeValue();
00119             
00120   }  
00121   
00122 void send(){
00123   memcpy(load.data,Data2.cry,4);
00124   pinName.write(load);
00125   
00126   VRer.fvr=vr;
00127   memcpy(bluemd.data,VRer.cvr,4);
00128   pinName.write(bluemd);
00129   
00130   //memcpy(load.data,RX.crx,4);
00131   load.data[4]=RX.crx[0];
00132   load.data[5]=RX.crx[1];
00133   load.data[6]=RX.crx[2];
00134   load.data[7]=RX.crx[3];
00135   pinName.write(load);
00136   
00137 
00138   if(c == 2){
00139     servo.data[5]=6;
00140     }
00141     else{bluemd.data[5]=0;}
00142     
00143  
00144   pinName.write(servo);
00145 }
00146 
00147  void mainLoop() {
00148     static int a = 0;
00149     switch(a){
00150         case 0:
00151            // motordriverTransmitMessage(&can,MD_1_MCU,md_can_flame::SEND_CONTROL_DUTY,V1);
00152            motordriverTransmitMessage(&can,MD_1_MCU,md_can_flame::SEND_CONTROL_DUTY,V4);
00153                 break;
00154         case 1:
00155            // motordriverTransmitMessage(&can,MD_2_MCU,md_can_flame::SEND_CONTROL_DUTY,V2);
00156            motordriverTransmitMessage(&can,MD_2_MCU,md_can_flame::SEND_CONTROL_DUTY,V1);
00157                 break;
00158         case 2:      
00159             //motordriverTransmitMessage(&can,MD_3_MCU,md_can_flame::SEND_CONTROL_DUTY,V3);
00160             motordriverTransmitMessage(&can,MD_3_MCU,md_can_flame::SEND_CONTROL_DUTY,V2);          
00161                 break;
00162         case 3:
00163                 //motordriverTransmitMessage(&can,MD_4_MCU,md_can_flame::SEND_CONTROL_DUTY,V4);
00164                 motordriverTransmitMessage(&can,MD_4_MCU,md_can_flame::SEND_CONTROL_DUTY,V3);
00165                 break;
00166        /* case 4:
00167             motordriverTransmitMessage(&can,MD_5_MCU,md_can_flame::SEND_CONTROL_DUTY,0.8);
00168                 break;*/
00169         default:
00170                 break;
00171     }
00172         if(a >= 4){
00173                 a = 0;
00174         }else{
00175                 a++;
00176          }
00177 }