main.cpp

00001 #include "mbed.h"
00002 #include "nRF24L01P.h"
00003 #include "SoftPWM.h"
00004 #define JY901transfersize 11
00005 #define TRANSFER_SIZE   11
00006 #include"JY901.h"
00007 #define pwm_period 1
00008 int flagg = 0;
00009 
00010 char targetdata[3];
00011 char receivedata[4];
00012 float change[4];
00013 int receivedatacnt = 0;
00014 SoftPWM PWM1(PA_2),PWM2(PA_3),PWM3(PA_6),PWM4(PA_7),PWM5(PB_0),PWM6(PB_1);
00015 
00016 int rollx=0,rolly=0,rollz=0,drx,dry,drz,ax,ay,az;
00017 float kp,ki,kd;
00018 void pwm_init()
00019 {
00020     PWM1.period_ms(pwm_period);
00021     PWM2.period_ms(pwm_period);
00022     PWM3.period_ms(pwm_period);
00023     PWM4.period_ms(pwm_period);
00024     PWM5.period_ms(pwm_period);
00025     PWM6.period_ms(pwm_period);
00026     PWM1=0.0;PWM2=0.0;PWM3=0.0;PWM4=0.0;PWM5=0.0;PWM6=0.0;
00027 }
00028 
00029 JY901 _JY901(PA_9,PA_10);
00030 //nRF24L01P my_nrf24l01p(PB_15, PB_14, PB_13, PC_1, PC_2, PC_0);
00031 char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
00032     int JY901cnt = 0;
00033     int txDataCnt = 0;
00034     int rxDataCnt = 0;
00035 void transferdata(){
00036     _JY901.receiveData();
00037     float acc[3];
00038             _JY901.getAttitude(acc[0], acc[1], acc[2]);
00039            PWM1=0.2;//0.13+0.13*(float)rollx/32767.0;
00040         PWM2=0.4;//0.13-0.13*(float)rolly/32767.0;
00041         PWM3=0.1-0.1*acc[0]/180;
00042 }
00043 
00044 int PIDcontrol(char *targetdata,char *data,float *change){
00045     return 1;
00046 }
00047 
00048 void update_roll()
00049 {
00050    // char *s;
00051     /*char data[JY901transfersize];
00052     char flag =0,flag2 = 0;
00053     int q=1;
00054     while(q < 1000){
00055         q++;
00056         switch(flag){
00057             case 0:
00058                 flag2=JY901.getc();
00059                 if(flag2==0x55) flag++;
00060             break;
00061             case 1:
00062                 flag2=JY901.getc();
00063                 if(flag2 == 0x53){
00064                     flag++;
00065                     data[0] = 0x55;
00066                     data[1] = flag2;
00067                     }
00068                 else flag--;
00069             break;
00070             case 2:
00071                 for(int i = 2;i != JY901transfersize;i++)
00072                     data[i] = JY901.getc();
00073                 q = 1001;
00074             break;
00075         }        
00076     }*/
00077 
00078    // rollx+=drx;
00079    // rolly+=dry;
00080     //rollz+=drz;
00081    // sprintf(s,"%f",rollx*180/32767);
00082    // my_nrf24l01p.write( NRF24L01P_PIPE_P0,s,11);
00083 }
00084 int main() { 
00085   //  my_nrf24l01p.powerUp();  
00086    // my_nrf24l01p.setTransferSize( 11);
00087    // my_nrf24l01p.setReceiveMode();
00088   //  my_nrf24l01p.enable();  
00089 
00090     Ticker time;
00091     time.attach(&transferdata,0.1); 
00092     while(1){
00093    
00094        // pc.printf("succes");
00095  
00096     
00097     } 
00098        
00099 }
00100