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