quadcopter cufe
Quadcopter Attitude Control(Yaw-Pitch-Roll)
Diff: main.cpp
- Revision:
- 0:e63996fd7d3e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Jul 03 11:16:02 2015 +0000
@@ -0,0 +1,321 @@
+/*
+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);
+ }
\ No newline at end of file