main.cpp

00001 #include "mbed.h"
00002 #include "sbus.h"
00003 #include "bno055_lib.h"     //BNO055 本家ライブラリ
00004 #include "bno055_use.h"     //BNO055 データ取得用Class
00005 #define BNO005_SDA p28
00006 #define BNO005_SCL p27  //ジャイロのピン指定
00007 
00008 
00009       
00010 Serial PC(USBTX,USBRX,115200);
00011 Sbus sbus(p9,p10);
00012 Ticker get_date;
00013 CAN tusin(p30,p29);
00014 CANMessage v1_can;
00015 
00016 
00017 
00018 
00019 union V1_data{float fv1;char cv1[4];}; //LX
00020 V1_data Data1;
00021 
00022 union V2_data{float fv2;char cv2[4];}; //LY
00023 V2_data Data2;
00024 
00025 
00026 
00027 
00028 
00029 
00030 int a,b,c,d,e,f;
00031 float rx,ry,lx,ly,vr,V1,V2,V3,V4;
00032 void puropo();     
00033 void PWM();
00034 void c_tx();
00035 Bno055 bno055;
00036 float yaw;
00037 float roll;
00038  
00039 int main()
00040 {  
00041    
00042    tusin.frequency(500000);
00043    get_date.attach(&puropo,0.01f);
00044    sbus.setup(Sbus::SW_A,Sbus::SW_B,Sbus::SW_C,Sbus::SW_D,Sbus::SW_E,Sbus::SW_F); 
00045    
00046    
00047  
00048    
00049 
00050 v1_can.id=(0x110);
00051   
00052 
00053     while(1) {
00054        
00055         
00056         
00057   
00058         
00059         
00060       V1=((-lx)+(ly))/2.0f;
00061       V2=((-lx)+(-ly))/2.0f;
00062       V3=((lx)+(-ly))/2.0f;
00063       V4=((lx)+(ly))/2.0f;
00064        
00065        
00066        
00067        Data1.fv1=lx; 
00068        v1_can.data[0]=Data1.cv1[0];  
00069        v1_can.data[1]=Data1.cv1[1];
00070        v1_can.data[2]=Data1.cv1[2];
00071        v1_can.data[3]=Data1.cv1[3];
00072        
00073        Data2.fv2=ly; 
00074        v1_can.data[4]=Data2.cv2[0];  
00075        v1_can.data[5]=Data2.cv2[1];
00076        v1_can.data[6]=Data2.cv2[2];
00077        v1_can.data[7]=Data2.cv2[3];
00078        
00079     tusin.write(v1_can);
00080       
00081        
00082        
00083        
00084 
00085 
00086       PC.printf("%d=a,%d=b,%d=c,%d=d,%d=e,%d=f,%f=rx,%f=ry,%f=lx,%f=ly,%f=V1,%f=V2,%f=V3,%f=V4,%f=角度\n\r",a,b,c,d,e,f,rx,ry,lx,ly,V1,V2,V3,V4,yaw);
00087 
00088     }
00089 }
00090 
00091 void puropo(){
00092     
00093             a=sbus.getSwitchValue(0);
00094             b=sbus.getSwitchValue(1); 
00095             c=sbus.getSwitchValue(2);
00096             d=sbus.getSwitchValue(3);
00097             e=sbus.getSwitchValue(4);
00098             f=sbus.getSwitchValue(5);
00099    
00100             rx=sbus.getStickValue(0);
00101             ry=sbus.getStickValue(1);
00102             lx=sbus.getStickValue(2);
00103             ly=sbus.getStickValue(3);
00104   }
00105 
00106    
00107    
00108      
00109                                 
00110     
00111     
00112     
00113