Wouter Bos / Mbed 2 deprecated 005-Motor_pot_PID-control

control flow doet nog niks

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@2:2563df4ee829, 2015-10-09 (annotated)

Committer:
Zeekat
Date:
Fri Oct 09 19:34:14 2015 +0000
Revision:
2:2563df4ee829
Parent:
1:2f23368e8638
Child:
3:7273bbe6aa02
redelijk werkende versie. D-actie is nog steeds kaput verder werkt het. go flag variabelen zijn geimplementeerd voor de controlloops. Verder code opgeschoond.

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