Marjolein Scheffers / Mbed 2 deprecated Gr14_Derivative_control_design

derivative design

Dependencies:   FastPWM HIDScope MODSERIAL QEI mbed

Der_design.cpp@0:588962464765, 2018-10-26 (annotated)

Committer:
s1680897
Date:
Fri Oct 26 10:41:14 2018 +0000
Revision:
0:588962464765
Derivative design

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1680897 0:588962464765 1 #include "mbed.h"
s1680897 0:588962464765 2 #include "FastPWM.h"
s1680897 0:588962464765 3 #include "MODSERIAL.h"
s1680897 0:588962464765 4 #include "QEI.h"
s1680897 0:588962464765 5 #include "math.h"
s1680897 0:588962464765 6 #include "HIDScope.h"
s1680897 0:588962464765 7
s1680897 0:588962464765 8 #define PI 3.14159265
s1680897 0:588962464765 9
s1680897 0:588962464765 10 // Declare variables
s1680897 0:588962464765 11 DigitalOut gpo(D0);
s1680897 0:588962464765 12 DigitalOut led(LED_RED);
s1680897 0:588962464765 13 AnalogIn pot1(A0);
s1680897 0:588962464765 14 AnalogIn pot2(A1);
s1680897 0:588962464765 15 QEI encoder1(D10, D11, NC, 32);
s1680897 0:588962464765 16 QEI encoder2(D12, D13, NC, 32);
s1680897 0:588962464765 17 FastPWM motor1_pwm(D6);
s1680897 0:588962464765 18 DigitalOut motor1_richting(D7);
s1680897 0:588962464765 19
s1680897 0:588962464765 20 // Define the HIDScope and Ticker objects
s1680897 0:588962464765 21 HIDScope scope(1);
s1680897 0:588962464765 22 Ticker scopeTimer;
s1680897 0:588962464765 23
s1680897 0:588962464765 24 MODSERIAL pc(USBTX, USBRX);
s1680897 0:588962464765 25
s1680897 0:588962464765 26 int count = 0;
s1680897 0:588962464765 27
s1680897 0:588962464765 28 // Tickers
s1680897 0:588962464765 29 Ticker MotorTicker;
s1680897 0:588962464765 30
s1680897 0:588962464765 31
s1680897 0:588962464765 32 // Functions
s1680897 0:588962464765 33
s1680897 0:588962464765 34 void read_pots(volatile double &pot1_value, volatile double &pot2_value) // read pot 1 en pot 2
s1680897 0:588962464765 35 {
s1680897 0:588962464765 36 pot1_value = pot1.read();
s1680897 0:588962464765 37 pot2_value = pot2.read();
s1680897 0:588962464765 38 }
s1680897 0:588962464765 39
s1680897 0:588962464765 40 double det_ref(double ref_value) //zet referentiewaarde om in positie
s1680897 0:588962464765 41 {
s1680897 0:588962464765 42 return ref_value * 2 * 3.1416;
s1680897 0:588962464765 43 }
s1680897 0:588962464765 44
s1680897 0:588962464765 45 double meas_pos () //Encoder functie
s1680897 0:588962464765 46 {
s1680897 0:588962464765 47 return encoder1.getPulses()/32.0/131.25*2.0*3.1416;
s1680897 0:588962464765 48 }
s1680897 0:588962464765 49
s1680897 0:588962464765 50 double comp_error (double meas_pos, double ref_pos)
s1680897 0:588962464765 51 {
s1680897 0:588962464765 52 return ref_pos - meas_pos;
s1680897 0:588962464765 53 }
s1680897 0:588962464765 54
s1680897 0:588962464765 55 double comp_integral_error (double error) // compute the integral error
s1680897 0:588962464765 56 {
s1680897 0:588962464765 57 double T = 1.0 / 400.0;
s1680897 0:588962464765 58 volatile double error_integral = error_integral + error * T;
s1680897 0:588962464765 59
s1680897 0:588962464765 60 return error_integral;
s1680897 0:588962464765 61 }
s1680897 0:588962464765 62
s1680897 0:588962464765 63 double comp_derivative_error (double error)
s1680897 0:588962464765 64 {
s1680897 0:588962464765 65 double T = 1.0 / 400.0;
s1680897 0:588962464765 66 volatile double error_prev;
s1680897 0:588962464765 67 double error_der = (error - error_prev)/T;
s1680897 0:588962464765 68 error_prev = error;
s1680897 0:588962464765 69
s1680897 0:588962464765 70 return error_der;
s1680897 0:588962464765 71 }
s1680897 0:588962464765 72
s1680897 0:588962464765 73 // PID controller
s1680897 0:588962464765 74 double PID_controller (double pot2_val, double error, double integral_error, double error_der)
s1680897 0:588962464765 75 {
s1680897 0:588962464765 76
s1680897 0:588962464765 77 // Proportional control
s1680897 0:588962464765 78 double u_k = 30.0 * error; //20.0 * pot2_val * error;
s1680897 0:588962464765 79
s1680897 0:588962464765 80
s1680897 0:588962464765 81 // Integral control
s1680897 0:588962464765 82 double u_i = 20.0 * pot2_val * integral_error;
s1680897 0:588962464765 83
s1680897 0:588962464765 84 // Derivative
s1680897 0:588962464765 85
s1680897 0:588962464765 86 double u_d = 30.0 * error_der;
s1680897 0:588962464765 87
s1680897 0:588962464765 88
s1680897 0:588962464765 89 //u definieren
s1680897 0:588962464765 90 double u = u_k; //+ u_d; //+ u_i;
s1680897 0:588962464765 91
s1680897 0:588962464765 92 // Signaal binnen de perken houden.
s1680897 0:588962464765 93 if (u > 1.0)
s1680897 0:588962464765 94 {
s1680897 0:588962464765 95 u = 1.0;
s1680897 0:588962464765 96 }
s1680897 0:588962464765 97
s1680897 0:588962464765 98 else if (u < -1.0)
s1680897 0:588962464765 99 {
s1680897 0:588962464765 100 u = -1.0;
s1680897 0:588962464765 101 }
s1680897 0:588962464765 102
s1680897 0:588962464765 103
s1680897 0:588962464765 104
s1680897 0:588962464765 105
s1680897 0:588962464765 106 return u;
s1680897 0:588962464765 107 }
s1680897 0:588962464765 108
s1680897 0:588962464765 109
s1680897 0:588962464765 110 //Move motor
s1680897 0:588962464765 111 void move_mot(double u)
s1680897 0:588962464765 112 {
s1680897 0:588962464765 113
s1680897 0:588962464765 114 if (u <= 0)
s1680897 0:588962464765 115 {
s1680897 0:588962464765 116 motor1_richting = 0;
s1680897 0:588962464765 117 motor1_pwm.write(-u); //write Duty cycle
s1680897 0:588962464765 118 }
s1680897 0:588962464765 119
s1680897 0:588962464765 120 if (u >= 0)
s1680897 0:588962464765 121 {
s1680897 0:588962464765 122 motor1_richting = 1;
s1680897 0:588962464765 123 motor1_pwm.write(u); //write Duty cycle
s1680897 0:588962464765 124 }
s1680897 0:588962464765 125 }
s1680897 0:588962464765 126
s1680897 0:588962464765 127 //sinefunction
s1680897 0:588962464765 128 double sinefunct()
s1680897 0:588962464765 129 {
s1680897 0:588962464765 130 static int t;
s1680897 0:588962464765 131 double pos_sine = sin((double)t*0.0025*0.5*PI);
s1680897 0:588962464765 132 t++;
s1680897 0:588962464765 133 return (pos_sine + 1.0) / 2.0;
s1680897 0:588962464765 134 }
s1680897 0:588962464765 135
s1680897 0:588962464765 136 // Alles samenvoegen
s1680897 0:588962464765 137 void Motor_control()
s1680897 0:588962464765 138 {
s1680897 0:588962464765 139
s1680897 0:588962464765 140 volatile double pot_value;
s1680897 0:588962464765 141
s1680897 0:588962464765 142 volatile double pot2_val;
s1680897 0:588962464765 143
s1680897 0:588962464765 144 read_pots(pot_value, pot2_val);
s1680897 0:588962464765 145
s1680897 0:588962464765 146 volatile double yref = sinefunct(); // reference position
s1680897 0:588962464765 147
s1680897 0:588962464765 148 volatile double y = meas_pos(); // measured position
s1680897 0:588962464765 149
s1680897 0:588962464765 150 volatile double error = comp_error (y, yref);
s1680897 0:588962464765 151
s1680897 0:588962464765 152 volatile double integral_error = comp_integral_error(error);
s1680897 0:588962464765 153
s1680897 0:588962464765 154 volatile double error_der = comp_derivative_error(error);
s1680897 0:588962464765 155
s1680897 0:588962464765 156 scope.set(0, error_der);
s1680897 0:588962464765 157
s1680897 0:588962464765 158 //scope.set(0, meas_pos());
s1680897 0:588962464765 159
s1680897 0:588962464765 160 volatile double u = PID_controller(pot2_val, error, integral_error, error_der); // output PID controller
s1680897 0:588962464765 161
s1680897 0:588962464765 162 move_mot(u); //functie die motor laat bewegen.
s1680897 0:588962464765 163
s1680897 0:588962464765 164 count++;
s1680897 0:588962464765 165 //pc.printf("count = %i", count);
s1680897 0:588962464765 166
s1680897 0:588962464765 167 if (count == 400) // print 1x per seconde waardes.
s1680897 0:588962464765 168 {
s1680897 0:588962464765 169 pc.printf("u = %lf, measured position y = %lf, reference position yref = %lf, gain = %lf, error = %lf, integral error = %lf, derivative error = %lf \n\r", u, y, yref, pot2_val*20.0, error, integral_error, error_der);
s1680897 0:588962464765 170 count = 0;
s1680897 0:588962464765 171 }
s1680897 0:588962464765 172 }
s1680897 0:588962464765 173
s1680897 0:588962464765 174
s1680897 0:588962464765 175 int main()
s1680897 0:588962464765 176 {
s1680897 0:588962464765 177 pc.baud(115200);
s1680897 0:588962464765 178
s1680897 0:588962464765 179 int frequency_pwm = 16700; //16.7 kHz PWM
s1680897 0:588962464765 180 motor1_pwm.period(1.0/frequency_pwm); // T = 1/f
s1680897 0:588962464765 181 MotorTicker.attach(Motor_control, 0.0025);
s1680897 0:588962464765 182 scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
s1680897 0:588962464765 183
s1680897 0:588962464765 184 while (true)
s1680897 0:588962464765 185 {}
s1680897 0:588962464765 186 }