Thomas Burgers / Mbed 2 deprecated ZZ-TheChenneRobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Encoder HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@26:b3693f431d6f, 2015-10-08 (annotated)

Committer:
ThomasBNL
Date:
Thu Oct 08 19:17:56 2015 +0000
Revision:
26:b3693f431d6f
Parent:
25:d536056a2666
Child:
27:b7caf668a682
Child:
34:c672f5c0763f
PID CONTROLLER (versie 1) : gains en filter moeten nog afgesteld worden en netter gedefinieerd worden

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 24:8ca471562212 8 MODSERIAL pc(USBTX,USBRX);
ThomasBNL 24:8ca471562212 9 DigitalOut debug_led_red(LED_RED); // Debug LED
ThomasBNL 24:8ca471562212 10 DigitalOut debug_led_green(LED_GREEN); // Debug LED
ThomasBNL 25:d536056a2666 11 DigitalOut debug_led_blue(LED_BLUE); // Debug LED
ThomasBNL 8:50d6e2323d3b 12
ThomasBNL 21:c75210216204 13 HIDScope scope(2); // HIDSCOPE declared
ThomasBNL 14:599896acf576 14
ThomasBNL 22:eaf4cbd1dcec 15 //Ticker Sample_Ticker; // HIDSCOPE voor main
ThomasBNL 22:eaf4cbd1dcec 16 //volatile bool sample; // HIDSCOPE voor main
ThomasBNL 14:599896acf576 17
ThomasBNL 14:599896acf576 18
ThomasBNL 8:50d6e2323d3b 19 // (DEBUGGING AND TESTING BUTTONS) (0 when pressed and 1 when not pressed)
ThomasBNL 8:50d6e2323d3b 20 DigitalIn buttonL1(PTC6); // Button 1 (laag op board) for testing at the lower board
ThomasBNL 8:50d6e2323d3b 21 DigitalIn buttonL2(PTA4); // Button 2 (laag op board) for testing at the lower board
ThomasBNL 8:50d6e2323d3b 22 DigitalIn buttonH1(D2); // Button 3 (hoog op board) for testing at the top board
ThomasBNL 8:50d6e2323d3b 23 DigitalIn buttonH2(D6); // Button 4 (hoog op board) for testing at the top board
ThomasBNL 0:40052f5ca77b 24
ThomasBNL 0:40052f5ca77b 25 volatile bool looptimerflag;
ThomasBNL 8:50d6e2323d3b 26 const double cw=0; // zero is clockwise (front view)
ThomasBNL 8:50d6e2323d3b 27 const double ccw=1; // one is counterclockwise (front view)
ThomasBNL 8:50d6e2323d3b 28
ThomasBNL 26:b3693f431d6f 29 const double Gain_P_turn=10; //0.0067;
ThomasBNL 8:50d6e2323d3b 30 // stel setpoint tussen (0 en 360) en position tussen (0 en 360)
ThomasBNL 8:50d6e2323d3b 31 // 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 32 // dus 0.1=15*gain gain=0.0067
ThomasBNL 14:599896acf576 33 // Als 3 graden verschil 0.1 dan 0.1/3=gain=0.33
ThomasBNL 13:bcf8ec7120ab 34
ThomasBNL 26:b3693f431d6f 35 double Gain_I_turn=0.1; //0.025; //(1/2000) //0.00000134
ThomasBNL 13:bcf8ec7120ab 36 // 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 37 // 0.1 / (4.01) = Gain = 0.025
ThomasBNL 13:bcf8ec7120ab 38
ThomasBNL 26:b3693f431d6f 39 double Gain_D_turn=50; //0.01;
ThomasBNL 13:bcf8ec7120ab 40 // error_derivative_turn=(error - previous_error_turn)/sample_time
ThomasBNL 14:599896acf576 41 //
ThomasBNL 8:50d6e2323d3b 42
ThomasBNL 8:50d6e2323d3b 43 double conversion_counts_to_degrees=0.085877862594198;
ThomasBNL 8:50d6e2323d3b 44 // gear ratio motor = 131
ThomasBNL 8:50d6e2323d3b 45 // ticks per magnet rotation = 32 (X2 Encoder)
ThomasBNL 8:50d6e2323d3b 46 // One revolution = 360 degrees
ThomasBNL 8:50d6e2323d3b 47 // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
ThomasBNL 8:50d6e2323d3b 48
ThomasBNL 18:6c065915f474 49
ThomasBNL 8:50d6e2323d3b 50 const double sample_time=0.01; // tijd voor een sample (100Hz)
ThomasBNL 8:50d6e2323d3b 51
ThomasBNL 8:50d6e2323d3b 52 // PID motor constants
ThomasBNL 8:50d6e2323d3b 53 double integrate_error_turn=0; // integration error turn motor
ThomasBNL 8:50d6e2323d3b 54 double previous_error_turn=0; // previous error turn motor
ThomasBNL 8:50d6e2323d3b 55
ThomasBNL 1:dc683e88b44e 56
ThomasBNL 7:ddd7fb357786 57 // Functions used (described after main)
ThomasBNL 7:ddd7fb357786 58 void keep_in_range(double * in, double min, double max);
ThomasBNL 7:ddd7fb357786 59 void setlooptimerflag(void);
ThomasBNL 8:50d6e2323d3b 60 double get_reference(double input);
ThomasBNL 22:eaf4cbd1dcec 61 //void get_sample(void); //HIDSCOPE
ThomasBNL 8:50d6e2323d3b 62
ThomasBNL 0:40052f5ca77b 63
ThomasBNL 7:ddd7fb357786 64 // MAIN function
ThomasBNL 0:40052f5ca77b 65 int main() {
ThomasBNL 25:d536056a2666 66 debug_led_red=1;
ThomasBNL 25:d536056a2666 67 debug_led_blue=1;
ThomasBNL 25:d536056a2666 68 debug_led_green=1;
ThomasBNL 8:50d6e2323d3b 69 AnalogIn potmeter(A0); // Potmeter that can read a reference value (DEBUG TOOL)
ThomasBNL 8:50d6e2323d3b 70 QEI motor_turn(D12,D13,NC,32); // Encoder for motor Turn
ThomasBNL 8:50d6e2323d3b 71 PwmOut pwm_motor_turn(D5); // Pwm for motor Turn
ThomasBNL 8:50d6e2323d3b 72 DigitalOut motordirection_turn(D4); // Direction of the motor Turn
ThomasBNL 18:6c065915f474 73 double reference_turn=0; // Set constant to store reference value of the Turn motor
ThomasBNL 8:50d6e2323d3b 74 double position_turn; // Set constant to store current position of the Turn motor
ThomasBNL 8:50d6e2323d3b 75 double error;
ThomasBNL 10:09ba965045a7 76 double pwm_to_motor_turn;
ThomasBNL 10:09ba965045a7 77 double pwm_motor_turn_P;
ThomasBNL 10:09ba965045a7 78 double pwm_motor_turn_I;
ThomasBNL 10:09ba965045a7 79 double pwm_motor_turn_D;
ThomasBNL 0:40052f5ca77b 80
ThomasBNL 7:ddd7fb357786 81 //START OF CODE
ThomasBNL 7:ddd7fb357786 82 pc.printf("Start of code \n\r");
ThomasBNL 0:40052f5ca77b 83
ThomasBNL 23:335a3b843a5e 84 pc.baud(115200); // Set the baudrate
ThomasBNL 0:40052f5ca77b 85
ThomasBNL 7:ddd7fb357786 86 // Tickers
ThomasBNL 8:50d6e2323d3b 87 Ticker looptimer; // Ticker called looptimer to set a looptimerflag
ThomasBNL 8:50d6e2323d3b 88 looptimer.attach(setlooptimerflag,sample_time); // calls the looptimer flag every 0.01s
ThomasBNL 0:40052f5ca77b 89
ThomasBNL 22:eaf4cbd1dcec 90 //Sample_Ticker.attach(&get_sample, sample_time); // HIDSCOPE sample Ticker
ThomasBNL 14:599896acf576 91
ThomasBNL 7:ddd7fb357786 92 pc.printf("Start infinite loop \n\r");
ThomasBNL 16:a8d2c721cf56 93 wait (3); // Wait before starting system
ThomasBNL 0:40052f5ca77b 94
ThomasBNL 0:40052f5ca77b 95 //INFINITE LOOP
ThomasBNL 5:8fb74a22fe3c 96 while(1)
ThomasBNL 8:50d6e2323d3b 97 { // Start while loop
ThomasBNL 8:50d6e2323d3b 98 // DEBUGGING BUTTON: interrupt button Disbalances the current motor position
ThomasBNL 8:50d6e2323d3b 99 if (buttonL2.read() < 0.5){ //if button pressed
ThomasBNL 8:50d6e2323d3b 100 motordirection_turn = cw;
ThomasBNL 8:50d6e2323d3b 101 pwm_motor_turn = 0.5f; // motorspeed
ThomasBNL 8:50d6e2323d3b 102 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 8:50d6e2323d3b 103
ThomasBNL 22:eaf4cbd1dcec 104 // // Change Reference button Positive
ThomasBNL 22:eaf4cbd1dcec 105 // if (buttonH1.read() < 0.5){ //if button pressed
ThomasBNL 22:eaf4cbd1dcec 106 // pc.printf("Reference after = %d \r\n", reference_turn);
ThomasBNL 22:eaf4cbd1dcec 107 // reference_turn=reference_turn+45;
ThomasBNL 22:eaf4cbd1dcec 108 // pc.printf("Reference after = %d \r\n", reference_turn);
ThomasBNL 22:eaf4cbd1dcec 109 // debug_led = !debug_led; }
ThomasBNL 22:eaf4cbd1dcec 110 //
ThomasBNL 22:eaf4cbd1dcec 111 // // Change Reference button Negative
ThomasBNL 22:eaf4cbd1dcec 112 // if (buttonH2.read() < 0.5){ //if button pressed
ThomasBNL 22:eaf4cbd1dcec 113 // pc.printf("Reference after = %d \r\n", reference_turn);
ThomasBNL 22:eaf4cbd1dcec 114 // reference_turn=reference_turn-45;
ThomasBNL 22:eaf4cbd1dcec 115 // pc.printf("Reference after = %d \r\n", reference_turn);
ThomasBNL 22:eaf4cbd1dcec 116 // debug_led = !debug_led; }
ThomasBNL 22:eaf4cbd1dcec 117
ThomasBNL 22:eaf4cbd1dcec 118 if (buttonL1.read() < 0.5){ //if button pressed
ThomasBNL 22:eaf4cbd1dcec 119 motordirection_turn = cw;
ThomasBNL 22:eaf4cbd1dcec 120 pwm_motor_turn = 0;
ThomasBNL 22:eaf4cbd1dcec 121 wait(1000);
ThomasBNL 22:eaf4cbd1dcec 122 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 17:aa167ab3cf75 123
ThomasBNL 8:50d6e2323d3b 124 // Wait until looptimer flag is true then execute PID controller.
ThomasBNL 5:8fb74a22fe3c 125 else
ThomasBNL 8:50d6e2323d3b 126 {
ThomasBNL 0:40052f5ca77b 127 while(looptimerflag != true);
ThomasBNL 0:40052f5ca77b 128
ThomasBNL 0:40052f5ca77b 129 looptimerflag = false;
ThomasBNL 0:40052f5ca77b 130
ThomasBNL 8:50d6e2323d3b 131 //reference = (potmeter.read()-0.5)*2000; // Potmeter bepaald reference (uitgeschakeld)
ThomasBNL 17:aa167ab3cf75 132 //reference_turn = 15; //BOVENAAN IN SCRIPT GEPLAATST
ThomasBNL 0:40052f5ca77b 133
ThomasBNL 8:50d6e2323d3b 134 // Keep motor position between -4200 and 4200 counts
ThomasBNL 8:50d6e2323d3b 135 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 136 {
ThomasBNL 8:50d6e2323d3b 137 motor_turn.reset();
ThomasBNL 3:11a7da46e093 138 pc.printf("RESET \n\r");
ThomasBNL 3:11a7da46e093 139 }
ThomasBNL 3:11a7da46e093 140
ThomasBNL 8:50d6e2323d3b 141 // Convert position to degrees
ThomasBNL 8:50d6e2323d3b 142 position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
ThomasBNL 0:40052f5ca77b 143
ThomasBNL 8:50d6e2323d3b 144 pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
ThomasBNL 3:11a7da46e093 145
ThomasBNL 0:40052f5ca77b 146
ThomasBNL 8:50d6e2323d3b 147 // P-CONTROLLER
ThomasBNL 8:50d6e2323d3b 148 // Calculate error then multiply it with the gain, and store in pwm_to_motor
ThomasBNL 8:50d6e2323d3b 149
ThomasBNL 10:09ba965045a7 150 error=(reference_turn - position_turn); // Current error (input controller)
ThomasBNL 8:50d6e2323d3b 151
ThomasBNL 10:09ba965045a7 152 integrate_error_turn=integrate_error_turn + sample_time*error; // integral error output
ThomasBNL 8:50d6e2323d3b 153 // // overwrite previous integrate error by adding the current error multiplied by the sample time.
ThomasBNL 8:50d6e2323d3b 154 //
ThomasBNL 11:ecd83b303252 155 double error_derivative_turn=(error - previous_error_turn)/sample_time; // derivative error output
ThomasBNL 8:50d6e2323d3b 156
ThomasBNL 8:50d6e2323d3b 157 // FILTER error_derivative_turn (lowpassfilter)
ThomasBNL 11:ecd83b303252 158
ThomasBNL 12:26759959c960 159 const double mT_f_a1=-1.965293372622690e+00;
ThomasBNL 12:26759959c960 160 const double mT_f_a2=9.658854605688177e-01;
ThomasBNL 12:26759959c960 161 const double mT_f_b0=1.480219865318266e-04;
ThomasBNL 12:26759959c960 162 const double mT_f_b1=2.960439730636533e-04;
ThomasBNL 12:26759959c960 163 const double mT_f_b2=1.480219865318266e-04; // Motor Turn filter constants
ThomasBNL 11:ecd83b303252 164
ThomasBNL 12:26759959c960 165 biquadFilter Lowpassfilter(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
ThomasBNL 11:ecd83b303252 166
ThomasBNL 12:26759959c960 167 error_derivative_turn=Lowpassfilter.step(error_derivative_turn);
ThomasBNL 3:11a7da46e093 168
ThomasBNL 11:ecd83b303252 169 previous_error_turn=error; // current error is saved to memory constant to be used in
ThomasBNL 8:50d6e2323d3b 170 // the next loop for calculating the derivative error
ThomasBNL 8:50d6e2323d3b 171
ThomasBNL 10:09ba965045a7 172 pwm_to_motor_turn = error*Gain_P_turn; // output P controller to pwm
ThomasBNL 8:50d6e2323d3b 173
ThomasBNL 10:09ba965045a7 174 pwm_motor_turn_P = error*Gain_P_turn; // output P controller to pwm
ThomasBNL 10:09ba965045a7 175 pwm_motor_turn_I = integrate_error_turn*Gain_I_turn; // output I controller to pwm
ThomasBNL 11:ecd83b303252 176 pwm_motor_turn_D = error_derivative_turn*Gain_D_turn; // output D controller to pwm
ThomasBNL 10:09ba965045a7 177
ThomasBNL 13:bcf8ec7120ab 178 pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D;
ThomasBNL 20:bdc62ee49197 179
ThomasBNL 0:40052f5ca77b 180
ThomasBNL 8:50d6e2323d3b 181 // Keep Pwm between -1 and 1
ThomasBNL 10:09ba965045a7 182 keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 10:09ba965045a7 183 pc.printf("pwm %f \n\r", pwm_to_motor_turn);
ThomasBNL 0:40052f5ca77b 184
ThomasBNL 8:50d6e2323d3b 185 // Check error and decide direction to turn
ThomasBNL 10:09ba965045a7 186 if(pwm_to_motor_turn > 0)
ThomasBNL 3:11a7da46e093 187 {
ThomasBNL 8:50d6e2323d3b 188 motordirection_turn=ccw;
ThomasBNL 15:f029351f1f3a 189 pc.printf("if loop pwm > 0 \n\r");
ThomasBNL 3:11a7da46e093 190 }
ThomasBNL 0:40052f5ca77b 191 else
ThomasBNL 3:11a7da46e093 192 {
ThomasBNL 8:50d6e2323d3b 193 motordirection_turn=cw;
ThomasBNL 15:f029351f1f3a 194 pc.printf("else loop pwm < 0 \n\r");
ThomasBNL 3:11a7da46e093 195 }
ThomasBNL 8:50d6e2323d3b 196
ThomasBNL 8:50d6e2323d3b 197 // Put pwm_motor to the motor
ThomasBNL 10:09ba965045a7 198 pwm_motor_turn=(abs(pwm_to_motor_turn));
ThomasBNL 14:599896acf576 199
ThomasBNL 22:eaf4cbd1dcec 200 // while(sample != true) // HIDSCOPE input => sample_go nu nog niet nodig opzich // BLINK LEDS TOEVOEGEN
ThomasBNL 22:eaf4cbd1dcec 201 // {
ThomasBNL 21:c75210216204 202 //sample_filter; (filter function zie EMG filter working script)
ThomasBNL 21:c75210216204 203 scope.set(0,reference_turn); // HIDSCOPE channel 0 : Current Reference
ThomasBNL 21:c75210216204 204 scope.set(0,position_turn); // HIDSCOPE channel 0 : Position_turn
ThomasBNL 21:c75210216204 205 scope.set(1,pwm_to_motor_turn); // HIDSCOPE channel 1 : Pwm_to_motor_turn
ThomasBNL 21:c75210216204 206 scope.send(); // Send channel info to HIDSCOPE
ThomasBNL 22:eaf4cbd1dcec 207 // sample = false;
ThomasBNL 22:eaf4cbd1dcec 208 // }
ThomasBNL 22:eaf4cbd1dcec 209 //debug_led = !debug_led; // should flicker with freq 50 Hz
ThomasBNL 0:40052f5ca77b 210 }
ThomasBNL 0:40052f5ca77b 211 }
ThomasBNL 5:8fb74a22fe3c 212 }
ThomasBNL 0:40052f5ca77b 213
ThomasBNL 0:40052f5ca77b 214 // Keep in range function
ThomasBNL 0:40052f5ca77b 215 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 216 {
ThomasBNL 0:40052f5ca77b 217 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 218 }
ThomasBNL 0:40052f5ca77b 219
ThomasBNL 0:40052f5ca77b 220 // Looptimerflag function
ThomasBNL 0:40052f5ca77b 221 void setlooptimerflag(void)
ThomasBNL 0:40052f5ca77b 222 {
ThomasBNL 0:40052f5ca77b 223 looptimerflag = true;
ThomasBNL 1:dc683e88b44e 224 }
ThomasBNL 1:dc683e88b44e 225
ThomasBNL 8:50d6e2323d3b 226 // Get setpoint -> potmeter (MOMENTEEL UITGESCHAKELD)
ThomasBNL 8:50d6e2323d3b 227 double get_reference(double input)
ThomasBNL 1:dc683e88b44e 228 {
ThomasBNL 1:dc683e88b44e 229 const float offset = 0.5;
ThomasBNL 1:dc683e88b44e 230 const float gain = 4.0;
ThomasBNL 1:dc683e88b44e 231 return (input-offset)*gain;
ThomasBNL 5:8fb74a22fe3c 232 }
ThomasBNL 14:599896acf576 233
ThomasBNL 22:eaf4cbd1dcec 234 //// Get sample
ThomasBNL 22:eaf4cbd1dcec 235 //void get_sample(void) // HIDSCOPE sample fuction
ThomasBNL 22:eaf4cbd1dcec 236 //{
ThomasBNL 22:eaf4cbd1dcec 237 // sample = true;
ThomasBNL 22:eaf4cbd1dcec 238 //}