quadcopter cufe / Mbed 2 deprecated 2006_Theta_Control_yaw

Quadcopter Attitude Control(Yaw-Pitch-Roll)

Dependencies:   mbed

main.cpp@0:e63996fd7d3e, 2015-07-03 (annotated)

Committer:
khaledelmadawi
Date:
Fri Jul 03 11:16:02 2015 +0000
Revision:
0:e63996fd7d3e
Quadcopter Attitude Control(Yaw-Pitch-Roll)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaledelmadawi 0:e63996fd7d3e 1 /*
khaledelmadawi 0:e63996fd7d3e 2 this program is to test yaw controlling plane.
khaledelmadawi 0:e63996fd7d3e 3 */
khaledelmadawi 0:e63996fd7d3e 4
khaledelmadawi 0:e63996fd7d3e 5 #include "mbed.h"
khaledelmadawi 0:e63996fd7d3e 6 #include "AngularDataAcquizition.h"
khaledelmadawi 0:e63996fd7d3e 7 //#define CascadedPID
khaledelmadawi 0:e63996fd7d3e 8 #include "PIDTheta.h"
khaledelmadawi 0:e63996fd7d3e 9 #include "PID_Alt.h"
khaledelmadawi 0:e63996fd7d3e 10 #include "PID_Yaw.h"
khaledelmadawi 0:e63996fd7d3e 11
khaledelmadawi 0:e63996fd7d3e 12 #define Listen_time_ms 20
khaledelmadawi 0:e63996fd7d3e 13 #define ILowerRPM -3.14
khaledelmadawi 0:e63996fd7d3e 14 #define IUpperRPM 3.14
khaledelmadawi 0:e63996fd7d3e 15 #define OLowerPWM 0.001
khaledelmadawi 0:e63996fd7d3e 16 #define OUpperPWM 0.002
khaledelmadawi 0:e63996fd7d3e 17 #define OLowerPWM_yaw 0.001
khaledelmadawi 0:e63996fd7d3e 18 #define OUpperPWM_yaw 0.0016
khaledelmadawi 0:e63996fd7d3e 19
khaledelmadawi 0:e63996fd7d3e 20
khaledelmadawi 0:e63996fd7d3e 21 #define ILowerIR 27
khaledelmadawi 0:e63996fd7d3e 22 #define IUpperIR 125
khaledelmadawi 0:e63996fd7d3e 23 #define OLowerPower 0.00115
khaledelmadawi 0:e63996fd7d3e 24 #define OUpperPower 0.0018
khaledelmadawi 0:e63996fd7d3e 25
khaledelmadawi 0:e63996fd7d3e 26 #define newILowerIR 27
khaledelmadawi 0:e63996fd7d3e 27 #define newIUpperIR 125
khaledelmadawi 0:e63996fd7d3e 28 #define newOLowerPower 0.00135
khaledelmadawi 0:e63996fd7d3e 29 #define newOUpperPower 0.00145
khaledelmadawi 0:e63996fd7d3e 30
khaledelmadawi 0:e63996fd7d3e 31
khaledelmadawi 0:e63996fd7d3e 32 #define KPgain 0.0000755/*0.000055 /*0.0000759*/
khaledelmadawi 0:e63996fd7d3e 33 #define KDgain 0.0009/*0.0007 /*0.001106*/
khaledelmadawi 0:e63996fd7d3e 34 #define KDDgain 0.0014/*0.0014*/
khaledelmadawi 0:e63996fd7d3e 35
khaledelmadawi 0:e63996fd7d3e 36 #define Yaw_KP 0.0000755/*00025*/
khaledelmadawi 0:e63996fd7d3e 37 #define Yaw_KI 0.000
khaledelmadawi 0:e63996fd7d3e 38 #define Yaw_KD 0.0009/*001*/
khaledelmadawi 0:e63996fd7d3e 39 #define Yaw_KDD 0.0014/*0011*/
khaledelmadawi 0:e63996fd7d3e 40
khaledelmadawi 0:e63996fd7d3e 41
khaledelmadawi 0:e63996fd7d3e 42 #define Pitch_KP KPgain
khaledelmadawi 0:e63996fd7d3e 43 #define Pitch_KI 0.000
khaledelmadawi 0:e63996fd7d3e 44 #define Pitch_KD KDgain
khaledelmadawi 0:e63996fd7d3e 45
khaledelmadawi 0:e63996fd7d3e 46 #define Roll_KP KPgain
khaledelmadawi 0:e63996fd7d3e 47 #define Roll_KI 0.000
khaledelmadawi 0:e63996fd7d3e 48 #define Roll_KD KDgain
khaledelmadawi 0:e63996fd7d3e 49
khaledelmadawi 0:e63996fd7d3e 50 #define IR_KP 00.00000005
khaledelmadawi 0:e63996fd7d3e 51 #define IR_KI 0.00
khaledelmadawi 0:e63996fd7d3e 52 #define IR_KD 0.000001
khaledelmadawi 0:e63996fd7d3e 53 #define IR_KDD 0.000000
khaledelmadawi 0:e63996fd7d3e 54
khaledelmadawi 0:e63996fd7d3e 55 #define MeanNum 5
khaledelmadawi 0:e63996fd7d3e 56 #define learningRate 0.00000001
khaledelmadawi 0:e63996fd7d3e 57
khaledelmadawi 0:e63996fd7d3e 58 #define _sig_A_Kp 50
khaledelmadawi 0:e63996fd7d3e 59 #define _sig_A_Kd 100
khaledelmadawi 0:e63996fd7d3e 60 Timer t;
khaledelmadawi 0:e63996fd7d3e 61 //Serial pc(USBTX, USBRX);
khaledelmadawi 0:e63996fd7d3e 62 Serial pc(p13, p14);
khaledelmadawi 0:e63996fd7d3e 63 DigitalOut myled(LED1);
khaledelmadawi 0:e63996fd7d3e 64 LocalFileSystem local("local"); //////setting the destination of the file that will be saved
khaledelmadawi 0:e63996fd7d3e 65
khaledelmadawi 0:e63996fd7d3e 66 AngularDataAcquizition Angle(p9, p10);
khaledelmadawi 0:e63996fd7d3e 67 PID_Yaw Yaw_Plane(Yaw_KP,Yaw_KI,Yaw_KD,Yaw_KDD);
khaledelmadawi 0:e63996fd7d3e 68 PIDTheta Pitch_Plan(p21,p23,Listen_time_ms,Pitch_KP,Pitch_KI,Pitch_KD,KDDgain);
khaledelmadawi 0:e63996fd7d3e 69 PIDTheta Roll_Plan(p22,p24,Listen_time_ms,Roll_KP,Roll_KI,Roll_KD,KDDgain);
khaledelmadawi 0:e63996fd7d3e 70 PID_Alt PID_IR(MeanNum,IR_KP,IR_KI,IR_KD,IR_KDD,p20);
khaledelmadawi 0:e63996fd7d3e 71
khaledelmadawi 0:e63996fd7d3e 72 //prototypes
khaledelmadawi 0:e63996fd7d3e 73 void PWM_change();
khaledelmadawi 0:e63996fd7d3e 74 void PWM_change2();
khaledelmadawi 0:e63996fd7d3e 75 void alt_update_Val();
khaledelmadawi 0:e63996fd7d3e 76 void sendstatus();
khaledelmadawi 0:e63996fd7d3e 77 float val,val2=0,val_test=0,x_plane,y_plane;
khaledelmadawi 0:e63996fd7d3e 78 char Calibrated;
khaledelmadawi 0:e63996fd7d3e 79 char flag=0;
khaledelmadawi 0:e63996fd7d3e 80 char selected_plane=3;
khaledelmadawi 0:e63996fd7d3e 81 float KD_NEW,KP_NEW;
khaledelmadawi 0:e63996fd7d3e 82 float AngleRoll,AnglePitch,AngleOffset[2],AngleYaw,yawOffset,y_old;
khaledelmadawi 0:e63996fd7d3e 83 Ticker timer,timer2;
khaledelmadawi 0:e63996fd7d3e 84 float acc_PitchPlane,acc_RollPlane,Pitchacc_Req,Rollacc_Req,Req_Pitch,Req_Roll;
khaledelmadawi 0:e63996fd7d3e 85
khaledelmadawi 0:e63996fd7d3e 86 int main() {
khaledelmadawi 0:e63996fd7d3e 87 KD_NEW=Pitch_KD;
khaledelmadawi 0:e63996fd7d3e 88 KP_NEW=Pitch_KP;
khaledelmadawi 0:e63996fd7d3e 89 //pc.baud(9600);
khaledelmadawi 0:e63996fd7d3e 90 pc.baud(115200);
khaledelmadawi 0:e63996fd7d3e 91 //PID_IR
khaledelmadawi 0:e63996fd7d3e 92 PID_IR.Set_IN_Limits(ILowerIR,IUpperIR);
khaledelmadawi 0:e63996fd7d3e 93 PID_IR.Set_OUT_Limits(OLowerPower,OUpperPower);
khaledelmadawi 0:e63996fd7d3e 94
khaledelmadawi 0:e63996fd7d3e 95 AngleOffset[0]=0;
khaledelmadawi 0:e63996fd7d3e 96 AngleOffset[1]=0;
khaledelmadawi 0:e63996fd7d3e 97 yawOffset=0;
khaledelmadawi 0:e63996fd7d3e 98
khaledelmadawi 0:e63996fd7d3e 99 //Yaw_Plan
khaledelmadawi 0:e63996fd7d3e 100 Yaw_Plane.Set_IN_Limits(ILowerRPM,IUpperRPM);
khaledelmadawi 0:e63996fd7d3e 101 Yaw_Plane.Set_OUT_Limits(OLowerPWM_yaw,OUpperPWM_yaw);
khaledelmadawi 0:e63996fd7d3e 102 //Pitch_Plan
khaledelmadawi 0:e63996fd7d3e 103 Pitch_Plan.Set_IN_Limits(ILowerRPM,IUpperRPM);
khaledelmadawi 0:e63996fd7d3e 104 Pitch_Plan.Set_OUT_Limits(OLowerPWM,OUpperPWM);
khaledelmadawi 0:e63996fd7d3e 105 //Roll_Plan
khaledelmadawi 0:e63996fd7d3e 106 Roll_Plan.Set_IN_Limits(ILowerRPM,IUpperRPM);
khaledelmadawi 0:e63996fd7d3e 107 Roll_Plan.Set_OUT_Limits(OLowerPWM,OUpperPWM);
khaledelmadawi 0:e63996fd7d3e 108 //Angle.BeginInterrupt(0.039);
khaledelmadawi 0:e63996fd7d3e 109 acc_PitchPlane=0;
khaledelmadawi 0:e63996fd7d3e 110 acc_RollPlane=0;
khaledelmadawi 0:e63996fd7d3e 111 Pitchacc_Req=0;
khaledelmadawi 0:e63996fd7d3e 112 Rollacc_Req=0;
khaledelmadawi 0:e63996fd7d3e 113 Req_Pitch=0;
khaledelmadawi 0:e63996fd7d3e 114 Req_Roll=0;
khaledelmadawi 0:e63996fd7d3e 115 val=0.00115;
khaledelmadawi 0:e63996fd7d3e 116 Calibrated=0;
khaledelmadawi 0:e63996fd7d3e 117 val2=0;
khaledelmadawi 0:e63996fd7d3e 118 while(1){
khaledelmadawi 0:e63996fd7d3e 119 while(!pc.readable());
khaledelmadawi 0:e63996fd7d3e 120 pc.getc();
khaledelmadawi 0:e63996fd7d3e 121 // timer.attach(alt_update_Val, 0.15);
khaledelmadawi 0:e63996fd7d3e 122 timer2.attach(sendstatus,0.25);
khaledelmadawi 0:e63996fd7d3e 123
khaledelmadawi 0:e63996fd7d3e 124 t.reset();
khaledelmadawi 0:e63996fd7d3e 125 t.start();
khaledelmadawi 0:e63996fd7d3e 126 FILE *fp=fopen("/local/out.xls","a");
khaledelmadawi 0:e63996fd7d3e 127
khaledelmadawi 0:e63996fd7d3e 128 while(1) {
khaledelmadawi 0:e63996fd7d3e 129 if(Angle.dmpReady)
khaledelmadawi 0:e63996fd7d3e 130 {
khaledelmadawi 0:e63996fd7d3e 131 PWM_change();
khaledelmadawi 0:e63996fd7d3e 132 Angle.callMeanFilteredReadings();
khaledelmadawi 0:e63996fd7d3e 133 //PWM_change2();
khaledelmadawi 0:e63996fd7d3e 134 //val_test=PID_IR.findNominalVal(val2,cos(AnglePitch),cos(AngleRoll),Calibrated);
khaledelmadawi 0:e63996fd7d3e 135
khaledelmadawi 0:e63996fd7d3e 136 AngleYaw=Angle.Meanypr[0]-yawOffset;
khaledelmadawi 0:e63996fd7d3e 137 AnglePitch=(Angle.Meanypr[1]-AngleOffset[0]);
khaledelmadawi 0:e63996fd7d3e 138 AngleRoll=(Angle.Meanypr[2]-AngleOffset[1]);
khaledelmadawi 0:e63996fd7d3e 139 if(abs(AngleYaw-y_old)<0.1*M_PI/180)yawOffset=yawOffset+(AngleYaw-y_old);
khaledelmadawi 0:e63996fd7d3e 140 y_old=AngleYaw;
khaledelmadawi 0:e63996fd7d3e 141 /*
khaledelmadawi 0:e63996fd7d3e 142 acc_PitchPlane=sin(AngleRoll);
khaledelmadawi 0:e63996fd7d3e 143 acc_RollPlane=sin(AnglePitch);
khaledelmadawi 0:e63996fd7d3e 144 Req_Pitch=asin(Pitchacc_Req-acc_PitchPlane);
khaledelmadawi 0:e63996fd7d3e 145 Req_Roll=asin(Rollacc_Req-acc_RollPlane);
khaledelmadawi 0:e63996fd7d3e 146 if(Req_Pitch>5)Req_Pitch=10;
khaledelmadawi 0:e63996fd7d3e 147 if(Req_Pitch<-5)Req_Pitch=-10;
khaledelmadawi 0:e63996fd7d3e 148 if(Req_Roll>5)Req_Roll=10;
khaledelmadawi 0:e63996fd7d3e 149 if(Req_Roll<-5)Req_Roll=-10;*/
khaledelmadawi 0:e63996fd7d3e 150
khaledelmadawi 0:e63996fd7d3e 151 Yaw_Plane.PWM_cal(0,AngleYaw,val,&x_plane,&y_plane);
khaledelmadawi 0:e63996fd7d3e 152
khaledelmadawi 0:e63996fd7d3e 153 if(selected_plane==1||selected_plane==3){
khaledelmadawi 0:e63996fd7d3e 154 Pitch_Plan.PWM_cal(0*3.14/180/*0*/,-1*AnglePitch,x_plane/*val*/);
khaledelmadawi 0:e63996fd7d3e 155 }
khaledelmadawi 0:e63996fd7d3e 156 if(selected_plane==2||selected_plane==3){
khaledelmadawi 0:e63996fd7d3e 157 Roll_Plan.PWM_cal(0,1*AngleRoll,y_plane/*val*/);
khaledelmadawi 0:e63996fd7d3e 158 }
khaledelmadawi 0:e63996fd7d3e 159 //fprintf(fp,"%f \t %f \t %f \t\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,AngleYaw*180/3.14);
khaledelmadawi 0:e63996fd7d3e 160
khaledelmadawi 0:e63996fd7d3e 161 //pc.printf("A1:%f A2:%f OP:%f OP2:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,val,val_test);
khaledelmadawi 0:e63996fd7d3e 162 //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());
khaledelmadawi 0:e63996fd7d3e 163 // fprintf(fp,"%f \t %f \t %f \t %f \t %f \t",AngleRoll,Req_Roll,AnglePitch,Req_Pitch,t.read());
khaledelmadawi 0:e63996fd7d3e 164
khaledelmadawi 0:e63996fd7d3e 165 /* 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()
khaledelmadawi 0:e63996fd7d3e 166 ,(AngleRoll)*180/3.14,Roll_Plan.readErrorVal(),Roll_Plan.readErrorDVal(),Roll_Plan.readGradientFunction(),Roll_Plan.readSigmoindFunVal()
khaledelmadawi 0:e63996fd7d3e 167 ,val,t.read());
khaledelmadawi 0:e63996fd7d3e 168 */ }
khaledelmadawi 0:e63996fd7d3e 169 if(flag){
khaledelmadawi 0:e63996fd7d3e 170 flag=0;
khaledelmadawi 0:e63996fd7d3e 171 Pitch_Plan.PWM_cal(0,0,0.001);
khaledelmadawi 0:e63996fd7d3e 172 Roll_Plan.PWM_cal(0,0,0.001);
khaledelmadawi 0:e63996fd7d3e 173 Pitch_Plan.PWM_cal(0,0,0.001);
khaledelmadawi 0:e63996fd7d3e 174 Roll_Plan.PWM_cal(0,0,0.001);
khaledelmadawi 0:e63996fd7d3e 175 //timer.detach();
khaledelmadawi 0:e63996fd7d3e 176 val2=0;
khaledelmadawi 0:e63996fd7d3e 177 break;}
khaledelmadawi 0:e63996fd7d3e 178 }
khaledelmadawi 0:e63996fd7d3e 179 fclose(fp);
khaledelmadawi 0:e63996fd7d3e 180
khaledelmadawi 0:e63996fd7d3e 181 }
khaledelmadawi 0:e63996fd7d3e 182 }
khaledelmadawi 0:e63996fd7d3e 183 void PWM_change()
khaledelmadawi 0:e63996fd7d3e 184 {
khaledelmadawi 0:e63996fd7d3e 185 if(pc.readable()) {
khaledelmadawi 0:e63996fd7d3e 186 switch (pc.getc()) {
khaledelmadawi 0:e63996fd7d3e 187 case 'u':
khaledelmadawi 0:e63996fd7d3e 188 val += 0.00005;
khaledelmadawi 0:e63996fd7d3e 189 val2 += 10;
khaledelmadawi 0:e63996fd7d3e 190 break;
khaledelmadawi 0:e63996fd7d3e 191 case 'd':
khaledelmadawi 0:e63996fd7d3e 192 val -= 0.00005;
khaledelmadawi 0:e63996fd7d3e 193 val2 -= 5;
khaledelmadawi 0:e63996fd7d3e 194 break;
khaledelmadawi 0:e63996fd7d3e 195 case 'w':
khaledelmadawi 0:e63996fd7d3e 196 val += 0.00001;
khaledelmadawi 0:e63996fd7d3e 197 break;
khaledelmadawi 0:e63996fd7d3e 198 case 'x':
khaledelmadawi 0:e63996fd7d3e 199 val -= 0.00001;
khaledelmadawi 0:e63996fd7d3e 200 break;
khaledelmadawi 0:e63996fd7d3e 201 case 'e':
khaledelmadawi 0:e63996fd7d3e 202 val+= 0.000005;
khaledelmadawi 0:e63996fd7d3e 203 break;
khaledelmadawi 0:e63996fd7d3e 204 case 'f':
khaledelmadawi 0:e63996fd7d3e 205 val-=0.000005;
khaledelmadawi 0:e63996fd7d3e 206 break;
khaledelmadawi 0:e63996fd7d3e 207 case '*':
khaledelmadawi 0:e63996fd7d3e 208 val = 0.002;
khaledelmadawi 0:e63996fd7d3e 209 break;
khaledelmadawi 0:e63996fd7d3e 210 case 'm':
khaledelmadawi 0:e63996fd7d3e 211 val =0.001;
khaledelmadawi 0:e63996fd7d3e 212 break;
khaledelmadawi 0:e63996fd7d3e 213 case 's':
khaledelmadawi 0:e63996fd7d3e 214 val = 0.001;
khaledelmadawi 0:e63996fd7d3e 215 flag=1;
khaledelmadawi 0:e63996fd7d3e 216 break;
khaledelmadawi 0:e63996fd7d3e 217 case'1':
khaledelmadawi 0:e63996fd7d3e 218 KD_NEW+=0.1;
khaledelmadawi 0:e63996fd7d3e 219 // Pitch_Plan.ChangeKDval(KD_NEW);
khaledelmadawi 0:e63996fd7d3e 220 // Roll_Plan.ChangeKDval(KD_NEW);
khaledelmadawi 0:e63996fd7d3e 221 break;
khaledelmadawi 0:e63996fd7d3e 222 case'2':
khaledelmadawi 0:e63996fd7d3e 223 KD_NEW-=0.1;
khaledelmadawi 0:e63996fd7d3e 224 // Pitch_Plan.ChangeKDval(KD_NEW);
khaledelmadawi 0:e63996fd7d3e 225 // Roll_Plan.ChangeKDval(KD_NEW);
khaledelmadawi 0:e63996fd7d3e 226 break;
khaledelmadawi 0:e63996fd7d3e 227 case'4':
khaledelmadawi 0:e63996fd7d3e 228 KP_NEW+=0.001;
khaledelmadawi 0:e63996fd7d3e 229 // Pitch_Plan.ChangeKDval(KD_NEW);
khaledelmadawi 0:e63996fd7d3e 230 // Roll_Plan.ChangeKDval(KD_NEW);
khaledelmadawi 0:e63996fd7d3e 231 break;
khaledelmadawi 0:e63996fd7d3e 232 case'5':
khaledelmadawi 0:e63996fd7d3e 233 KP_NEW-=0.001;
khaledelmadawi 0:e63996fd7d3e 234 // Pitch_Plan.ChangeKPval(KD_NEW);
khaledelmadawi 0:e63996fd7d3e 235 // Roll_Plan.ChangeKPval(KD_NEW);
khaledelmadawi 0:e63996fd7d3e 236 break;
khaledelmadawi 0:e63996fd7d3e 237 case 'c':
khaledelmadawi 0:e63996fd7d3e 238 AngleOffset[0]=Angle.Meanypr[1];
khaledelmadawi 0:e63996fd7d3e 239 AngleOffset[1]=Angle.Meanypr[2];
khaledelmadawi 0:e63996fd7d3e 240 yawOffset=Angle.Meanypr[0];
khaledelmadawi 0:e63996fd7d3e 241
khaledelmadawi 0:e63996fd7d3e 242 Calibrated=1;
khaledelmadawi 0:e63996fd7d3e 243
khaledelmadawi 0:e63996fd7d3e 244 break;
khaledelmadawi 0:e63996fd7d3e 245 /*case '1':
khaledelmadawi 0:e63996fd7d3e 246 case '2':
khaledelmadawi 0:e63996fd7d3e 247 case '4':
khaledelmadawi 0:e63996fd7d3e 248 case '5':
khaledelmadawi 0:e63996fd7d3e 249 */case '7':
khaledelmadawi 0:e63996fd7d3e 250 selected_plane=1;
khaledelmadawi 0:e63996fd7d3e 251 val = 0.001;
khaledelmadawi 0:e63996fd7d3e 252 break;
khaledelmadawi 0:e63996fd7d3e 253 case '8':
khaledelmadawi 0:e63996fd7d3e 254 selected_plane=2;
khaledelmadawi 0:e63996fd7d3e 255 val = 0.001;
khaledelmadawi 0:e63996fd7d3e 256 break;
khaledelmadawi 0:e63996fd7d3e 257 case '9':
khaledelmadawi 0:e63996fd7d3e 258 selected_plane=3;
khaledelmadawi 0:e63996fd7d3e 259 val = 0.001;
khaledelmadawi 0:e63996fd7d3e 260 break;
khaledelmadawi 0:e63996fd7d3e 261
khaledelmadawi 0:e63996fd7d3e 262 case 'l':
khaledelmadawi 0:e63996fd7d3e 263 PID_IR.Set_IN_Limits(newILowerIR,newIUpperIR);
khaledelmadawi 0:e63996fd7d3e 264 PID_IR.Set_OUT_Limits(newOLowerPower,newOUpperPower);
khaledelmadawi 0:e63996fd7d3e 265 break;
khaledelmadawi 0:e63996fd7d3e 266 case 'p':
khaledelmadawi 0:e63996fd7d3e 267 PID_IR.Set_IN_Limits(ILowerIR,IUpperIR);
khaledelmadawi 0:e63996fd7d3e 268 PID_IR.Set_OUT_Limits(OLowerPower,OUpperPower);
khaledelmadawi 0:e63996fd7d3e 269 break;
khaledelmadawi 0:e63996fd7d3e 270
khaledelmadawi 0:e63996fd7d3e 271 }
khaledelmadawi 0:e63996fd7d3e 272 // pc.printf("char:%c",pc.getc());
khaledelmadawi 0:e63996fd7d3e 273
khaledelmadawi 0:e63996fd7d3e 274 }
khaledelmadawi 0:e63996fd7d3e 275
khaledelmadawi 0:e63996fd7d3e 276 }
khaledelmadawi 0:e63996fd7d3e 277
khaledelmadawi 0:e63996fd7d3e 278
khaledelmadawi 0:e63996fd7d3e 279 void PWM_change2(){//distance in cm
khaledelmadawi 0:e63996fd7d3e 280 if(pc.readable()) {
khaledelmadawi 0:e63996fd7d3e 281 switch (pc.getc()) {
khaledelmadawi 0:e63996fd7d3e 282 case 'u':
khaledelmadawi 0:e63996fd7d3e 283 val2 += 5;
khaledelmadawi 0:e63996fd7d3e 284 break;
khaledelmadawi 0:e63996fd7d3e 285 case 'd':
khaledelmadawi 0:e63996fd7d3e 286 val2-=5;
khaledelmadawi 0:e63996fd7d3e 287 break;
khaledelmadawi 0:e63996fd7d3e 288 case 's':
khaledelmadawi 0:e63996fd7d3e 289 val2=0;
khaledelmadawi 0:e63996fd7d3e 290 flag=1;
khaledelmadawi 0:e63996fd7d3e 291 break;
khaledelmadawi 0:e63996fd7d3e 292 case 'c':
khaledelmadawi 0:e63996fd7d3e 293 AngleOffset[0]=Angle.Meanypr[1];
khaledelmadawi 0:e63996fd7d3e 294 AngleOffset[1]=Angle.Meanypr[2];
khaledelmadawi 0:e63996fd7d3e 295 yawOffset=Angle.Meanypr[0];
khaledelmadawi 0:e63996fd7d3e 296 Calibrated=1;
khaledelmadawi 0:e63996fd7d3e 297 break;
khaledelmadawi 0:e63996fd7d3e 298 case 'm':
khaledelmadawi 0:e63996fd7d3e 299 PID_IR.Set_IN_Limits(newILowerIR,newIUpperIR);
khaledelmadawi 0:e63996fd7d3e 300 PID_IR.Set_OUT_Limits(newOLowerPower,newOUpperPower);
khaledelmadawi 0:e63996fd7d3e 301 break;
khaledelmadawi 0:e63996fd7d3e 302 case 'p':
khaledelmadawi 0:e63996fd7d3e 303 PID_IR.Set_IN_Limits(ILowerIR,IUpperIR);
khaledelmadawi 0:e63996fd7d3e 304 PID_IR.Set_OUT_Limits(OLowerPower,OUpperPower);
khaledelmadawi 0:e63996fd7d3e 305 break;
khaledelmadawi 0:e63996fd7d3e 306
khaledelmadawi 0:e63996fd7d3e 307 }
khaledelmadawi 0:e63996fd7d3e 308 }
khaledelmadawi 0:e63996fd7d3e 309 }
khaledelmadawi 0:e63996fd7d3e 310 void alt_update_Val(){
khaledelmadawi 0:e63996fd7d3e 311 val_test=PID_IR.findNominalVal(val2,cos(AnglePitch),cos(AngleRoll),Calibrated);
khaledelmadawi 0:e63996fd7d3e 312 //val=PID_IR.findNominalVal(val2,cos(AnglePitch),cos(AngleRoll),Calibrated);
khaledelmadawi 0:e63996fd7d3e 313
khaledelmadawi 0:e63996fd7d3e 314 //pc.printf("here\r\n");
khaledelmadawi 0:e63996fd7d3e 315
khaledelmadawi 0:e63996fd7d3e 316 }
khaledelmadawi 0:e63996fd7d3e 317 void sendstatus(){
khaledelmadawi 0:e63996fd7d3e 318 //pc.printf("A1:%f A2:%f OP:%f OP2:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,val,val_test);
khaledelmadawi 0:e63996fd7d3e 319 //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());
khaledelmadawi 0:e63996fd7d3e 320 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);
khaledelmadawi 0:e63996fd7d3e 321 }