Arnoud Meutstege / Mbed 2 deprecated Conceptcontroller_v_1_0

first publish not working

Dependencies:   MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter

main.cpp@2:2563d1d8461f, 2017-10-27 (annotated)

Committer:
Arnoud113
Date:
Fri Oct 27 09:29:18 2017 +0000
Revision:
2:2563d1d8461f
Parent:
1:13d8940f0fd4
Child:
3:b353ee86230a
second try publish;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arnoud113 0:77ad62c61c78 1 #include "mbed.h"
Arnoud113 0:77ad62c61c78 2 #include "QEI.h"
Arnoud113 0:77ad62c61c78 3 #include "MODSERIAL.h"
Arnoud113 0:77ad62c61c78 4 #include "math.h"
Arnoud113 2:2563d1d8461f 5 #include "FastPWM.h"
Arnoud113 0:77ad62c61c78 6
Arnoud113 0:77ad62c61c78 7
Arnoud113 0:77ad62c61c78 8
Arnoud113 0:77ad62c61c78 9 DigitalOut gpo(D0);
Arnoud113 0:77ad62c61c78 10 DigitalOut ledb(LED_BLUE);
Arnoud113 0:77ad62c61c78 11 DigitalOut ledr(LED_RED);
Arnoud113 0:77ad62c61c78 12 DigitalOut ledg(LED_GREEN);
Arnoud113 0:77ad62c61c78 13 DigitalOut motor1DC(D7);
Arnoud113 2:2563d1d8461f 14 FastPWM motor1PWM(D6);
Arnoud113 0:77ad62c61c78 15 DigitalOut motor2DC(D4);
Arnoud113 2:2563d1d8461f 16 FastPWM motor2PWM(D5);
Arnoud113 0:77ad62c61c78 17
Arnoud113 0:77ad62c61c78 18 AnalogIn potMeter1(A0);
Arnoud113 0:77ad62c61c78 19 AnalogIn potMeter2(A1);
Arnoud113 0:77ad62c61c78 20 DigitalIn button1(D11);
Arnoud113 0:77ad62c61c78 21 DigitalIn button2(D12);
Arnoud113 0:77ad62c61c78 22 QEI Encoder1(D12,D13,NC,4200);
Arnoud113 0:77ad62c61c78 23 QEI Encoder2(D9,D8,NC,4200);
Arnoud113 0:77ad62c61c78 24
Arnoud113 0:77ad62c61c78 25 MODSERIAL pc(USBTX,USBRX);
Arnoud113 0:77ad62c61c78 26
Arnoud113 0:77ad62c61c78 27 Ticker controller;
Arnoud113 0:77ad62c61c78 28
Arnoud113 0:77ad62c61c78 29 int potmultiplier = 8000; // Multiplier for the pot meter reference which is normally between 0 and 1
Arnoud113 0:77ad62c61c78 30
Arnoud113 2:2563d1d8461f 31 int frequency_pwm = 1000;
Arnoud113 2:2563d1d8461f 32
Arnoud113 0:77ad62c61c78 33 //Start constants for the PID -------------------------------
Arnoud113 0:77ad62c61c78 34 const double pi = 3.1415926535897;
Arnoud113 2:2563d1d8461f 35 const double M1_TS = 1/100; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
Arnoud113 0:77ad62c61c78 36
Arnoud113 0:77ad62c61c78 37 //verplaatst
Arnoud113 0:77ad62c61c78 38 const float RAD_PER_PULSE = (2*pi)/4200;
Arnoud113 0:77ad62c61c78 39
Arnoud113 0:77ad62c61c78 40 const float M1_KP = 10, M1_KI = 0.5, M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
Arnoud113 0:77ad62c61c78 41 double m1_err_int = 0, m1_prev_err = 0 ;
Arnoud113 0:77ad62c61c78 42 const double M1_F_A1 = 1.0 , M1_F_A2 = 2.0 , M1_F_B0 = 1.0 , M1_F_B1 = 3.0 , M1_F_B2 = 4.0 ;
Arnoud113 0:77ad62c61c78 43 double m1_f_v1 = 0 , m1_f_v2 = 0 ;
Arnoud113 0:77ad62c61c78 44 //End of constant for the PID
Arnoud113 0:77ad62c61c78 45
Arnoud113 1:13d8940f0fd4 46 //Get reference position in different way-----------------START
Arnoud113 0:77ad62c61c78 47 float Get_X_Position(){
Arnoud113 1:13d8940f0fd4 48 double X = potMeter1 * potmultiplier;
Arnoud113 1:13d8940f0fd4 49 return X;
Arnoud113 0:77ad62c61c78 50 }
Arnoud113 0:77ad62c61c78 51
Arnoud113 0:77ad62c61c78 52 float Get_Y_Position(){
Arnoud113 1:13d8940f0fd4 53 double Y = potMeter2 * potmultiplier;
Arnoud113 1:13d8940f0fd4 54 return Y;
Arnoud113 0:77ad62c61c78 55 }
Arnoud113 1:13d8940f0fd4 56 //-----------------------------------------------------------END
Arnoud113 0:77ad62c61c78 57
Arnoud113 0:77ad62c61c78 58 //Start PID part ------------------------------------------START
Arnoud113 0:77ad62c61c78 59 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
Arnoud113 0:77ad62c61c78 60
Arnoud113 0:77ad62c61c78 61 // Derivative
Arnoud113 0:77ad62c61c78 62 double e_der = (e - e_prev)/Ts; // Ts = motor1-timestep
Arnoud113 0:77ad62c61c78 63
Arnoud113 0:77ad62c61c78 64 // biquad part, see slide
Arnoud113 0:77ad62c61c78 65 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
Arnoud113 0:77ad62c61c78 66
Arnoud113 0:77ad62c61c78 67 e_prev = e;
Arnoud113 0:77ad62c61c78 68
Arnoud113 0:77ad62c61c78 69 // Integral
Arnoud113 0:77ad62c61c78 70 e_int += Ts*e;
Arnoud113 0:77ad62c61c78 71
Arnoud113 0:77ad62c61c78 72
Arnoud113 0:77ad62c61c78 73 //PID
Arnoud113 0:77ad62c61c78 74 return Kp*e + Ki*e_int + Kd * e_der;
Arnoud113 0:77ad62c61c78 75
Arnoud113 0:77ad62c61c78 76 }
Arnoud113 0:77ad62c61c78 77
Arnoud113 0:77ad62c61c78 78
Arnoud113 0:77ad62c61c78 79
Arnoud113 0:77ad62c61c78 80
Arnoud113 0:77ad62c61c78 81
Arnoud113 0:77ad62c61c78 82
Arnoud113 0:77ad62c61c78 83 void Controller(){
Arnoud113 1:13d8940f0fd4 84 //double x = potMeter1 * potmultiplier;
Arnoud113 1:13d8940f0fd4 85 double x = Get_X_Position();
Arnoud113 1:13d8940f0fd4 86
Arnoud113 1:13d8940f0fd4 87 double y = Get_Y_Position();
Arnoud113 1:13d8940f0fd4 88 //double y = potMeter2 * potmultiplier;
Arnoud113 0:77ad62c61c78 89
Arnoud113 0:77ad62c61c78 90 double reference_motor1 = atan(y/x); // reference for Theta
Arnoud113 0:77ad62c61c78 91 double reference_motor2 = sqrt((x*x+y*y)); // reference for radius
Arnoud113 0:77ad62c61c78 92
Arnoud113 2:2563d1d8461f 93 int position_motor1 = Encoder1.getPulses(); // current position for theta
Arnoud113 2:2563d1d8461f 94 int position_motor2 = Encoder2.getPulses(); // current position for the radius
Arnoud113 0:77ad62c61c78 95
Arnoud113 0:77ad62c61c78 96 double motor1 = PID(reference_motor1 - position_motor1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
Arnoud113 0:77ad62c61c78 97 double motor2 = PID(reference_motor2 - position_motor2, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
Arnoud113 0:77ad62c61c78 98
Arnoud113 0:77ad62c61c78 99 pc.baud(115200);
Arnoud113 1:13d8940f0fd4 100 pc.printf("\r Position(X,Y)=(%f,%f), Ref(Theta,R): (%f,%f), Pos(Theta,R):(%i,%i), Motor Value(M1,M2):(%f,%f).\n",x, y, reference_motor1, reference_motor2, position_motor1, position_motor2, motor1, motor2);
Arnoud113 0:77ad62c61c78 101
Arnoud113 2:2563d1d8461f 102 //motor1PWM = motor1;
Arnoud113 2:2563d1d8461f 103 //motor2PWM = motor2;
Arnoud113 0:77ad62c61c78 104
Arnoud113 1:13d8940f0fd4 105 if(motor1 > 0.3){
Arnoud113 0:77ad62c61c78 106 motor1DC = 1;
Arnoud113 0:77ad62c61c78 107
Arnoud113 0:77ad62c61c78 108 ledr = 1;
Arnoud113 0:77ad62c61c78 109 ledg = 1; //Blau
Arnoud113 0:77ad62c61c78 110 ledb = 0;
Arnoud113 0:77ad62c61c78 111 }
Arnoud113 1:13d8940f0fd4 112 else if (motor1<-0.3) {
Arnoud113 0:77ad62c61c78 113 motor1DC = 0;
Arnoud113 0:77ad62c61c78 114
Arnoud113 0:77ad62c61c78 115 ledb = 1;
Arnoud113 0:77ad62c61c78 116 ledr = 1;
Arnoud113 0:77ad62c61c78 117 ledg = 0; //Groen
Arnoud113 0:77ad62c61c78 118
Arnoud113 0:77ad62c61c78 119 }
Arnoud113 0:77ad62c61c78 120 else{
Arnoud113 0:77ad62c61c78 121 motor1PWM = 0;
Arnoud113 0:77ad62c61c78 122
Arnoud113 0:77ad62c61c78 123 ledb = 1; //Rood
Arnoud113 0:77ad62c61c78 124 ledr = 0;
Arnoud113 0:77ad62c61c78 125 ledg = 1;
Arnoud113 0:77ad62c61c78 126 }
Arnoud113 0:77ad62c61c78 127
Arnoud113 0:77ad62c61c78 128 if(motor2 > 0.3){
Arnoud113 0:77ad62c61c78 129 motor1DC = 1;
Arnoud113 0:77ad62c61c78 130
Arnoud113 0:77ad62c61c78 131 ledr = 1;
Arnoud113 0:77ad62c61c78 132 ledg = 1; //Blau
Arnoud113 0:77ad62c61c78 133 ledb = 0;
Arnoud113 0:77ad62c61c78 134 }
Arnoud113 0:77ad62c61c78 135 else if (motor2<-0.3) {
Arnoud113 0:77ad62c61c78 136 motor1DC = 0;
Arnoud113 0:77ad62c61c78 137
Arnoud113 0:77ad62c61c78 138 ledb = 1;
Arnoud113 0:77ad62c61c78 139 ledr = 1;
Arnoud113 0:77ad62c61c78 140 ledg = 0; //Groen
Arnoud113 0:77ad62c61c78 141
Arnoud113 0:77ad62c61c78 142 }
Arnoud113 0:77ad62c61c78 143 else{
Arnoud113 0:77ad62c61c78 144 motor2PWM = 0;
Arnoud113 0:77ad62c61c78 145
Arnoud113 0:77ad62c61c78 146 ledb = 1; //Rood
Arnoud113 0:77ad62c61c78 147 ledr = 0;
Arnoud113 0:77ad62c61c78 148 ledg = 1;
Arnoud113 0:77ad62c61c78 149 }
Arnoud113 0:77ad62c61c78 150 }
Arnoud113 0:77ad62c61c78 151
Arnoud113 0:77ad62c61c78 152 int main()
Arnoud113 0:77ad62c61c78 153 {
Arnoud113 0:77ad62c61c78 154 controller.attach(&Controller, M1_TS);
Arnoud113 0:77ad62c61c78 155
Arnoud113 0:77ad62c61c78 156 while(1){}
Arnoud113 0:77ad62c61c78 157
Arnoud113 0:77ad62c61c78 158 }