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@42:0a7898cb3e94, 2015-10-14 (annotated)

Committer:
ThomasBNL
Date:
Wed Oct 14 11:39:13 2015 +0000
Revision:
42:0a7898cb3e94
Parent:
41:424264a4c39c
Child:
43:fb69ef657f30
PID Controller + HIDSCOPE (versie 7): specified a PID activator for the turn and strike motor (needed if different P, I, D, gain values) and added some comments

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 38:17a1622a27f7 64 double P_gain_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 38:17a1622a27f7 70 double I_gain_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 38:17a1622a27f7 74 double D_gain_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 41:424264a4c39c 84 // ___________________________
ThomasBNL 41:424264a4c39c 85 // // \\
ThomasBNL 41:424264a4c39c 86 // || [FILTER CONSTANTS] ||
ThomasBNL 41:424264a4c39c 87 // \\___________________________//
ThomasBNL 41:424264a4c39c 88 //
ThomasBNL 41:424264a4c39c 89
ThomasBNL 41:424264a4c39c 90 const double mT_f_a1=-1.965293372622690e+00;
ThomasBNL 41:424264a4c39c 91 const double mT_f_a2=9.658854605688177e-01;
ThomasBNL 41:424264a4c39c 92 const double mT_f_b0=1.480219865318266e-04;
ThomasBNL 41:424264a4c39c 93 const double mT_f_b1=2.960439730636533e-04;
ThomasBNL 41:424264a4c39c 94 const double mT_f_b2=1.480219865318266e-04; // Motor Turn filter constants
ThomasBNL 41:424264a4c39c 95
ThomasBNL 41:424264a4c39c 96 biquadFilter Lowpassfilter_derivative(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2);
ThomasBNL 8:50d6e2323d3b 97
ThomasBNL 34:c672f5c0763f 98 // ___________________________
ThomasBNL 34:c672f5c0763f 99 // // \\
ThomasBNL 34:c672f5c0763f 100 // || [FUNCTIONS USED] ||
ThomasBNL 34:c672f5c0763f 101 // \\___________________________//
ThomasBNL 34:c672f5c0763f 102 //
ThomasBNL 8:50d6e2323d3b 103
ThomasBNL 7:ddd7fb357786 104 void keep_in_range(double * in, double min, double max);
ThomasBNL 7:ddd7fb357786 105 void setlooptimerflag(void);
ThomasBNL 8:50d6e2323d3b 106 double get_reference(double input);
ThomasBNL 39:104a038f7b92 107
ThomasBNL 42:0a7898cb3e94 108 // STRIKE: Deactivate PID Controller
ThomasBNL 42:0a7898cb3e94 109 void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain);
ThomasBNL 42:0a7898cb3e94 110 // TURN: Activate PID Controller
ThomasBNL 42:0a7898cb3e94 111 void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain);
ThomasBNL 42:0a7898cb3e94 112 // STRIKE: Activate PID Controller
ThomasBNL 42:0a7898cb3e94 113 void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain);
ThomasBNL 8:50d6e2323d3b 114
ThomasBNL 0:40052f5ca77b 115
ThomasBNL 39:104a038f7b92 116 // Change reference position (two functions)
ThomasBNL 39:104a038f7b92 117 void Change_Turn_Position_Left (double& reference, double change_one_bottle);
ThomasBNL 39:104a038f7b92 118
ThomasBNL 39:104a038f7b92 119 void Change_Turn_Position_Right (double& reference, double change_one_bottle);
ThomasBNL 39:104a038f7b92 120
ThomasBNL 34:c672f5c0763f 121 ///////////////////////////////
ThomasBNL 34:c672f5c0763f 122 // //
ThomasBNL 34:c672f5c0763f 123 /////////////////////////////////////////////////////// [MAIN FUNCTION] ////////////////////////////////////////////////////////////////////////////
ThomasBNL 34:c672f5c0763f 124 // // //
ThomasBNL 34:c672f5c0763f 125 /////////////////////////////// //
ThomasBNL 0:40052f5ca77b 126 int main() {
ThomasBNL 36:da07b5c2984d 127 debug_led_red=off;
ThomasBNL 36:da07b5c2984d 128 debug_led_blue=off;
ThomasBNL 36:da07b5c2984d 129 debug_led_green=off;
ThomasBNL 36:da07b5c2984d 130
ThomasBNL 39:104a038f7b92 131 const double change_one_bottle=45;
ThomasBNL 8:50d6e2323d3b 132 double position_turn; // Set constant to store current position of the Turn motor
ThomasBNL 42:0a7898cb3e94 133 double error_turn;
ThomasBNL 10:09ba965045a7 134 double pwm_to_motor_turn;
ThomasBNL 10:09ba965045a7 135 double pwm_motor_turn_P;
ThomasBNL 10:09ba965045a7 136 double pwm_motor_turn_I;
ThomasBNL 10:09ba965045a7 137 double pwm_motor_turn_D;
ThomasBNL 36:da07b5c2984d 138 double reference_turn=0; // Set constant to store reference value of the Turn motor
ThomasBNL 0:40052f5ca77b 139
ThomasBNL 40:bbe7922723df 140 // CALIBRATE (STARTING POSITION)
ThomasBNL 40:bbe7922723df 141
ThomasBNL 40:bbe7922723df 142
ThomasBNL 39:104a038f7b92 143 // TEST: Does change PID gains work? => works correctly
ThomasBNL 42:0a7898cb3e94 144 deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn); // Deactivate Turn motor
ThomasBNL 38:17a1622a27f7 145 pc.printf("P = %d I = %d D = %d \n\r");
ThomasBNL 42:0a7898cb3e94 146 activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);
ThomasBNL 38:17a1622a27f7 147 pc.printf("P = %d I = %d D = %d \n\r");
ThomasBNL 38:17a1622a27f7 148
ThomasBNL 39:104a038f7b92 149 // TEST: Does change reference function work? => still needs to be tested
ThomasBNL 39:104a038f7b92 150 pc.printf("initial reference_turn = %d \n\r", reference_turn);
ThomasBNL 39:104a038f7b92 151 Change_Turn_Position_Left (reference_turn, change_one_bottle);
ThomasBNL 39:104a038f7b92 152 pc.printf("After left turn reference_turn = %d \n\r", reference_turn);
ThomasBNL 39:104a038f7b92 153 Change_Turn_Position_Right (reference_turn, change_one_bottle);
ThomasBNL 39:104a038f7b92 154 pc.printf("After right turn reference_turn = %d \n\r", reference_turn);
ThomasBNL 39:104a038f7b92 155 Change_Turn_Position_Right (reference_turn, change_one_bottle);
ThomasBNL 39:104a038f7b92 156 Change_Turn_Position_Right (reference_turn, change_one_bottle);
ThomasBNL 39:104a038f7b92 157 Change_Turn_Position_Right (reference_turn, change_one_bottle);
ThomasBNL 39:104a038f7b92 158 Change_Turn_Position_Right (reference_turn, change_one_bottle);
ThomasBNL 39:104a038f7b92 159 pc.printf("After right turn reference_turn = %d \n\r", reference_turn); // DOES THE LIMITER WORK? (should be -90 degrees)
ThomasBNL 39:104a038f7b92 160
ThomasBNL 7:ddd7fb357786 161 //START OF CODE
ThomasBNL 7:ddd7fb357786 162 pc.printf("Start of code \n\r");
ThomasBNL 0:40052f5ca77b 163
ThomasBNL 23:335a3b843a5e 164 pc.baud(115200); // Set the baudrate
ThomasBNL 0:40052f5ca77b 165
ThomasBNL 7:ddd7fb357786 166 // Tickers
ThomasBNL 8:50d6e2323d3b 167 Ticker looptimer; // Ticker called looptimer to set a looptimerflag
ThomasBNL 34:c672f5c0763f 168 looptimer.attach(setlooptimerflag,(float)1/Fs); // calls the looptimer flag every 0.01s
ThomasBNL 14:599896acf576 169
ThomasBNL 7:ddd7fb357786 170 pc.printf("Start infinite loop \n\r");
ThomasBNL 16:a8d2c721cf56 171 wait (3); // Wait before starting system
ThomasBNL 0:40052f5ca77b 172
ThomasBNL 34:c672f5c0763f 173
ThomasBNL 0:40052f5ca77b 174 //INFINITE LOOP
ThomasBNL 5:8fb74a22fe3c 175 while(1)
ThomasBNL 8:50d6e2323d3b 176 { // Start while loop
ThomasBNL 34:c672f5c0763f 177
ThomasBNL 34:c672f5c0763f 178 // ___________________________
ThomasBNL 34:c672f5c0763f 179 // // \\
ThomasBNL 34:c672f5c0763f 180 // || [DEBUG BUTTONS] ||
ThomasBNL 34:c672f5c0763f 181 // \\___________________________//
ThomasBNL 34:c672f5c0763f 182 // interrupt button Disbalances the current motor position
ThomasBNL 34:c672f5c0763f 183
ThomasBNL 36:da07b5c2984d 184 //if button L2 pressed => disbalance motor
ThomasBNL 34:c672f5c0763f 185 if (buttonL2.read() < 0.5){
ThomasBNL 8:50d6e2323d3b 186 motordirection_turn = cw;
ThomasBNL 34:c672f5c0763f 187 pwm_motor_turn = 0.5f;
ThomasBNL 8:50d6e2323d3b 188 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 34:c672f5c0763f 189
ThomasBNL 8:50d6e2323d3b 190
ThomasBNL 36:da07b5c2984d 191 // if button L1 pressed => shut down motor for 1000 seconds
ThomasBNL 34:c672f5c0763f 192 if (buttonL1.read() < 0.5){
ThomasBNL 22:eaf4cbd1dcec 193 motordirection_turn = cw;
ThomasBNL 22:eaf4cbd1dcec 194 pwm_motor_turn = 0;
ThomasBNL 22:eaf4cbd1dcec 195 wait(1000);
ThomasBNL 22:eaf4cbd1dcec 196 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 17:aa167ab3cf75 197
ThomasBNL 34:c672f5c0763f 198 else
ThomasBNL 8:50d6e2323d3b 199 {
ThomasBNL 34:c672f5c0763f 200 // ___________________________
ThomasBNL 34:c672f5c0763f 201 // // \\
ThomasBNL 34:c672f5c0763f 202 // || [Wait for go signal] ||
ThomasBNL 34:c672f5c0763f 203 // \\___________________________//
ThomasBNL 36:da07b5c2984d 204 // // Wait until looptimer flag is true then execute PID controller loop.
ThomasBNL 34:c672f5c0763f 205
ThomasBNL 0:40052f5ca77b 206 while(looptimerflag != true);
ThomasBNL 0:40052f5ca77b 207
ThomasBNL 0:40052f5ca77b 208 looptimerflag = false;
ThomasBNL 0:40052f5ca77b 209
ThomasBNL 34:c672f5c0763f 210 //reference = (potmeter.read()-0.5)*2000; // Potmeter bepaald reference (uitgeschakeld)
ThomasBNL 0:40052f5ca77b 211
ThomasBNL 34:c672f5c0763f 212 // ___________________________
ThomasBNL 34:c672f5c0763f 213 // // \\
ThomasBNL 34:c672f5c0763f 214 // || [Calibrate position] ||
ThomasBNL 34:c672f5c0763f 215 // \\___________________________//
ThomasBNL 36:da07b5c2984d 216 // // Keep motor position between -4200 and 4200 counts
ThomasBNL 34:c672f5c0763f 217
ThomasBNL 8:50d6e2323d3b 218 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 219 {
ThomasBNL 8:50d6e2323d3b 220 motor_turn.reset();
ThomasBNL 3:11a7da46e093 221 pc.printf("RESET \n\r");
ThomasBNL 3:11a7da46e093 222 }
ThomasBNL 3:11a7da46e093 223
ThomasBNL 34:c672f5c0763f 224 // Convert position to degrees \\
ThomasBNL 34:c672f5c0763f 225
ThomasBNL 8:50d6e2323d3b 226 position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
ThomasBNL 0:40052f5ca77b 227
ThomasBNL 8:50d6e2323d3b 228 pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
ThomasBNL 3:11a7da46e093 229
ThomasBNL 0:40052f5ca77b 230
ThomasBNL 34:c672f5c0763f 231 // ___________________________
ThomasBNL 34:c672f5c0763f 232 // // \\
ThomasBNL 34:c672f5c0763f 233 // || [PID CONTROLLER] ||
ThomasBNL 34:c672f5c0763f 234 // \\___________________________//
ThomasBNL 34:c672f5c0763f 235 // // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
ThomasBNL 34:c672f5c0763f 236
ThomasBNL 42:0a7898cb3e94 237 error_turn=(reference_turn - position_turn); // TURN: Current error (input controller)
ThomasBNL 42:0a7898cb3e94 238 integrate_error_turn=integrate_error_turn + sample_time*error_turn; // TURN: integral error output
ThomasBNL 34:c672f5c0763f 239 // // overwrite previous integrate error by adding the current error
ThomasBNL 34:c672f5c0763f 240 // multiplied by the sample time.
ThomasBNL 8:50d6e2323d3b 241 //
ThomasBNL 42:0a7898cb3e94 242 double error_derivative_turn=(error_turn - previous_error_turn)/sample_time; // TURN: derivative error output
ThomasBNL 41:424264a4c39c 243 error_derivative_turn=Lowpassfilter_derivative.step(error_derivative_turn); // TURN: Filter
ThomasBNL 41:424264a4c39c 244 error_derivative_turn=Lowpassfilter_derivative.step(error_derivative_turn);
ThomasBNL 3:11a7da46e093 245
ThomasBNL 42:0a7898cb3e94 246 previous_error_turn=error_turn; // current error is saved to memory constant to be used in
ThomasBNL 8:50d6e2323d3b 247 // the next loop for calculating the derivative error
ThomasBNL 8:50d6e2323d3b 248
ThomasBNL 42:0a7898cb3e94 249 pwm_to_motor_turn = error_turn*P_gain_turn; // output P controller to pwm
ThomasBNL 8:50d6e2323d3b 250
ThomasBNL 42:0a7898cb3e94 251 pwm_motor_turn_P = error_turn*P_gain_turn; // output P controller to pwm
ThomasBNL 38:17a1622a27f7 252 pwm_motor_turn_I = integrate_error_turn*I_gain_turn; // output I controller to pwm
ThomasBNL 38:17a1622a27f7 253 pwm_motor_turn_D = error_derivative_turn*D_gain_turn; // output D controller to pwm
ThomasBNL 10:09ba965045a7 254
ThomasBNL 13:bcf8ec7120ab 255 pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D;
ThomasBNL 20:bdc62ee49197 256
ThomasBNL 34:c672f5c0763f 257 // ___________________________
ThomasBNL 34:c672f5c0763f 258 // // \\
ThomasBNL 34:c672f5c0763f 259 // || [PID error -> pwm motor] ||
ThomasBNL 34:c672f5c0763f 260 // \\___________________________//
ThomasBNL 36:da07b5c2984d 261 // // Keep Pwm between -1 and 1 -> and decide the direction of the motor to compensate the error
ThomasBNL 34:c672f5c0763f 262
ThomasBNL 10:09ba965045a7 263 keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 10:09ba965045a7 264 pc.printf("pwm %f \n\r", pwm_to_motor_turn);
ThomasBNL 0:40052f5ca77b 265
ThomasBNL 8:50d6e2323d3b 266 // Check error and decide direction to turn
ThomasBNL 10:09ba965045a7 267 if(pwm_to_motor_turn > 0)
ThomasBNL 3:11a7da46e093 268 {
ThomasBNL 8:50d6e2323d3b 269 motordirection_turn=ccw;
ThomasBNL 15:f029351f1f3a 270 pc.printf("if loop pwm > 0 \n\r");
ThomasBNL 3:11a7da46e093 271 }
ThomasBNL 0:40052f5ca77b 272 else
ThomasBNL 3:11a7da46e093 273 {
ThomasBNL 8:50d6e2323d3b 274 motordirection_turn=cw;
ThomasBNL 15:f029351f1f3a 275 pc.printf("else loop pwm < 0 \n\r");
ThomasBNL 3:11a7da46e093 276 }
ThomasBNL 8:50d6e2323d3b 277
ThomasBNL 34:c672f5c0763f 278 // ___________________________
ThomasBNL 34:c672f5c0763f 279 // // \\
ThomasBNL 34:c672f5c0763f 280 // || [pwm -> Plant] ||
ThomasBNL 34:c672f5c0763f 281 // \\___________________________//
ThomasBNL 34:c672f5c0763f 282 // // Put pwm to the motor \\
ThomasBNL 34:c672f5c0763f 283
ThomasBNL 40:bbe7922723df 284 pwm_motor_turn=(abs(pwm_to_motor_turn));
ThomasBNL 14:599896acf576 285
ThomasBNL 34:c672f5c0763f 286 // ___________________________
ThomasBNL 34:c672f5c0763f 287 // // \\
ThomasBNL 34:c672f5c0763f 288 // || [HIDSCOPE] ||
ThomasBNL 34:c672f5c0763f 289 // \\___________________________//
ThomasBNL 34:c672f5c0763f 290 // // Check signals inside HIDSCOPE \\
ThomasBNL 34:c672f5c0763f 291
ThomasBNL 42:0a7898cb3e94 292 scope.set(0,error_turn); // HIDSCOPE channel 0 : Current Error
ThomasBNL 36:da07b5c2984d 293 scope.set(1,position_turn); // HIDSCOPE channel 1 : Position_turn
ThomasBNL 36:da07b5c2984d 294 scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
ThomasBNL 34:c672f5c0763f 295 scope.send(); // Send channel info to HIDSCOPE
ThomasBNL 40:bbe7922723df 296
ThomasBNL 40:bbe7922723df 297 // ___________________________
ThomasBNL 40:bbe7922723df 298 // // \\
ThomasBNL 40:bbe7922723df 299 // || [Deactivate?] ||
ThomasBNL 40:bbe7922723df 300 // \\___________________________//
ThomasBNL 41:424264a4c39c 301 // // Check whether the motor has reached reference position and can be shut down
ThomasBNL 40:bbe7922723df 302
ThomasBNL 42:0a7898cb3e94 303 if(fabs(error_turn)<2) // Shut down if error is smaller than two degrees
ThomasBNL 42:0a7898cb3e94 304 {deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn);}
ThomasBNL 40:bbe7922723df 305 else
ThomasBNL 42:0a7898cb3e94 306 {activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);}
ThomasBNL 0:40052f5ca77b 307 }
ThomasBNL 0:40052f5ca77b 308 }
ThomasBNL 5:8fb74a22fe3c 309 }
ThomasBNL 0:40052f5ca77b 310
ThomasBNL 34:c672f5c0763f 311 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 34:c672f5c0763f 312 // //
ThomasBNL 34:c672f5c0763f 313 // [Functions Described] //
ThomasBNL 34:c672f5c0763f 314 // //
ThomasBNL 34:c672f5c0763f 315 ////////////////////////////////////
ThomasBNL 34:c672f5c0763f 316
ThomasBNL 0:40052f5ca77b 317 // Keep in range function
ThomasBNL 0:40052f5ca77b 318 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 319 {
ThomasBNL 0:40052f5ca77b 320 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 321 }
ThomasBNL 0:40052f5ca77b 322
ThomasBNL 0:40052f5ca77b 323 // Looptimerflag function
ThomasBNL 0:40052f5ca77b 324 void setlooptimerflag(void)
ThomasBNL 0:40052f5ca77b 325 {
ThomasBNL 0:40052f5ca77b 326 looptimerflag = true;
ThomasBNL 1:dc683e88b44e 327 }
ThomasBNL 1:dc683e88b44e 328
ThomasBNL 8:50d6e2323d3b 329 // Get setpoint -> potmeter (MOMENTEEL UITGESCHAKELD)
ThomasBNL 8:50d6e2323d3b 330 double get_reference(double input)
ThomasBNL 1:dc683e88b44e 331 {
ThomasBNL 1:dc683e88b44e 332 const float offset = 0.5;
ThomasBNL 1:dc683e88b44e 333 const float gain = 4.0;
ThomasBNL 1:dc683e88b44e 334 return (input-offset)*gain;
ThomasBNL 5:8fb74a22fe3c 335 }
ThomasBNL 39:104a038f7b92 336
ThomasBNL 39:104a038f7b92 337 // Change reference
ThomasBNL 39:104a038f7b92 338 void Change_Turn_Position_Left (double& reference, double change_one_bottle)
ThomasBNL 39:104a038f7b92 339 {
ThomasBNL 39:104a038f7b92 340 reference = reference + change_one_bottle;
ThomasBNL 39:104a038f7b92 341 keep_in_range(&reference, -90, 90); // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
ThomasBNL 39:104a038f7b92 342 }
ThomasBNL 39:104a038f7b92 343
ThomasBNL 39:104a038f7b92 344 void Change_Turn_Position_Right (double& reference, double change_one_bottle)
ThomasBNL 39:104a038f7b92 345 {
ThomasBNL 39:104a038f7b92 346 reference = reference - change_one_bottle;
ThomasBNL 39:104a038f7b92 347 keep_in_range(&reference, -90, 90);
ThomasBNL 39:104a038f7b92 348 }
ThomasBNL 39:104a038f7b92 349
ThomasBNL 42:0a7898cb3e94 350 // Deactivate or Activate PID_Controller
ThomasBNL 42:0a7898cb3e94 351 void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 352 {
ThomasBNL 42:0a7898cb3e94 353 P_gain=0;
ThomasBNL 42:0a7898cb3e94 354 I_gain=0;
ThomasBNL 42:0a7898cb3e94 355 D_gain=0;
ThomasBNL 42:0a7898cb3e94 356 }
ThomasBNL 42:0a7898cb3e94 357
ThomasBNL 42:0a7898cb3e94 358 void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 359 {
ThomasBNL 42:0a7898cb3e94 360 P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
ThomasBNL 42:0a7898cb3e94 361 I_gain=0;
ThomasBNL 42:0a7898cb3e94 362 D_gain=0;
ThomasBNL 42:0a7898cb3e94 363 }
ThomasBNL 42:0a7898cb3e94 364
ThomasBNL 42:0a7898cb3e94 365 void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 366 {
ThomasBNL 42:0a7898cb3e94 367 P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
ThomasBNL 42:0a7898cb3e94 368 I_gain=0;
ThomasBNL 42:0a7898cb3e94 369 D_gain=0;
ThomasBNL 42:0a7898cb3e94 370 }