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@41:424264a4c39c, 2015-10-14 (annotated)

Committer:
ThomasBNL
Date:
Wed Oct 14 11:28:09 2015 +0000
Revision:
41:424264a4c39c
Parent:
40:bbe7922723df
Child:
42:0a7898cb3e94
PID CONTROLLER + HIDSCOPE (versie 5) : changed the location where the filters are defined for the derivative step

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