Wouter Bos / Mbed 2 deprecated 005-Motor_pot_PID-control

control flow doet nog niks

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@1:2f23368e8638, 2015-10-07 (annotated)

Committer:
Zeekat
Date:
Wed Oct 07 18:19:07 2015 +0000
Revision:
1:2f23368e8638
Parent:
0:f6306e179750
Child:
2:2563df4ee829
untested PID controller with P-controller settings. output is not yet limited

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:f6306e179750 1 #include "mbed.h"
Zeekat 0:f6306e179750 2 #include "MODSERIAL.h"
Zeekat 0:f6306e179750 3 #include "encoder.h"
Zeekat 0:f6306e179750 4 // #include "HIDScope.h"
Zeekat 0:f6306e179750 5
Zeekat 0:f6306e179750 6 Serial pc(USBTX,USBRX);
Zeekat 0:f6306e179750 7 // HIDScope scope(4); // definieerd het aantal kanalen van de scope
Zeekat 0:f6306e179750 8
Zeekat 0:f6306e179750 9 // Define Tickers and stuff
Zeekat 0:f6306e179750 10 Ticker controller1, controller2; // definieer de ticker die controler1 doet
Zeekat 0:f6306e179750 11 double controlfreq = 10 ; // controlloops frequentie (Hz)
Zeekat 0:f6306e179750 12 double controlstep = 1/controlfreq; // timestep derived from controlfreq
Zeekat 0:f6306e179750 13
Zeekat 0:f6306e179750 14 //MOTOR INPUTPINS
Zeekat 0:f6306e179750 15 // motor 1
Zeekat 0:f6306e179750 16 PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets)
Zeekat 0:f6306e179750 17 DigitalOut motor1_rich(D7); // digitaal signaal voor richting
Zeekat 0:f6306e179750 18 // motor 2
Zeekat 0:f6306e179750 19 PwmOut motor2_aan(D5);
Zeekat 0:f6306e179750 20 DigitalOut motor2_rich(D4);
Zeekat 0:f6306e179750 21
Zeekat 0:f6306e179750 22 // ENCODER
Zeekat 0:f6306e179750 23 Encoder motor1_enc(D12,D11); // encoder outputpins
Zeekat 0:f6306e179750 24 Encoder motor2_enc(D10,D9);
Zeekat 0:f6306e179750 25
Zeekat 0:f6306e179750 26 //POTMETERS
Zeekat 0:f6306e179750 27 AnalogIn potright(A0); // define the potmeter outputpins
Zeekat 0:f6306e179750 28 AnalogIn potleft(A1);
Zeekat 0:f6306e179750 29
Zeekat 0:f6306e179750 30 // RESETBUTTON
Zeekat 0:f6306e179750 31 DigitalIn button(PTA4); // defines the button used for a encoder reset
Zeekat 0:f6306e179750 32 int button_pressed = 0;
Zeekat 0:f6306e179750 33
Zeekat 0:f6306e179750 34 int reference1 = 0; // set the reference position of the encoders
Zeekat 0:f6306e179750 35 int reference2 = 0;
Zeekat 0:f6306e179750 36
Zeekat 0:f6306e179750 37 // CONTROLLER SETTINGS
Zeekat 1:2f23368e8638 38 // motor 1
Zeekat 1:2f23368e8638 39 const double m1_Kp = 1; // Proportional constant
Zeekat 1:2f23368e8638 40 const double m1_Ki = 0; // integration constant
Zeekat 1:2f23368e8638 41 const double m1_Kd = 0; // differentiation constant
Zeekat 1:2f23368e8638 42 // motor 2
Zeekat 1:2f23368e8638 43 const double m2_Kp = 1;
Zeekat 1:2f23368e8638 44 const double m2_Ki = 0;
Zeekat 1:2f23368e8638 45 const double m2_Kd = 0;
Zeekat 1:2f23368e8638 46 // storage variables
Zeekat 1:2f23368e8638 47 // motor 1
Zeekat 1:2f23368e8638 48 double m1_err_int = 0;
Zeekat 1:2f23368e8638 49 double m1_prev_err = 0;
Zeekat 1:2f23368e8638 50 // motor 2
Zeekat 1:2f23368e8638 51 double m2_err_int = 0;
Zeekat 1:2f23368e8638 52 double m2_prev_err = 0;
Zeekat 0:f6306e179750 53
Zeekat 0:f6306e179750 54
Zeekat 0:f6306e179750 55 // SETPOINT SETTINGS
Zeekat 0:f6306e179750 56 // define storage variables for setpoint values
Zeekat 1:2f23368e8638 57 double c_setpoint1 = 0;
Zeekat 1:2f23368e8638 58 double c_setpoint2 = 0;
Zeekat 0:f6306e179750 59 // define the maximum rate of change for the setpoint (velocity)
Zeekat 1:2f23368e8638 60 double Vmax = 5; // rad/sec
Zeekat 1:2f23368e8638 61
Zeekat 1:2f23368e8638 62
Zeekat 1:2f23368e8638 63 //// FILTER VARIABLES
Zeekat 1:2f23368e8638 64 // storage variables
Zeekat 1:2f23368e8638 65 // differential action filter, same is used for both controllers
Zeekat 1:2f23368e8638 66 double m_f_v1 = 0;
Zeekat 1:2f23368e8638 67 double m_f_v2 = 0;
Zeekat 1:2f23368e8638 68
Zeekat 1:2f23368e8638 69 // Filter coefficients
Zeekat 1:2f23368e8638 70 // differential action filter
Zeekat 1:2f23368e8638 71 const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0; // coefficients from sheets are used as first test.
Zeekat 0:f6306e179750 72
Zeekat 0:f6306e179750 73
Zeekat 0:f6306e179750 74
Zeekat 0:f6306e179750 75 ////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 76 /////////////////// START OF SIDE FUNCTIONS ////////////////////
Zeekat 0:f6306e179750 77 //////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 78 // these functions are tailored to perform 1 specific function
Zeekat 0:f6306e179750 79
Zeekat 0:f6306e179750 80
Zeekat 0:f6306e179750 81 // counts 2 radians
Zeekat 0:f6306e179750 82 // this function takes counts from the encoder and converts it to the amount of radians from the zero position.
Zeekat 0:f6306e179750 83 // It has been set up for standard 2X DECODING!!!!!!
Zeekat 0:f6306e179750 84 double get_radians(double counts)
Zeekat 0:f6306e179750 85 {
Zeekat 0:f6306e179750 86 double pi = 3.14159265359;
Zeekat 0:f6306e179750 87 double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, extra warning)
Zeekat 0:f6306e179750 88 return radians;
Zeekat 0:f6306e179750 89 }
Zeekat 0:f6306e179750 90
Zeekat 0:f6306e179750 91
Zeekat 0:f6306e179750 92 // This functions takes a 0->1 input converts it to -1->1 and uses passing by reference (&c_setpoint)
Zeekat 0:f6306e179750 93 // to create a reference that moves with a variable speed. It is now set up for potmeter values.
Zeekat 0:f6306e179750 94 double setpoint_f(double input, double &c_setpoint)
Zeekat 0:f6306e179750 95 {
Zeekat 0:f6306e179750 96 double offset = 0.5, gain = 2;
Zeekat 0:f6306e179750 97 double potset = (input-offset)*gain;
Zeekat 0:f6306e179750 98 double setpoint = c_setpoint + potset * controlstep * Vmax ;
Zeekat 0:f6306e179750 99 c_setpoint = setpoint;
Zeekat 0:f6306e179750 100 return setpoint;
Zeekat 0:f6306e179750 101 }
Zeekat 0:f6306e179750 102
Zeekat 0:f6306e179750 103
Zeekat 0:f6306e179750 104 // This function takes the inputvalue and ensures it is between -1 and 1
Zeekat 0:f6306e179750 105 // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
Zeekat 0:f6306e179750 106 // I am aware it has unnecesary steps but it works.
Zeekat 0:f6306e179750 107 double outputlimiter (double output)
Zeekat 0:f6306e179750 108 {
Zeekat 0:f6306e179750 109 if(output>1)
Zeekat 0:f6306e179750 110 {
Zeekat 0:f6306e179750 111 output = 1;
Zeekat 0:f6306e179750 112 }
Zeekat 0:f6306e179750 113 else if(output < 1 && output > 0)
Zeekat 0:f6306e179750 114 {
Zeekat 0:f6306e179750 115 output = output;
Zeekat 0:f6306e179750 116 }
Zeekat 0:f6306e179750 117 else if(output > -1 && output < 0)
Zeekat 0:f6306e179750 118 {
Zeekat 0:f6306e179750 119 output = output;
Zeekat 0:f6306e179750 120 }
Zeekat 0:f6306e179750 121 else if(output < -1)
Zeekat 0:f6306e179750 122 {
Zeekat 0:f6306e179750 123 (output = -1);
Zeekat 0:f6306e179750 124 }
Zeekat 0:f6306e179750 125 return output;
Zeekat 0:f6306e179750 126 }
Zeekat 0:f6306e179750 127
Zeekat 0:f6306e179750 128
Zeekat 1:2f23368e8638 129 // BIQUADFILTER CODE GIVEN IN SHEETS
Zeekat 1:2f23368e8638 130 // used for d-control
Zeekat 1:2f23368e8638 131 double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
Zeekat 1:2f23368e8638 132 {
Zeekat 1:2f23368e8638 133 double v = u- a1*v1-a2*v2;
Zeekat 1:2f23368e8638 134 double y = b0*v+b1*v1+b2*v2;
Zeekat 1:2f23368e8638 135 v2 = v1;
Zeekat 1:2f23368e8638 136 v1 = v;
Zeekat 1:2f23368e8638 137 return y;
Zeekat 1:2f23368e8638 138 }
Zeekat 1:2f23368e8638 139
Zeekat 1:2f23368e8638 140
Zeekat 1:2f23368e8638 141 // PID Controller given in sheets
Zeekat 1:2f23368e8638 142 // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!)
Zeekat 1:2f23368e8638 143 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
Zeekat 1:2f23368e8638 144 {
Zeekat 1:2f23368e8638 145 // Proportional
Zeekat 1:2f23368e8638 146 double P = Kp * e;
Zeekat 1:2f23368e8638 147
Zeekat 1:2f23368e8638 148 // Derivative
Zeekat 1:2f23368e8638 149 double e_derr = (e - e_prev)/Ts;
Zeekat 1:2f23368e8638 150 e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2);
Zeekat 1:2f23368e8638 151 e_prev = e;
Zeekat 1:2f23368e8638 152 double D = Kd* e_derr;
Zeekat 1:2f23368e8638 153
Zeekat 1:2f23368e8638 154 // Integral
Zeekat 1:2f23368e8638 155 e_int = e_int + Ts * e;
Zeekat 1:2f23368e8638 156 double I = e_int * Ki;
Zeekat 1:2f23368e8638 157
Zeekat 1:2f23368e8638 158 // PID
Zeekat 1:2f23368e8638 159 double output = P + I + D;
Zeekat 1:2f23368e8638 160 return output;
Zeekat 1:2f23368e8638 161 }
Zeekat 1:2f23368e8638 162
Zeekat 1:2f23368e8638 163
Zeekat 1:2f23368e8638 164
Zeekat 0:f6306e179750 165 // this function is a simple K control called by the motor function
Zeekat 0:f6306e179750 166 double K_control(double error,double K)
Zeekat 0:f6306e179750 167 {
Zeekat 0:f6306e179750 168 double output = K*error; // controller output K*e
Zeekat 0:f6306e179750 169 output = outputlimiter(output); // limit the output to -1->1
Zeekat 0:f6306e179750 170 return output;
Zeekat 0:f6306e179750 171 }
Zeekat 0:f6306e179750 172
Zeekat 0:f6306e179750 173 /////////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 174 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
Zeekat 0:f6306e179750 175 ///////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 176 // these functions are used to control all aspects of a single electric motor and are called by the main function
Zeekat 0:f6306e179750 177
Zeekat 0:f6306e179750 178 // MOTOR 1
Zeekat 0:f6306e179750 179 void motor1_control()
Zeekat 0:f6306e179750 180 {
Zeekat 0:f6306e179750 181 double setpoint1 = setpoint_f(potright.read(), c_setpoint1); // determine the setpoint that has been set by the inputsignal
Zeekat 0:f6306e179750 182 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 0:f6306e179750 183 double error1 = (setpoint1 - rads1); // determine the error (reference - position)
Zeekat 0:f6306e179750 184 // scope.set(0,setpoint1); // disabled because HIDScope is acting strangely
Zeekat 0:f6306e179750 185 // scope.set(1,rads1);
Zeekat 0:f6306e179750 186 // scope.send();
Zeekat 1:2f23368e8638 187
Zeekat 1:2f23368e8638 188 // double output1 = K_control(error1, m1_Kp); // bereken de controller output voor motor 1(zie hierboven)
Zeekat 1:2f23368e8638 189 double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
Zeekat 0:f6306e179750 190 if(output1 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:f6306e179750 191 motor1_rich.write(0);
Zeekat 0:f6306e179750 192 motor1_aan.write(output1);
Zeekat 0:f6306e179750 193 } else if(output1 < 0) {
Zeekat 0:f6306e179750 194 motor1_rich.write(1);
Zeekat 0:f6306e179750 195 motor1_aan.write(abs(output1));
Zeekat 0:f6306e179750 196 }
Zeekat 0:f6306e179750 197 }
Zeekat 0:f6306e179750 198
Zeekat 0:f6306e179750 199 // MOTOR 2
Zeekat 0:f6306e179750 200 void motor2_control()
Zeekat 0:f6306e179750 201 {
Zeekat 0:f6306e179750 202 double setpoint2 = setpoint_f(potleft.read(), c_setpoint2); // determine the setpoint that has been set by the inputsignal
Zeekat 0:f6306e179750 203 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 0:f6306e179750 204 double error2 = (setpoint2 - rads2); // determine the error (reference - position)
Zeekat 0:f6306e179750 205 // scope.set(3,setpoint2); // disabled because HIDScope is acting strangely
Zeekat 0:f6306e179750 206 // scope.set(4,rads2);
Zeekat 0:f6306e179750 207 // scope.send();
Zeekat 1:2f23368e8638 208
Zeekat 1:2f23368e8638 209 // double output2 = K_control(error2, m2_Kp); // bereken de controller output voor motor 1(zie hierboven)
Zeekat 1:2f23368e8638 210 double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
Zeekat 0:f6306e179750 211 if(output2 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:f6306e179750 212 motor2_rich.write(0);
Zeekat 0:f6306e179750 213 motor2_aan.write(output2);
Zeekat 0:f6306e179750 214 } else if(output2 < 0) {
Zeekat 0:f6306e179750 215 motor2_rich.write(1);
Zeekat 0:f6306e179750 216 motor2_aan.write(abs(output2));
Zeekat 0:f6306e179750 217 }
Zeekat 0:f6306e179750 218 }
Zeekat 0:f6306e179750 219
Zeekat 0:f6306e179750 220
Zeekat 0:f6306e179750 221 int main()
Zeekat 0:f6306e179750 222 {
Zeekat 1:2f23368e8638 223 // Ticker calling the primary functions. If neccessary use the ticker to change bools and execute in the main loop as shown in the sheets.
Zeekat 0:f6306e179750 224 controller1.attach(&motor1_control, controlstep);
Zeekat 0:f6306e179750 225 controller2.attach(&motor2_control, controlstep);
Zeekat 0:f6306e179750 226 while(true)
Zeekat 0:f6306e179750 227 {
Zeekat 0:f6306e179750 228 if(button.read() == button_pressed) // reset the encoder to reference position
Zeekat 0:f6306e179750 229 {
Zeekat 0:f6306e179750 230 motor1_enc.setPosition(reference1);
Zeekat 0:f6306e179750 231 motor2_enc.setPosition(reference2);
Zeekat 0:f6306e179750 232 }
Zeekat 0:f6306e179750 233 }
Zeekat 0:f6306e179750 234 }