Dominik Diedrich / Mbed 2 deprecated drone_quado_x

code to fly a quadrocopter

Dependencies:   mbed

PID/PID_roll.cpp@0:b0f9c5ac0305, 2020-05-05 (annotated)

Committer:
DD1993
Date:
Tue May 05 21:11:38 2020 +0000
Revision:
0:b0f9c5ac0305
initial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DD1993 0:b0f9c5ac0305 1 #include "PID_roll.h"
DD1993 0:b0f9c5ac0305 2
DD1993 0:b0f9c5ac0305 3 RawSerial pc_pid_roll(USBTX, USBRX);
DD1993 0:b0f9c5ac0305 4
DD1993 0:b0f9c5ac0305 5 PID_ROLL_Class::PID_ROLL_Class(float AngleKp,float AngleKi, float RateKp, float RateKd){
DD1993 0:b0f9c5ac0305 6
DD1993 0:b0f9c5ac0305 7 Angle_Kp=AngleKp;
DD1993 0:b0f9c5ac0305 8 Angle_Ki=AngleKi;
DD1993 0:b0f9c5ac0305 9 Rate_Kp=RateKp;
DD1993 0:b0f9c5ac0305 10 Rate_Kd=RateKd;
DD1993 0:b0f9c5ac0305 11 }
DD1993 0:b0f9c5ac0305 12
DD1993 0:b0f9c5ac0305 13 float PID_ROLL_Class::update_roll(float Setpoint, float CurrentPosition, float Rate, float dt){
DD1993 0:b0f9c5ac0305 14
DD1993 0:b0f9c5ac0305 15 AngleError = 0;
DD1993 0:b0f9c5ac0305 16 AngleError = Setpoint - CurrentPosition;
DD1993 0:b0f9c5ac0305 17 //pc_pid.printf("Setpoint: %f| CurrentPosition: %f| angle: %f\n", Setpoint, CurrentPosition, AngleError);
DD1993 0:b0f9c5ac0305 18
DD1993 0:b0f9c5ac0305 19 Angle_I +=AngleError*dt;
DD1993 0:b0f9c5ac0305 20
DD1993 0:b0f9c5ac0305 21 if(Angle_I>100.0f) Angle_I=100.0f;
DD1993 0:b0f9c5ac0305 22 else if(Angle_I<-100.0f) Angle_I= -100.0f;
DD1993 0:b0f9c5ac0305 23
DD1993 0:b0f9c5ac0305 24 RateError = (AngleError*Angle_Kp)- Rate;
DD1993 0:b0f9c5ac0305 25
DD1993 0:b0f9c5ac0305 26 Rate_D = (RateError-PreviousError)/dt;
DD1993 0:b0f9c5ac0305 27
DD1993 0:b0f9c5ac0305 28 Output = (float)((RateError*Rate_Kp) + (Angle_I*Angle_Ki) + (Rate_D*Rate_Kd));
DD1993 0:b0f9c5ac0305 29
DD1993 0:b0f9c5ac0305 30 PreviousError = RateError;
DD1993 0:b0f9c5ac0305 31
DD1993 0:b0f9c5ac0305 32 return Output;
DD1993 0:b0f9c5ac0305 33 }