File content as of revision 15:934289377f7a:

#define HIGH 1
#define LOW 0
#include "mbed.h"
#include <string>
typedef bool boolean;
typedef std::string String;
#include "nRF24L01P.h"
#include "JY901.h"
#include "QEI.h"
#include "converters.h"
float Ps,Ds,Is;
float Pa,Da,Ia;
float Error_S,Error_SI,Error_SD,Error_SLast;
float Error_A,Error_AI,Error_AD,Error_ALast;
float Target_A,Target_S;
float Speed_Out,Angle_Out;
float Roll,Yaw,Pitch;
float Speed_L,Speed_R;
float Gyo_x,Gyo_y,Gyo_z;
float Left_Speed,Right_Speed;

Serial Serial_1(PA_2,PA_3);
#define NRF24_TRANSFER_SIZE 32
nRF24L01P nrf24_PB_15(PB_15,PB_14,PB_13,PA_8,PB_12,PA_11);
JY901 jy901_Serial_3(PB_10,PB_11);
PwmOut myServoPA_7(PA_7);
PwmOut myServoPA_6(PA_6);
PwmOut myServoPB_1(PB_1);
PwmOut myServoPB_0(PB_0);
QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING);
QEI qei_PC_14(PC_14,PC_15,NC,13,QEI::X4_ENCODING);
Ticker tick969801;

void init() {
    Pa=6;Ia=0;Da=1;
    Ps=0.1;Is=0;Ds=0;
    Target_A=0;
    Target_S=0;
    Error_SLast=Error_ALast=0;
    qei_PA_0.reset();
    qei_PC_14.reset();
}

void Set_Pwm() {
    Left_Speed=Speed_Out+Angle_Out;
    Right_Speed=Speed_Out+Angle_Out;
    if (Left_Speed >=0) 
    {
        myServoPA_7.period_ms(20);
        myServoPA_7.pulsewidth_us(0);
        myServoPA_6.period_ms(20);
        myServoPA_6.pulsewidth_us((Left_Speed * (200000 / 1000)));
    } 
    else 
    {
        myServoPA_7.period_ms(20);
        myServoPA_7.pulsewidth_us(((-Left_Speed) * (200000 / 1000)));
        myServoPA_6.period_ms(20);
        myServoPA_6.pulsewidth_us(0);
    }
    if (Right_Speed >=0) 
    {
        myServoPB_1.period_ms(20);
        myServoPB_1.pulsewidth_us(0);
        myServoPB_0.period_ms(20);
        myServoPB_0.pulsewidth_us((Right_Speed * (200000 / 1000)));
    } 
    else {
        myServoPB_1.period_ms(20);
        myServoPB_1.pulsewidth_us(((-Right_Speed) * (200000 / 1000)));
        myServoPB_0.period_ms(20);
        myServoPB_0.pulsewidth_us(0);
    }
}
void Speed_Control()
{
    Target_S = 0;
    Error_S=Target_S-(-Speed_L+Speed_R)/2;
    Error_SD=Error_S-Error_SLast;
    Error_SLast=Error_S;
    Error_SI+=Error_S;
    if(Error_SI>500)
        Error_SI=500;
    else if(Error_SI<-500)
        Error_SI=-500;
    Target_A=Ps * Error_S + Is * Error_SI + Ds * Error_SD;
    if(Target_A>10) Target_A=10;
    else if(Target_A<-10) Target_A=-10;   
}
void Angle_Control() {
    Error_A = Target_A - Roll;
    Error_AD = Error_A - Error_ALast;
    Error_ALast = Error_A;
    Error_AI += Error_A;
    if (Error_AI > 500) 
    {
        Error_AI = 500;
    } 
    else if (Error_AI < -500) 
    {
        Error_AI = -500;
    }
    Angle_Out = Pa * Error_A + (Ia * Error_AI - Da * Gyo_x);
    if(Angle_Out>1000)
        Angle_Out=1000;
    else if(Angle_Out<-1000)
        Angle_Out=-1000;
}
void read_speed()
{
    Speed_R = qei_PA_0.getPulses();
    Speed_L = qei_PC_14.getPulses();
    qei_PA_0.reset();
    qei_PC_14.reset();
}

void tick969801_handle() {
    read_speed();
//    Serial_1.printf("%.2f %.2f\n",_p(Speed_L),_p(Speed_R));
    if(Roll>180)
    Roll-=360;
    if(Gyo_x>2000)
    Gyo_x-=4000;
    Roll-=3;
//    Serial_1.printf("%.2f %.2f\n",_p(Roll),_p(Gyo_x));
    Speed_Control();
    Angle_Control();
    Set_Pwm();
}


int main() {
    Serial_1.baud(115200);
    jy901_Serial_3.baud(115200);
    tick969801.attach(&tick969801_handle,0.04);
    init();
    while (true) 
    {
        jy901_Serial_3.receiveData();
        jy901_Serial_3.getAttitude(Roll,Pitch,Yaw);
        jy901_Serial_3.getGyo(Gyo_x,Gyo_y,Gyo_z);
    }
}