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@40:bbe7922723df, 2015-10-14 (annotated)

Committer:
ThomasBNL
Date:
Wed Oct 14 11:19:58 2015 +0000
Revision:
40:bbe7922723df
Parent:
39:104a038f7b92
Child:
41:424264a4c39c
PID CONTROLLER + HIDSCOPE (versie 4): added deactivor that deactivates the PID gains (the plant) if the error is small enough to prevent the motor from spinning

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