Thomas Burgers / Mbed 2 deprecated ZZ-TheChenneRobot

Dependencies:   Encoder HIDScope MODSERIAL QEI biquadFilter mbed

Committer:
ThomasBNL
Date:
Tue Oct 13 21:37:23 2015 +0000
Revision:
36:da07b5c2984d
Parent:
34:c672f5c0763f
Child:
37:23660d12d772
Child:
38:17a1622a27f7
PID Controller + WERKENDE HIDSCOPE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomasBNL 0:40052f5ca77b 1 #include "mbed.h"
ThomasBNL 21:c75210216204 2 #include "HIDScope.h"
ThomasBNL 0:40052f5ca77b 3 #include "QEI.h"
ThomasBNL 0:40052f5ca77b 4 #include "MODSERIAL.h"
ThomasBNL 8:50d6e2323d3b 5 #include "biquadFilter.h"
ThomasBNL 0:40052f5ca77b 6 #include "encoder.h"
ThomasBNL 0:40052f5ca77b 7
ThomasBNL 34:c672f5c0763f 8 // ___________________________
ThomasBNL 34:c672f5c0763f 9 // // \\
ThomasBNL 34:c672f5c0763f 10 // || [Inputs] ||
ThomasBNL 34:c672f5c0763f 11 // \\___________________________//
ThomasBNL 34:c672f5c0763f 12 //
ThomasBNL 34:c672f5c0763f 13
ThomasBNL 34:c672f5c0763f 14
ThomasBNL 24:8ca471562212 15 MODSERIAL pc(USBTX,USBRX);
ThomasBNL 34:c672f5c0763f 16
ThomasBNL 34:c672f5c0763f 17 DigitalOut debug_led_red(LED_RED); // Debug LED
ThomasBNL 34:c672f5c0763f 18 DigitalOut debug_led_green(LED_GREEN); // Debug LED
ThomasBNL 34:c672f5c0763f 19 DigitalOut debug_led_blue(LED_BLUE); // Debug LED
ThomasBNL 34:c672f5c0763f 20
ThomasBNL 34:c672f5c0763f 21 DigitalIn buttonL1(PTC6); // Button 1 (low on k64f) for testing at the lower board
ThomasBNL 34:c672f5c0763f 22 DigitalIn buttonL2(PTA4); // Button 2 (low on k64f) for testing at the lower board
ThomasBNL 34:c672f5c0763f 23 DigitalIn buttonH1(D2); // Button 3 (high on k64f) for testing at the top board
ThomasBNL 34:c672f5c0763f 24 DigitalIn buttonH2(D6); // Button 4 (high on k64f) for testing at the top board
ThomasBNL 34:c672f5c0763f 25
ThomasBNL 34:c672f5c0763f 26 AnalogIn potmeter(A0); // Potmeter that can read a reference value (DEBUG TOOL)
ThomasBNL 34:c672f5c0763f 27
ThomasBNL 34:c672f5c0763f 28 // ___________________________
ThomasBNL 34:c672f5c0763f 29 // // \\
ThomasBNL 34:c672f5c0763f 30 // || [MOTOR TURN] ||
ThomasBNL 34:c672f5c0763f 31 // \\___________________________//
ThomasBNL 34:c672f5c0763f 32 //
ThomasBNL 34:c672f5c0763f 33
ThomasBNL 34:c672f5c0763f 34 QEI motor_turn(D12,D13,NC,32); // Encoder for motor Turn
ThomasBNL 34:c672f5c0763f 35 PwmOut pwm_motor_turn(D5); // Pwm for motor Turn
ThomasBNL 34:c672f5c0763f 36 DigitalOut motordirection_turn(D4); // Direction of the motor Turn
ThomasBNL 34:c672f5c0763f 37
ThomasBNL 34:c672f5c0763f 38 // ___________________________
ThomasBNL 34:c672f5c0763f 39 // // \\
ThomasBNL 34:c672f5c0763f 40 // || [HIDSCOPE] ||
ThomasBNL 34:c672f5c0763f 41 // \\___________________________//
ThomasBNL 34:c672f5c0763f 42 //
ThomasBNL 8:50d6e2323d3b 43
ThomasBNL 21:c75210216204 44 HIDScope scope(2); // HIDSCOPE declared
ThomasBNL 14:599896acf576 45
ThomasBNL 34:c672f5c0763f 46 // ___________________________
ThomasBNL 34:c672f5c0763f 47 // // \\
ThomasBNL 34:c672f5c0763f 48 // || [CONSTANTS] ||
ThomasBNL 34:c672f5c0763f 49 // \\___________________________//
ThomasBNL 34:c672f5c0763f 50 //
ThomasBNL 0:40052f5ca77b 51
ThomasBNL 0:40052f5ca77b 52 volatile bool looptimerflag;
ThomasBNL 8:50d6e2323d3b 53 const double cw=0; // zero is clockwise (front view)
ThomasBNL 8:50d6e2323d3b 54 const double ccw=1; // one is counterclockwise (front view)
ThomasBNL 36:da07b5c2984d 55 const double off=1; //Led off
ThomasBNL 36:da07b5c2984d 56 const double on=0; //Led on
ThomasBNL 34:c672f5c0763f 57 const int Fs = 512; // sampling frequency (512 Hz)
ThomasBNL 36:da07b5c2984d 58 const double sample_time = 0.001953125; // sampling frequency (512 Hz)
ThomasBNL 34:c672f5c0763f 59
ThomasBNL 34:c672f5c0763f 60 // PID motor constants
ThomasBNL 34:c672f5c0763f 61 double integrate_error_turn=0; // integration error turn motor
ThomasBNL 34:c672f5c0763f 62 double previous_error_turn=0; // previous error turn motor
ThomasBNL 8:50d6e2323d3b 63
ThomasBNL 26:b3693f431d6f 64 const double Gain_P_turn=10; //0.0067;
ThomasBNL 8:50d6e2323d3b 65 // stel setpoint tussen (0 en 360) en position tussen (0 en 360)
ThomasBNL 8:50d6e2323d3b 66 // max verschil: 360 -> dan pwm_to_motor 1 tot aan een verschil van 15 graden-> bij 15 moet pwm_to_motor ong 0.1 zijn
ThomasBNL 8:50d6e2323d3b 67 // dus 0.1=15*gain gain=0.0067
ThomasBNL 14:599896acf576 68 // Als 3 graden verschil 0.1 dan 0.1/3=gain=0.33
ThomasBNL 13:bcf8ec7120ab 69
ThomasBNL 34:c672f5c0763f 70 double Gain_I_turn=0.1; //0.025; //(1/2000) //0.00000134
ThomasBNL 13:bcf8ec7120ab 71 // pwm_motor_I=(integrate_error_turn + sample_time*error)*gain; pwm = (4*0.01 + 4)* Gain => 0.1 pwm gewenst (na 1 seconde een verschil van 4 graden)
ThomasBNL 13:bcf8ec7120ab 72 // 0.1 / (4.01) = Gain = 0.025
ThomasBNL 13:bcf8ec7120ab 73
ThomasBNL 34:c672f5c0763f 74 double Gain_D_turn=50; //0.01;
ThomasBNL 13:bcf8ec7120ab 75 // error_derivative_turn=(error - previous_error_turn)/sample_time
ThomasBNL 14:599896acf576 76 //
ThomasBNL 8:50d6e2323d3b 77
ThomasBNL 8:50d6e2323d3b 78 double conversion_counts_to_degrees=0.085877862594198;
ThomasBNL 8:50d6e2323d3b 79 // gear ratio motor = 131
ThomasBNL 8:50d6e2323d3b 80 // ticks per magnet rotation = 32 (X2 Encoder)
ThomasBNL 8:50d6e2323d3b 81 // One revolution = 360 degrees
ThomasBNL 8:50d6e2323d3b 82 // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
ThomasBNL 8:50d6e2323d3b 83
ThomasBNL 8:50d6e2323d3b 84
ThomasBNL 34:c672f5c0763f 85 // ___________________________
ThomasBNL 34:c672f5c0763f 86 // // \\
ThomasBNL 34:c672f5c0763f 87 // || [FUNCTIONS USED] ||
ThomasBNL 34:c672f5c0763f 88 // \\___________________________//
ThomasBNL 34:c672f5c0763f 89 //
ThomasBNL 8:50d6e2323d3b 90
ThomasBNL 7:ddd7fb357786 91 void keep_in_range(double * in, double min, double max);
ThomasBNL 7:ddd7fb357786 92 void setlooptimerflag(void);
ThomasBNL 8:50d6e2323d3b 93 double get_reference(double input);
ThomasBNL 34:c672f5c0763f 94
ThomasBNL 8:50d6e2323d3b 95
ThomasBNL 0:40052f5ca77b 96
ThomasBNL 34:c672f5c0763f 97 ///////////////////////////////
ThomasBNL 34:c672f5c0763f 98 // //
ThomasBNL 34:c672f5c0763f 99 /////////////////////////////////////////////////////// [MAIN FUNCTION] ////////////////////////////////////////////////////////////////////////////
ThomasBNL 34:c672f5c0763f 100 // // //
ThomasBNL 34:c672f5c0763f 101 /////////////////////////////// //
ThomasBNL 0:40052f5ca77b 102 int main() {
ThomasBNL 36:da07b5c2984d 103 debug_led_red=off;
ThomasBNL 36:da07b5c2984d 104 debug_led_blue=off;
ThomasBNL 36:da07b5c2984d 105 debug_led_green=off;
ThomasBNL 36:da07b5c2984d 106
ThomasBNL 8:50d6e2323d3b 107 double position_turn; // Set constant to store current position of the Turn motor
ThomasBNL 8:50d6e2323d3b 108 double error;
ThomasBNL 10:09ba965045a7 109 double pwm_to_motor_turn;
ThomasBNL 10:09ba965045a7 110 double pwm_motor_turn_P;
ThomasBNL 10:09ba965045a7 111 double pwm_motor_turn_I;
ThomasBNL 10:09ba965045a7 112 double pwm_motor_turn_D;
ThomasBNL 36:da07b5c2984d 113 double reference_turn=0; // Set constant to store reference value of the Turn motor
ThomasBNL 0:40052f5ca77b 114
ThomasBNL 7:ddd7fb357786 115 //START OF CODE
ThomasBNL 7:ddd7fb357786 116 pc.printf("Start of code \n\r");
ThomasBNL 0:40052f5ca77b 117
ThomasBNL 23:335a3b843a5e 118 pc.baud(115200); // Set the baudrate
ThomasBNL 0:40052f5ca77b 119
ThomasBNL 7:ddd7fb357786 120 // Tickers
ThomasBNL 8:50d6e2323d3b 121 Ticker looptimer; // Ticker called looptimer to set a looptimerflag
ThomasBNL 34:c672f5c0763f 122 looptimer.attach(setlooptimerflag,(float)1/Fs); // calls the looptimer flag every 0.01s
ThomasBNL 14:599896acf576 123
ThomasBNL 7:ddd7fb357786 124 pc.printf("Start infinite loop \n\r");
ThomasBNL 16:a8d2c721cf56 125 wait (3); // Wait before starting system
ThomasBNL 0:40052f5ca77b 126
ThomasBNL 34:c672f5c0763f 127
ThomasBNL 0:40052f5ca77b 128 //INFINITE LOOP
ThomasBNL 5:8fb74a22fe3c 129 while(1)
ThomasBNL 8:50d6e2323d3b 130 { // Start while loop
ThomasBNL 34:c672f5c0763f 131
ThomasBNL 34:c672f5c0763f 132 // ___________________________
ThomasBNL 34:c672f5c0763f 133 // // \\
ThomasBNL 34:c672f5c0763f 134 // || [DEBUG BUTTONS] ||
ThomasBNL 34:c672f5c0763f 135 // \\___________________________//
ThomasBNL 34:c672f5c0763f 136 // interrupt button Disbalances the current motor position
ThomasBNL 34:c672f5c0763f 137
ThomasBNL 36:da07b5c2984d 138 //if button L2 pressed => disbalance motor
ThomasBNL 34:c672f5c0763f 139 if (buttonL2.read() < 0.5){
ThomasBNL 8:50d6e2323d3b 140 motordirection_turn = cw;
ThomasBNL 34:c672f5c0763f 141 pwm_motor_turn = 0.5f;
ThomasBNL 8:50d6e2323d3b 142 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 34:c672f5c0763f 143
ThomasBNL 8:50d6e2323d3b 144
ThomasBNL 36:da07b5c2984d 145 // if button L1 pressed => shut down motor for 1000 seconds
ThomasBNL 34:c672f5c0763f 146 if (buttonL1.read() < 0.5){
ThomasBNL 22:eaf4cbd1dcec 147 motordirection_turn = cw;
ThomasBNL 22:eaf4cbd1dcec 148 pwm_motor_turn = 0;
ThomasBNL 22:eaf4cbd1dcec 149 wait(1000);
ThomasBNL 22:eaf4cbd1dcec 150 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 17:aa167ab3cf75 151
ThomasBNL 34:c672f5c0763f 152 else
ThomasBNL 8:50d6e2323d3b 153 {
ThomasBNL 34:c672f5c0763f 154 // ___________________________
ThomasBNL 34:c672f5c0763f 155 // // \\
ThomasBNL 34:c672f5c0763f 156 // || [Wait for go signal] ||
ThomasBNL 34:c672f5c0763f 157 // \\___________________________//
ThomasBNL 36:da07b5c2984d 158 // // Wait until looptimer flag is true then execute PID controller loop.
ThomasBNL 34:c672f5c0763f 159
ThomasBNL 0:40052f5ca77b 160 while(looptimerflag != true);
ThomasBNL 0:40052f5ca77b 161
ThomasBNL 0:40052f5ca77b 162 looptimerflag = false;
ThomasBNL 0:40052f5ca77b 163
ThomasBNL 34:c672f5c0763f 164 //reference = (potmeter.read()-0.5)*2000; // Potmeter bepaald reference (uitgeschakeld)
ThomasBNL 0:40052f5ca77b 165
ThomasBNL 34:c672f5c0763f 166 // ___________________________
ThomasBNL 34:c672f5c0763f 167 // // \\
ThomasBNL 34:c672f5c0763f 168 // || [Calibrate position] ||
ThomasBNL 34:c672f5c0763f 169 // \\___________________________//
ThomasBNL 36:da07b5c2984d 170 // // Keep motor position between -4200 and 4200 counts
ThomasBNL 34:c672f5c0763f 171
ThomasBNL 8:50d6e2323d3b 172 if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
ThomasBNL 3:11a7da46e093 173 {
ThomasBNL 8:50d6e2323d3b 174 motor_turn.reset();
ThomasBNL 3:11a7da46e093 175 pc.printf("RESET \n\r");
ThomasBNL 3:11a7da46e093 176 }
ThomasBNL 3:11a7da46e093 177
ThomasBNL 34:c672f5c0763f 178 // Convert position to degrees \\
ThomasBNL 34:c672f5c0763f 179
ThomasBNL 8:50d6e2323d3b 180 position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
ThomasBNL 0:40052f5ca77b 181
ThomasBNL 8:50d6e2323d3b 182 pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
ThomasBNL 3:11a7da46e093 183
ThomasBNL 0:40052f5ca77b 184
ThomasBNL 34:c672f5c0763f 185 // ___________________________
ThomasBNL 34:c672f5c0763f 186 // // \\
ThomasBNL 34:c672f5c0763f 187 // || [PID CONTROLLER] ||
ThomasBNL 34:c672f5c0763f 188 // \\___________________________//
ThomasBNL 34:c672f5c0763f 189 // // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
ThomasBNL 34:c672f5c0763f 190
ThomasBNL 34:c672f5c0763f 191 error=(reference_turn - position_turn); // Current error (input controller)
ThomasBNL 8:50d6e2323d3b 192
ThomasBNL 34:c672f5c0763f 193 integrate_error_turn=integrate_error_turn + sample_time*error; // integral error output
ThomasBNL 34:c672f5c0763f 194 // // overwrite previous integrate error by adding the current error
ThomasBNL 34:c672f5c0763f 195 // multiplied by the sample time.
ThomasBNL 8:50d6e2323d3b 196 //
ThomasBNL 34:c672f5c0763f 197 double error_derivative_turn=(error - previous_error_turn)/sample_time; // derivative error output
ThomasBNL 8:50d6e2323d3b 198
ThomasBNL 34:c672f5c0763f 199 // FILTER error_derivative_turn (lowpassfilter)
ThomasBNL 11:ecd83b303252 200
ThomasBNL 12:26759959c960 201 const double mT_f_a1=-1.965293372622690e+00;
ThomasBNL 12:26759959c960 202 const double mT_f_a2=9.658854605688177e-01;
ThomasBNL 12:26759959c960 203 const double mT_f_b0=1.480219865318266e-04;
ThomasBNL 12:26759959c960 204 const double mT_f_b1=2.960439730636533e-04;
ThomasBNL 12:26759959c960 205 const double mT_f_b2=1.480219865318266e-04; // Motor Turn filter constants
ThomasBNL 11:ecd83b303252 206
ThomasBNL 12:26759959c960 207 biquadFilter Lowpassfilter(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
ThomasBNL 11:ecd83b303252 208
ThomasBNL 12:26759959c960 209 error_derivative_turn=Lowpassfilter.step(error_derivative_turn);
ThomasBNL 3:11a7da46e093 210
ThomasBNL 11:ecd83b303252 211 previous_error_turn=error; // current error is saved to memory constant to be used in
ThomasBNL 8:50d6e2323d3b 212 // the next loop for calculating the derivative error
ThomasBNL 8:50d6e2323d3b 213
ThomasBNL 10:09ba965045a7 214 pwm_to_motor_turn = error*Gain_P_turn; // output P controller to pwm
ThomasBNL 8:50d6e2323d3b 215
ThomasBNL 10:09ba965045a7 216 pwm_motor_turn_P = error*Gain_P_turn; // output P controller to pwm
ThomasBNL 10:09ba965045a7 217 pwm_motor_turn_I = integrate_error_turn*Gain_I_turn; // output I controller to pwm
ThomasBNL 11:ecd83b303252 218 pwm_motor_turn_D = error_derivative_turn*Gain_D_turn; // output D controller to pwm
ThomasBNL 10:09ba965045a7 219
ThomasBNL 13:bcf8ec7120ab 220 pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D;
ThomasBNL 20:bdc62ee49197 221
ThomasBNL 34:c672f5c0763f 222 // ___________________________
ThomasBNL 34:c672f5c0763f 223 // // \\
ThomasBNL 34:c672f5c0763f 224 // || [PID error -> pwm motor] ||
ThomasBNL 34:c672f5c0763f 225 // \\___________________________//
ThomasBNL 36:da07b5c2984d 226 // // Keep Pwm between -1 and 1 -> and decide the direction of the motor to compensate the error
ThomasBNL 34:c672f5c0763f 227
ThomasBNL 10:09ba965045a7 228 keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 10:09ba965045a7 229 pc.printf("pwm %f \n\r", pwm_to_motor_turn);
ThomasBNL 0:40052f5ca77b 230
ThomasBNL 8:50d6e2323d3b 231 // Check error and decide direction to turn
ThomasBNL 10:09ba965045a7 232 if(pwm_to_motor_turn > 0)
ThomasBNL 3:11a7da46e093 233 {
ThomasBNL 8:50d6e2323d3b 234 motordirection_turn=ccw;
ThomasBNL 15:f029351f1f3a 235 pc.printf("if loop pwm > 0 \n\r");
ThomasBNL 3:11a7da46e093 236 }
ThomasBNL 0:40052f5ca77b 237 else
ThomasBNL 3:11a7da46e093 238 {
ThomasBNL 8:50d6e2323d3b 239 motordirection_turn=cw;
ThomasBNL 15:f029351f1f3a 240 pc.printf("else loop pwm < 0 \n\r");
ThomasBNL 3:11a7da46e093 241 }
ThomasBNL 8:50d6e2323d3b 242
ThomasBNL 34:c672f5c0763f 243 // ___________________________
ThomasBNL 34:c672f5c0763f 244 // // \\
ThomasBNL 34:c672f5c0763f 245 // || [pwm -> Plant] ||
ThomasBNL 34:c672f5c0763f 246 // \\___________________________//
ThomasBNL 34:c672f5c0763f 247 // // Put pwm to the motor \\
ThomasBNL 34:c672f5c0763f 248
ThomasBNL 10:09ba965045a7 249 pwm_motor_turn=(abs(pwm_to_motor_turn));
ThomasBNL 14:599896acf576 250
ThomasBNL 34:c672f5c0763f 251 // ___________________________
ThomasBNL 34:c672f5c0763f 252 // // \\
ThomasBNL 34:c672f5c0763f 253 // || [HIDSCOPE] ||
ThomasBNL 34:c672f5c0763f 254 // \\___________________________//
ThomasBNL 34:c672f5c0763f 255 // // Check signals inside HIDSCOPE \\
ThomasBNL 34:c672f5c0763f 256
ThomasBNL 36:da07b5c2984d 257 scope.set(0,error); // HIDSCOPE channel 0 : Current Reference
ThomasBNL 36:da07b5c2984d 258 scope.set(1,position_turn); // HIDSCOPE channel 1 : Position_turn
ThomasBNL 36:da07b5c2984d 259 scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
ThomasBNL 34:c672f5c0763f 260 scope.send(); // Send channel info to HIDSCOPE
ThomasBNL 0:40052f5ca77b 261 }
ThomasBNL 0:40052f5ca77b 262 }
ThomasBNL 5:8fb74a22fe3c 263 }
ThomasBNL 0:40052f5ca77b 264
ThomasBNL 34:c672f5c0763f 265 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 34:c672f5c0763f 266 // //
ThomasBNL 34:c672f5c0763f 267 // [Functions Described] //
ThomasBNL 34:c672f5c0763f 268 // //
ThomasBNL 34:c672f5c0763f 269 ////////////////////////////////////
ThomasBNL 34:c672f5c0763f 270
ThomasBNL 0:40052f5ca77b 271 // Keep in range function
ThomasBNL 0:40052f5ca77b 272 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 273 {
ThomasBNL 0:40052f5ca77b 274 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 275 }
ThomasBNL 0:40052f5ca77b 276
ThomasBNL 0:40052f5ca77b 277 // Looptimerflag function
ThomasBNL 0:40052f5ca77b 278 void setlooptimerflag(void)
ThomasBNL 0:40052f5ca77b 279 {
ThomasBNL 0:40052f5ca77b 280 looptimerflag = true;
ThomasBNL 1:dc683e88b44e 281 }
ThomasBNL 1:dc683e88b44e 282
ThomasBNL 8:50d6e2323d3b 283 // Get setpoint -> potmeter (MOMENTEEL UITGESCHAKELD)
ThomasBNL 8:50d6e2323d3b 284 double get_reference(double input)
ThomasBNL 1:dc683e88b44e 285 {
ThomasBNL 1:dc683e88b44e 286 const float offset = 0.5;
ThomasBNL 1:dc683e88b44e 287 const float gain = 4.0;
ThomasBNL 1:dc683e88b44e 288 return (input-offset)*gain;
ThomasBNL 5:8fb74a22fe3c 289 }