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@37:23660d12d772, 2015-10-14 (annotated)

Committer:
ThomasBNL
Date:
Wed Oct 14 05:35:11 2015 +0000
Revision:
37:23660d12d772
Parent:
36:da07b5c2984d 
TEST pid in FUNCTIE : getPulses lijkt niet te werken in de functie

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