![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Quadcopter Attitude Control(Yaw-Pitch-Roll)
main.cpp
- Committer:
- khaledelmadawi
- Date:
- 2015-07-03
- Revision:
- 0:e63996fd7d3e
File content as of revision 0:e63996fd7d3e:
/* this program is to test yaw controlling plane. */ #include "mbed.h" #include "AngularDataAcquizition.h" //#define CascadedPID #include "PIDTheta.h" #include "PID_Alt.h" #include "PID_Yaw.h" #define Listen_time_ms 20 #define ILowerRPM -3.14 #define IUpperRPM 3.14 #define OLowerPWM 0.001 #define OUpperPWM 0.002 #define OLowerPWM_yaw 0.001 #define OUpperPWM_yaw 0.0016 #define ILowerIR 27 #define IUpperIR 125 #define OLowerPower 0.00115 #define OUpperPower 0.0018 #define newILowerIR 27 #define newIUpperIR 125 #define newOLowerPower 0.00135 #define newOUpperPower 0.00145 #define KPgain 0.0000755/*0.000055 /*0.0000759*/ #define KDgain 0.0009/*0.0007 /*0.001106*/ #define KDDgain 0.0014/*0.0014*/ #define Yaw_KP 0.0000755/*00025*/ #define Yaw_KI 0.000 #define Yaw_KD 0.0009/*001*/ #define Yaw_KDD 0.0014/*0011*/ #define Pitch_KP KPgain #define Pitch_KI 0.000 #define Pitch_KD KDgain #define Roll_KP KPgain #define Roll_KI 0.000 #define Roll_KD KDgain #define IR_KP 00.00000005 #define IR_KI 0.00 #define IR_KD 0.000001 #define IR_KDD 0.000000 #define MeanNum 5 #define learningRate 0.00000001 #define _sig_A_Kp 50 #define _sig_A_Kd 100 Timer t; //Serial pc(USBTX, USBRX); Serial pc(p13, p14); DigitalOut myled(LED1); LocalFileSystem local("local"); //////setting the destination of the file that will be saved AngularDataAcquizition Angle(p9, p10); PID_Yaw Yaw_Plane(Yaw_KP,Yaw_KI,Yaw_KD,Yaw_KDD); PIDTheta Pitch_Plan(p21,p23,Listen_time_ms,Pitch_KP,Pitch_KI,Pitch_KD,KDDgain); PIDTheta Roll_Plan(p22,p24,Listen_time_ms,Roll_KP,Roll_KI,Roll_KD,KDDgain); PID_Alt PID_IR(MeanNum,IR_KP,IR_KI,IR_KD,IR_KDD,p20); //prototypes void PWM_change(); void PWM_change2(); void alt_update_Val(); void sendstatus(); float val,val2=0,val_test=0,x_plane,y_plane; char Calibrated; char flag=0; char selected_plane=3; float KD_NEW,KP_NEW; float AngleRoll,AnglePitch,AngleOffset[2],AngleYaw,yawOffset,y_old; Ticker timer,timer2; float acc_PitchPlane,acc_RollPlane,Pitchacc_Req,Rollacc_Req,Req_Pitch,Req_Roll; int main() { KD_NEW=Pitch_KD; KP_NEW=Pitch_KP; //pc.baud(9600); pc.baud(115200); //PID_IR PID_IR.Set_IN_Limits(ILowerIR,IUpperIR); PID_IR.Set_OUT_Limits(OLowerPower,OUpperPower); AngleOffset[0]=0; AngleOffset[1]=0; yawOffset=0; //Yaw_Plan Yaw_Plane.Set_IN_Limits(ILowerRPM,IUpperRPM); Yaw_Plane.Set_OUT_Limits(OLowerPWM_yaw,OUpperPWM_yaw); //Pitch_Plan Pitch_Plan.Set_IN_Limits(ILowerRPM,IUpperRPM); Pitch_Plan.Set_OUT_Limits(OLowerPWM,OUpperPWM); //Roll_Plan Roll_Plan.Set_IN_Limits(ILowerRPM,IUpperRPM); Roll_Plan.Set_OUT_Limits(OLowerPWM,OUpperPWM); //Angle.BeginInterrupt(0.039); acc_PitchPlane=0; acc_RollPlane=0; Pitchacc_Req=0; Rollacc_Req=0; Req_Pitch=0; Req_Roll=0; val=0.00115; Calibrated=0; val2=0; while(1){ while(!pc.readable()); pc.getc(); // timer.attach(alt_update_Val, 0.15); timer2.attach(sendstatus,0.25); t.reset(); t.start(); FILE *fp=fopen("/local/out.xls","a"); while(1) { if(Angle.dmpReady) { PWM_change(); Angle.callMeanFilteredReadings(); //PWM_change2(); //val_test=PID_IR.findNominalVal(val2,cos(AnglePitch),cos(AngleRoll),Calibrated); AngleYaw=Angle.Meanypr[0]-yawOffset; AnglePitch=(Angle.Meanypr[1]-AngleOffset[0]); AngleRoll=(Angle.Meanypr[2]-AngleOffset[1]); if(abs(AngleYaw-y_old)<0.1*M_PI/180)yawOffset=yawOffset+(AngleYaw-y_old); y_old=AngleYaw; /* acc_PitchPlane=sin(AngleRoll); acc_RollPlane=sin(AnglePitch); Req_Pitch=asin(Pitchacc_Req-acc_PitchPlane); Req_Roll=asin(Rollacc_Req-acc_RollPlane); if(Req_Pitch>5)Req_Pitch=10; if(Req_Pitch<-5)Req_Pitch=-10; if(Req_Roll>5)Req_Roll=10; if(Req_Roll<-5)Req_Roll=-10;*/ Yaw_Plane.PWM_cal(0,AngleYaw,val,&x_plane,&y_plane); if(selected_plane==1||selected_plane==3){ Pitch_Plan.PWM_cal(0*3.14/180/*0*/,-1*AnglePitch,x_plane/*val*/); } if(selected_plane==2||selected_plane==3){ Roll_Plan.PWM_cal(0,1*AngleRoll,y_plane/*val*/); } //fprintf(fp,"%f \t %f \t %f \t\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,AngleYaw*180/3.14); //pc.printf("A1:%f A2:%f OP:%f OP2:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,val,val_test); //pc.printf("A1:%f A2:%f OP:%f RAlt:%f OP_test:%f alt:%f t:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,val,val2,val_test,PID_IR.MeanIR(3),t.read()); // fprintf(fp,"%f \t %f \t %f \t %f \t %f \t",AngleRoll,Req_Roll,AnglePitch,Req_Pitch,t.read()); /* fprintf(fp,"%f \t %f \t %f \t %f \t %f \t %f \t %f \t %f \t %f \t %f \t %f \t %f\r\n",(AnglePitch)*180/3.14,Pitch_Plan.readErrorVal(),Pitch_Plan.readErrorDVal(),Pitch_Plan.readGradientFunction(),Pitch_Plan.readSigmoindFunVal() ,(AngleRoll)*180/3.14,Roll_Plan.readErrorVal(),Roll_Plan.readErrorDVal(),Roll_Plan.readGradientFunction(),Roll_Plan.readSigmoindFunVal() ,val,t.read()); */ } if(flag){ flag=0; Pitch_Plan.PWM_cal(0,0,0.001); Roll_Plan.PWM_cal(0,0,0.001); Pitch_Plan.PWM_cal(0,0,0.001); Roll_Plan.PWM_cal(0,0,0.001); //timer.detach(); val2=0; break;} } fclose(fp); } } void PWM_change() { if(pc.readable()) { switch (pc.getc()) { case 'u': val += 0.00005; val2 += 10; break; case 'd': val -= 0.00005; val2 -= 5; break; case 'w': val += 0.00001; break; case 'x': val -= 0.00001; break; case 'e': val+= 0.000005; break; case 'f': val-=0.000005; break; case '*': val = 0.002; break; case 'm': val =0.001; break; case 's': val = 0.001; flag=1; break; case'1': KD_NEW+=0.1; // Pitch_Plan.ChangeKDval(KD_NEW); // Roll_Plan.ChangeKDval(KD_NEW); break; case'2': KD_NEW-=0.1; // Pitch_Plan.ChangeKDval(KD_NEW); // Roll_Plan.ChangeKDval(KD_NEW); break; case'4': KP_NEW+=0.001; // Pitch_Plan.ChangeKDval(KD_NEW); // Roll_Plan.ChangeKDval(KD_NEW); break; case'5': KP_NEW-=0.001; // Pitch_Plan.ChangeKPval(KD_NEW); // Roll_Plan.ChangeKPval(KD_NEW); break; case 'c': AngleOffset[0]=Angle.Meanypr[1]; AngleOffset[1]=Angle.Meanypr[2]; yawOffset=Angle.Meanypr[0]; Calibrated=1; break; /*case '1': case '2': case '4': case '5': */case '7': selected_plane=1; val = 0.001; break; case '8': selected_plane=2; val = 0.001; break; case '9': selected_plane=3; val = 0.001; break; case 'l': PID_IR.Set_IN_Limits(newILowerIR,newIUpperIR); PID_IR.Set_OUT_Limits(newOLowerPower,newOUpperPower); break; case 'p': PID_IR.Set_IN_Limits(ILowerIR,IUpperIR); PID_IR.Set_OUT_Limits(OLowerPower,OUpperPower); break; } // pc.printf("char:%c",pc.getc()); } } void PWM_change2(){//distance in cm if(pc.readable()) { switch (pc.getc()) { case 'u': val2 += 5; break; case 'd': val2-=5; break; case 's': val2=0; flag=1; break; case 'c': AngleOffset[0]=Angle.Meanypr[1]; AngleOffset[1]=Angle.Meanypr[2]; yawOffset=Angle.Meanypr[0]; Calibrated=1; break; case 'm': PID_IR.Set_IN_Limits(newILowerIR,newIUpperIR); PID_IR.Set_OUT_Limits(newOLowerPower,newOUpperPower); break; case 'p': PID_IR.Set_IN_Limits(ILowerIR,IUpperIR); PID_IR.Set_OUT_Limits(OLowerPower,OUpperPower); break; } } } void alt_update_Val(){ val_test=PID_IR.findNominalVal(val2,cos(AnglePitch),cos(AngleRoll),Calibrated); //val=PID_IR.findNominalVal(val2,cos(AnglePitch),cos(AngleRoll),Calibrated); //pc.printf("here\r\n"); } void sendstatus(){ //pc.printf("A1:%f A2:%f OP:%f OP2:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,val,val_test); //pc.printf("A1:%f A2:%f OP:%f RAlt:%f OP_test:%f alt:%f t:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,val,val2,val_test,PID_IR.MeanIR(3),t.read()); pc.printf("A1:%f A2:%f A3:%f OP:%f RA:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,AngleYaw*180/3.14,val,val2); }