Wouter Bos / Mbed 2 deprecated 005-Motor_pot_PID-control

control flow doet nog niks

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@3:7273bbe6aa02, 2015-10-09 (annotated)

Committer:
Zeekat
Date:
Fri Oct 09 20:13:19 2015 +0000
Revision:
3:7273bbe6aa02
Parent:
2:2563df4ee829
Child:
4:072b99947fc6
simple textual update, and clean up

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