Wouter Bos / Mbed 2 deprecated 005-Motor_pot_PID-control

control flow doet nog niks

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@4:072b99947fc6, 2015-10-10 (annotated)

Committer:
Zeekat
Date:
Sat Oct 10 08:26:48 2015 +0000
Revision:
4:072b99947fc6
Parent:
3:7273bbe6aa02
Child:
5:67afe491a1e3
nog niet getest. heb wat programflow toegevoegd met ledjes die branden in een bepaald deel van het programma

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 4:072b99947fc6 40 // BUTTONS
Zeekat 4:072b99947fc6 41 DigitalIn buttonlinks(PTA4);
Zeekat 4:072b99947fc6 42 DigitalIn buttonrechts(PTC6);
Zeekat 4:072b99947fc6 43 // init values
Zeekat 4:072b99947fc6 44 bool loop_start = false;
Zeekat 4:072b99947fc6 45 bool calib_start = false;
Zeekat 2:2563df4ee829 46
Zeekat 4:072b99947fc6 47 // LED
Zeekat 4:072b99947fc6 48 DigitalOut ledred(LED1);
Zeekat 4:072b99947fc6 49 DigitalOut ledgreen(LED2);
Zeekat 4:072b99947fc6 50 DigitalOut ledblue(LED3);
Zeekat 2:2563df4ee829 51
Zeekat 3:7273bbe6aa02 52 // REFERENCE SIGNAL SETTINGS
Zeekat 3:7273bbe6aa02 53 // Define signal amplification (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!) ??
Zeekat 3:7273bbe6aa02 54 double signalamp1 = 1;
Zeekat 3:7273bbe6aa02 55 double signalamp2 = 1;
Zeekat 3:7273bbe6aa02 56 // Define gain and offset of the input(input between 0 and 1 is optimized). For EMG use 0, 1 and false, for POT use 0.5, 2 and true
Zeekat 3:7273bbe6aa02 57 double offset = 0.5;
Zeekat 3:7273bbe6aa02 58 double gain = 2;
Zeekat 3:7273bbe6aa02 59 bool POT_in = true; // show potmeter is attached, increases the range for which 0 is the output.
Zeekat 3:7273bbe6aa02 60 // Define storage variables for reference values
Zeekat 3:7273bbe6aa02 61 double c_reference1 = 0;
Zeekat 3:7273bbe6aa02 62 double c_reference2 = 0;
Zeekat 3:7273bbe6aa02 63 // Define the maximum rate of change for the reference (velocity)
Zeekat 2:2563df4ee829 64 double Vmax = 5; // rad/sec
Zeekat 2:2563df4ee829 65
Zeekat 0:f6306e179750 66 // CONTROLLER SETTINGS
Zeekat 1:2f23368e8638 67 // motor 1
Zeekat 1:2f23368e8638 68 const double m1_Kp = 1; // Proportional constant
Zeekat 2:2563df4ee829 69 const double m1_Ki = 1; // integration constant
Zeekat 1:2f23368e8638 70 const double m1_Kd = 0; // differentiation constant
Zeekat 1:2f23368e8638 71 // motor 2
Zeekat 1:2f23368e8638 72 const double m2_Kp = 1;
Zeekat 1:2f23368e8638 73 const double m2_Ki = 0;
Zeekat 1:2f23368e8638 74 const double m2_Kd = 0;
Zeekat 1:2f23368e8638 75 // storage variables
Zeekat 1:2f23368e8638 76 // motor 1
Zeekat 1:2f23368e8638 77 double m1_err_int = 0;
Zeekat 1:2f23368e8638 78 double m1_prev_err = 0;
Zeekat 1:2f23368e8638 79 // motor 2
Zeekat 1:2f23368e8638 80 double m2_err_int = 0;
Zeekat 1:2f23368e8638 81 double m2_prev_err = 0;
Zeekat 0:f6306e179750 82
Zeekat 0:f6306e179750 83
Zeekat 1:2f23368e8638 84 //// FILTER VARIABLES
Zeekat 1:2f23368e8638 85 // storage variables
Zeekat 1:2f23368e8638 86 // differential action filter, same is used for both controllers
Zeekat 1:2f23368e8638 87 double m_f_v1 = 0;
Zeekat 1:2f23368e8638 88 double m_f_v2 = 0;
Zeekat 1:2f23368e8638 89
Zeekat 1:2f23368e8638 90 // Filter coefficients
Zeekat 1:2f23368e8638 91 // differential action filter
Zeekat 1:2f23368e8638 92 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 93
Zeekat 0:f6306e179750 94
Zeekat 0:f6306e179750 95
Zeekat 0:f6306e179750 96 ////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 97 /////////////////// START OF SIDE FUNCTIONS ////////////////////
Zeekat 0:f6306e179750 98 //////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 99 // these functions are tailored to perform 1 specific function
Zeekat 0:f6306e179750 100
Zeekat 0:f6306e179750 101
Zeekat 0:f6306e179750 102 // counts 2 radians
Zeekat 0:f6306e179750 103 // this function takes counts from the encoder and converts it to the amount of radians from the zero position.
Zeekat 0:f6306e179750 104 // It has been set up for standard 2X DECODING!!!!!!
Zeekat 0:f6306e179750 105 double get_radians(double counts)
Zeekat 0:f6306e179750 106 {
Zeekat 0:f6306e179750 107 double pi = 3.14159265359;
Zeekat 0:f6306e179750 108 double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, extra warning)
Zeekat 0:f6306e179750 109 return radians;
Zeekat 0:f6306e179750 110 }
Zeekat 0:f6306e179750 111
Zeekat 0:f6306e179750 112
Zeekat 3:7273bbe6aa02 113 // This functions takes a 0->1 input converts it to -1->1 and uses passing by reference (&c_reference)
Zeekat 0:f6306e179750 114 // to create a reference that moves with a variable speed. It is now set up for potmeter values.
Zeekat 3:7273bbe6aa02 115 double reference_f(double input, double &c_reference)
Zeekat 0:f6306e179750 116 {
Zeekat 3:7273bbe6aa02 117 double potset = (input-offset)*gain;
Zeekat 3:7273bbe6aa02 118 if(POT_in == true && potset < 0.1 && potset > -0.1) // larger area for potmeter to get a zero value
Zeekat 2:2563df4ee829 119 {
Zeekat 2:2563df4ee829 120 potset = 0;
Zeekat 2:2563df4ee829 121 }
Zeekat 3:7273bbe6aa02 122 double reference = c_reference + potset * controlstep * Vmax ;
Zeekat 3:7273bbe6aa02 123 c_reference = reference; // change the global variable to the latest location.
Zeekat 3:7273bbe6aa02 124 return reference;
Zeekat 0:f6306e179750 125 }
Zeekat 0:f6306e179750 126
Zeekat 0:f6306e179750 127
Zeekat 3:7273bbe6aa02 128 // This function takes the controller outputvalue and ensures it is between -1 and 1
Zeekat 0:f6306e179750 129 // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
Zeekat 3:7273bbe6aa02 130 // needs more work to use it for the wind-up prevention.
Zeekat 2:2563df4ee829 131 double outputlimiter (double output, double limit)
Zeekat 0:f6306e179750 132 {
Zeekat 2:2563df4ee829 133 if(output> limit)
Zeekat 0:f6306e179750 134 {
Zeekat 0:f6306e179750 135 output = 1;
Zeekat 0:f6306e179750 136 }
Zeekat 2:2563df4ee829 137 else if(output < limit && output > 0)
Zeekat 0:f6306e179750 138 {
Zeekat 0:f6306e179750 139 output = output;
Zeekat 0:f6306e179750 140 }
Zeekat 2:2563df4ee829 141 else if(output > -limit && output < 0)
Zeekat 0:f6306e179750 142 {
Zeekat 0:f6306e179750 143 output = output;
Zeekat 0:f6306e179750 144 }
Zeekat 2:2563df4ee829 145 else if(output < -limit)
Zeekat 0:f6306e179750 146 {
Zeekat 0:f6306e179750 147 (output = -1);
Zeekat 0:f6306e179750 148 }
Zeekat 0:f6306e179750 149 return output;
Zeekat 0:f6306e179750 150 }
Zeekat 0:f6306e179750 151
Zeekat 0:f6306e179750 152
Zeekat 1:2f23368e8638 153 // BIQUADFILTER CODE GIVEN IN SHEETS
Zeekat 1:2f23368e8638 154 // used for d-control
Zeekat 1:2f23368e8638 155 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 156 {
Zeekat 3:7273bbe6aa02 157 double v = u - a1*v1 - a2*v2;
Zeekat 3:7273bbe6aa02 158 double y = b0*v + b1*v1 + b2*v2;
Zeekat 1:2f23368e8638 159 v2 = v1;
Zeekat 1:2f23368e8638 160 v1 = v;
Zeekat 1:2f23368e8638 161 return y;
Zeekat 1:2f23368e8638 162 }
Zeekat 1:2f23368e8638 163
Zeekat 1:2f23368e8638 164
Zeekat 1:2f23368e8638 165 // PID Controller given in sheets
Zeekat 1:2f23368e8638 166 // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!)
Zeekat 2:2563df4ee829 167 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
Zeekat 1:2f23368e8638 168 {
Zeekat 1:2f23368e8638 169 // Proportional
Zeekat 1:2f23368e8638 170 double P = Kp * e;
Zeekat 3:7273bbe6aa02 171 pc.printf("P = %f, ",P);
Zeekat 1:2f23368e8638 172 // Integral
Zeekat 1:2f23368e8638 173 e_int = e_int + Ts * e;
Zeekat 1:2f23368e8638 174 double I = e_int * Ki;
Zeekat 3:7273bbe6aa02 175 pc.printf("I = %f, ",I);
Zeekat 3:7273bbe6aa02 176 // Derivative (Disabled for now because of NaN problem from filter
Zeekat 3:7273bbe6aa02 177 double e_derr = (e - e_prev)/Ts;
Zeekat 3:7273bbe6aa02 178 pc.printf("e_derr %f, ",e_derr);
Zeekat 3:7273bbe6aa02 179 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 3:7273bbe6aa02 180 pc.printf("fil_e_derr %f, ",e_derr); // check for NaN ??
Zeekat 3:7273bbe6aa02 181 e_prev = e;
Zeekat 3:7273bbe6aa02 182 // double D = Kd* e_derr;
Zeekat 1:2f23368e8638 183
Zeekat 1:2f23368e8638 184 // PID
Zeekat 2:2563df4ee829 185 double output = P + I ;// + D;
Zeekat 1:2f23368e8638 186 return output;
Zeekat 1:2f23368e8638 187 }
Zeekat 1:2f23368e8638 188
Zeekat 1:2f23368e8638 189
Zeekat 1:2f23368e8638 190
Zeekat 0:f6306e179750 191 // this function is a simple K control called by the motor function
Zeekat 0:f6306e179750 192 double K_control(double error,double K)
Zeekat 0:f6306e179750 193 {
Zeekat 0:f6306e179750 194 double output = K*error; // controller output K*e
Zeekat 2:2563df4ee829 195 output = outputlimiter(output,1); // limit the output to -1->1
Zeekat 0:f6306e179750 196 return output;
Zeekat 0:f6306e179750 197 }
Zeekat 0:f6306e179750 198
Zeekat 0:f6306e179750 199 /////////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 200 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
Zeekat 0:f6306e179750 201 ///////////////////////////////////////////////////////////////////
Zeekat 0:f6306e179750 202 // these functions are used to control all aspects of a single electric motor and are called by the main function
Zeekat 0:f6306e179750 203
Zeekat 0:f6306e179750 204 // MOTOR 1
Zeekat 0:f6306e179750 205 void motor1_control()
Zeekat 0:f6306e179750 206 {
Zeekat 3:7273bbe6aa02 207 double input1 = potright.read()*signalamp1;
Zeekat 3:7273bbe6aa02 208 double reference1 = reference_f(input1, c_reference1); // determine the reference that has been set by the inputsignal
Zeekat 0:f6306e179750 209 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 3:7273bbe6aa02 210 double error1 = (reference1 - rads1); // determine the error (reference - position)
Zeekat 3:7273bbe6aa02 211 scope.set(0,reference1);
Zeekat 2:2563df4ee829 212 scope.set(1,rads1);
Zeekat 2:2563df4ee829 213 scope.send();
Zeekat 1:2f23368e8638 214 double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
Zeekat 2:2563df4ee829 215 pc.printf("output 1 %f \n",output1);
Zeekat 2:2563df4ee829 216
Zeekat 4:072b99947fc6 217 // ws best locatie om output te blokkeren als grenzen bereikt zijn, simpel if-loopje met rad1??
Zeekat 2:2563df4ee829 218
Zeekat 4:072b99947fc6 219
Zeekat 4:072b99947fc6 220 // onderstaand kan vervangen worden door een functie voor beide motoren??
Zeekat 0:f6306e179750 221 if(output1 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:f6306e179750 222 motor1_rich.write(0);
Zeekat 0:f6306e179750 223 motor1_aan.write(output1);
Zeekat 0:f6306e179750 224 } else if(output1 < 0) {
Zeekat 0:f6306e179750 225 motor1_rich.write(1);
Zeekat 0:f6306e179750 226 motor1_aan.write(abs(output1));
Zeekat 0:f6306e179750 227 }
Zeekat 0:f6306e179750 228 }
Zeekat 0:f6306e179750 229
Zeekat 0:f6306e179750 230 // MOTOR 2
Zeekat 0:f6306e179750 231 void motor2_control()
Zeekat 0:f6306e179750 232 {
Zeekat 3:7273bbe6aa02 233 double input2 = potleft.read()*signalamp2;
Zeekat 3:7273bbe6aa02 234 double reference2 = reference_f(input2, c_reference2); // determine the reference that has been set by the inputsignal
Zeekat 0:f6306e179750 235 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 3:7273bbe6aa02 236 double error2 = (reference2 - rads2); // determine the error (reference - position)
Zeekat 3:7273bbe6aa02 237 // scope.set(3,reference2);
Zeekat 0:f6306e179750 238 // scope.set(4,rads2);
Zeekat 0:f6306e179750 239 // scope.send();
Zeekat 1:2f23368e8638 240
Zeekat 1:2f23368e8638 241 double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
Zeekat 0:f6306e179750 242 if(output2 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:f6306e179750 243 motor2_rich.write(0);
Zeekat 0:f6306e179750 244 motor2_aan.write(output2);
Zeekat 0:f6306e179750 245 } else if(output2 < 0) {
Zeekat 0:f6306e179750 246 motor2_rich.write(1);
Zeekat 0:f6306e179750 247 motor2_aan.write(abs(output2));
Zeekat 0:f6306e179750 248 }
Zeekat 0:f6306e179750 249 }
Zeekat 0:f6306e179750 250
Zeekat 0:f6306e179750 251
Zeekat 2:2563df4ee829 252 //////////////////////////////////////////////////////////////////
Zeekat 2:2563df4ee829 253 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
Zeekat 2:2563df4ee829 254 ////////////////////////////////////////////////////////////////
Zeekat 2:2563df4ee829 255
Zeekat 2:2563df4ee829 256 void motor1_activate()
Zeekat 2:2563df4ee829 257 {
Zeekat 2:2563df4ee829 258 motor1_go = true;
Zeekat 2:2563df4ee829 259 }
Zeekat 2:2563df4ee829 260
Zeekat 2:2563df4ee829 261 void motor2_activate()
Zeekat 2:2563df4ee829 262 {
Zeekat 2:2563df4ee829 263 motor2_go = true;
Zeekat 2:2563df4ee829 264 }
Zeekat 2:2563df4ee829 265
Zeekat 0:f6306e179750 266 int main()
Zeekat 0:f6306e179750 267 {
Zeekat 2:2563df4ee829 268 pc.baud(115200);
Zeekat 4:072b99947fc6 269 controller1.attach(&motor1_activate, controlstep); // call a go-flag
Zeekat 4:072b99947fc6 270 // controller2.attach(&motor2_activate, controlstep); Disabled while debugging PID-controller ??
Zeekat 0:f6306e179750 271 while(true)
Zeekat 0:f6306e179750 272 {
Zeekat 4:072b99947fc6 273 // button functions
Zeekat 4:072b99947fc6 274 if(buttonlinks.read() == 0)
Zeekat 2:2563df4ee829 275 {
Zeekat 4:072b99947fc6 276 loop_start = !loop_start; // reverse the boolean loop_start value
Zeekat 4:072b99947fc6 277 wait(buttonlinks.read() == 1);
Zeekat 4:072b99947fc6 278 wait(0.3); // check if it causes instability!! ??
Zeekat 2:2563df4ee829 279 }
Zeekat 4:072b99947fc6 280
Zeekat 4:072b99947fc6 281 if(buttonrechts.read() == 0)
Zeekat 4:072b99947fc6 282 {
Zeekat 4:072b99947fc6 283 calib_start = !calib_start; // reverse the boolean calib_start value
Zeekat 4:072b99947fc6 284 wait(buttonrechts.read() == 1);
Zeekat 4:072b99947fc6 285 wait(0.3); // check if it causes instability!! ??
Zeekat 4:072b99947fc6 286 }
Zeekat 4:072b99947fc6 287
Zeekat 4:072b99947fc6 288 // Main Control stuff and options
Zeekat 4:072b99947fc6 289 if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops
Zeekat 2:2563df4ee829 290 {
Zeekat 4:072b99947fc6 291 ledred.write(1);
Zeekat 4:072b99947fc6 292 ledgreen.write(1);
Zeekat 4:072b99947fc6 293 ledblue.write(0);
Zeekat 4:072b99947fc6 294
Zeekat 4:072b99947fc6 295 if(motor1_go)
Zeekat 4:072b99947fc6 296 {
Zeekat 4:072b99947fc6 297 motor1_go = false;
Zeekat 4:072b99947fc6 298 motor1_control();
Zeekat 4:072b99947fc6 299 }
Zeekat 4:072b99947fc6 300 if(motor2_go)
Zeekat 4:072b99947fc6 301 {
Zeekat 4:072b99947fc6 302 motor2_go = false;
Zeekat 4:072b99947fc6 303 motor2_control();
Zeekat 4:072b99947fc6 304 }
Zeekat 2:2563df4ee829 305 }
Zeekat 4:072b99947fc6 306 if(loop_start == false && calib_start == true) // start calibration procedures
Zeekat 4:072b99947fc6 307 {
Zeekat 4:072b99947fc6 308 ledred.write(1);
Zeekat 4:072b99947fc6 309 ledgreen.write(0);
Zeekat 4:072b99947fc6 310 ledblue.write(1);
Zeekat 4:072b99947fc6 311 }
Zeekat 4:072b99947fc6 312 if(loop_start == true && calib_start == true) // check if both buttons are enabled and stop everything
Zeekat 0:f6306e179750 313 {
Zeekat 4:072b99947fc6 314 ledred.write(0);
Zeekat 4:072b99947fc6 315 ledgreen.write(1);
Zeekat 4:072b99947fc6 316 ledblue.write(1);
Zeekat 4:072b99947fc6 317 }
Zeekat 4:072b99947fc6 318 else
Zeekat 4:072b99947fc6 319 {
Zeekat 4:072b99947fc6 320 ledred.write(1);
Zeekat 4:072b99947fc6 321 ledgreen.write(1);
Zeekat 4:072b99947fc6 322 ledblue.write(1);
Zeekat 0:f6306e179750 323 }
Zeekat 0:f6306e179750 324 }
Zeekat 0:f6306e179750 325 }