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@45:359df0594588, 2015-10-14 (annotated)

Committer:
ThomasBNL
Date:
Wed Oct 14 18:57:27 2015 +0000
Revision:
45:359df0594588
Parent:
44:5dd0a3d24662
Child:
46:9279c7a725bf
PID controller (hidscope?/werkend?): added reference if at boundary bottle and try to go to the side where no bottles are; than immediatly turn 5 bottles to the left

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomasBNL 43:fb69ef657f30 1 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 2 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 3 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 4 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 5 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 6 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 7 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 8 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 9 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 10 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 11 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 43:fb69ef657f30 12 // ___________________________
ThomasBNL 43:fb69ef657f30 13 // // \\
ThomasBNL 43:fb69ef657f30 14 // || [Libraries] ||
ThomasBNL 43:fb69ef657f30 15 // \\___________________________//
ThomasBNL 43:fb69ef657f30 16 //
ThomasBNL 43:fb69ef657f30 17
ThomasBNL 0:40052f5ca77b 18 #include "mbed.h"
ThomasBNL 21:c75210216204 19 #include "HIDScope.h"
ThomasBNL 0:40052f5ca77b 20 #include "QEI.h"
ThomasBNL 0:40052f5ca77b 21 #include "MODSERIAL.h"
ThomasBNL 8:50d6e2323d3b 22 #include "biquadFilter.h"
ThomasBNL 0:40052f5ca77b 23 #include "encoder.h"
ThomasBNL 0:40052f5ca77b 24
ThomasBNL 34:c672f5c0763f 25 // ___________________________
ThomasBNL 34:c672f5c0763f 26 // // \\
ThomasBNL 34:c672f5c0763f 27 // || [Inputs] ||
ThomasBNL 34:c672f5c0763f 28 // \\___________________________//
ThomasBNL 34:c672f5c0763f 29 //
ThomasBNL 34:c672f5c0763f 30
ThomasBNL 34:c672f5c0763f 31
ThomasBNL 24:8ca471562212 32 MODSERIAL pc(USBTX,USBRX);
ThomasBNL 34:c672f5c0763f 33
ThomasBNL 34:c672f5c0763f 34 DigitalOut debug_led_red(LED_RED); // Debug LED
ThomasBNL 34:c672f5c0763f 35 DigitalOut debug_led_green(LED_GREEN); // Debug LED
ThomasBNL 34:c672f5c0763f 36 DigitalOut debug_led_blue(LED_BLUE); // Debug LED
ThomasBNL 34:c672f5c0763f 37
ThomasBNL 43:fb69ef657f30 38 DigitalIn buttonL1(PTC6); // Button 1 (low on k64f) for testing at the lower board
ThomasBNL 43:fb69ef657f30 39 DigitalIn buttonL2(PTA4); // Button 2 (low on k64f) for testing at the lower board
ThomasBNL 43:fb69ef657f30 40 DigitalIn buttonH1(D2); // Button 3 (high on k64f) for testing at the top board
ThomasBNL 43:fb69ef657f30 41 DigitalIn buttonH2(D6); // Button 4 (high on k64f) for testing at the top board
ThomasBNL 34:c672f5c0763f 42
ThomasBNL 43:fb69ef657f30 43 AnalogIn potmeter(A0); // Potmeter that can read a reference value (DEBUG TOOL)
ThomasBNL 34:c672f5c0763f 44
ThomasBNL 44:5dd0a3d24662 45 Ticker looptimer; // Ticker called looptimer to set a looptimerflag
ThomasBNL 44:5dd0a3d24662 46
ThomasBNL 34:c672f5c0763f 47 // ___________________________
ThomasBNL 34:c672f5c0763f 48 // // \\
ThomasBNL 34:c672f5c0763f 49 // || [MOTOR TURN] ||
ThomasBNL 34:c672f5c0763f 50 // \\___________________________//
ThomasBNL 34:c672f5c0763f 51 //
ThomasBNL 34:c672f5c0763f 52
ThomasBNL 43:fb69ef657f30 53 QEI motor_turn(D12,D13,NC,32); // TURN: Encoder for motor
ThomasBNL 43:fb69ef657f30 54 PwmOut pwm_motor_turn(D5); // TURN: Pwm for motor
ThomasBNL 43:fb69ef657f30 55 DigitalOut motordirection_turn(D4); // TURN: Direction of the motor
ThomasBNL 43:fb69ef657f30 56
ThomasBNL 43:fb69ef657f30 57 // PID motor constants
ThomasBNL 43:fb69ef657f30 58
ThomasBNL 43:fb69ef657f30 59 double integrate_error_turn=0; // integration error turn motor
ThomasBNL 43:fb69ef657f30 60 double previous_error_turn=0; // previous error turn motor
ThomasBNL 43:fb69ef657f30 61
ThomasBNL 43:fb69ef657f30 62 // ___________________________
ThomasBNL 43:fb69ef657f30 63 // // \\
ThomasBNL 43:fb69ef657f30 64 // || [MOTOR STRIKE] ||
ThomasBNL 43:fb69ef657f30 65 // \\___________________________//
ThomasBNL 43:fb69ef657f30 66
ThomasBNL 43:fb69ef657f30 67 //QEI motor_strike(XX,XX,NC,32); // STRIKE: Encoder for motor Turn
ThomasBNL 43:fb69ef657f30 68 //PwmOut pwm_motor_strike(XX); // STRIKE: Pwm for motor Turn
ThomasBNL 43:fb69ef657f30 69 //DigitalOut motordirection_strike(XX); // STRIKE: Direction of the motor Turn
ThomasBNL 43:fb69ef657f30 70
ThomasBNL 43:fb69ef657f30 71 // PID motor constants
ThomasBNL 43:fb69ef657f30 72 //double integrate_error_strike=0; // STRIKE: integration error turn motor
ThomasBNL 43:fb69ef657f30 73 //double previous_error_strike=0; // STRIKE: previous error turn motor
ThomasBNL 34:c672f5c0763f 74
ThomasBNL 34:c672f5c0763f 75 // ___________________________
ThomasBNL 34:c672f5c0763f 76 // // \\
ThomasBNL 34:c672f5c0763f 77 // || [HIDSCOPE] ||
ThomasBNL 34:c672f5c0763f 78 // \\___________________________//
ThomasBNL 34:c672f5c0763f 79 //
ThomasBNL 8:50d6e2323d3b 80
ThomasBNL 43:fb69ef657f30 81 HIDScope scope(3); // HIDSCOPE declared
ThomasBNL 14:599896acf576 82
ThomasBNL 34:c672f5c0763f 83 // ___________________________
ThomasBNL 34:c672f5c0763f 84 // // \\
ThomasBNL 43:fb69ef657f30 85 // || [System CONSTANTS] ||
ThomasBNL 34:c672f5c0763f 86 // \\___________________________//
ThomasBNL 34:c672f5c0763f 87 //
ThomasBNL 0:40052f5ca77b 88
ThomasBNL 0:40052f5ca77b 89 volatile bool looptimerflag;
ThomasBNL 43:fb69ef657f30 90 const double cw=0; // zero is clockwise (front view)
ThomasBNL 43:fb69ef657f30 91 const double ccw=1; // one is counterclockwise (front view)
ThomasBNL 43:fb69ef657f30 92 const double off=1; // Led off
ThomasBNL 43:fb69ef657f30 93 const double on=0; // Led on
ThomasBNL 43:fb69ef657f30 94 const int Fs = 512; // sampling frequency (512 Hz)
ThomasBNL 43:fb69ef657f30 95 const double sample_time = 0.001953125; // duration of one sample
ThomasBNL 44:5dd0a3d24662 96 double conversion_counts_to_degrees=0.085877862594198; // Calculation conversion counts to degrees
ThomasBNL 44:5dd0a3d24662 97 // gear ratio motor = 131
ThomasBNL 44:5dd0a3d24662 98 // ticks per magnet rotation = 32 (X2 Encoder)
ThomasBNL 44:5dd0a3d24662 99 // One revolution = 360 degrees
ThomasBNL 44:5dd0a3d24662 100 // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
ThomasBNL 44:5dd0a3d24662 101
ThomasBNL 8:50d6e2323d3b 102
ThomasBNL 41:424264a4c39c 103 // ___________________________
ThomasBNL 41:424264a4c39c 104 // // \\
ThomasBNL 41:424264a4c39c 105 // || [FILTER CONSTANTS] ||
ThomasBNL 41:424264a4c39c 106 // \\___________________________//
ThomasBNL 41:424264a4c39c 107 //
ThomasBNL 41:424264a4c39c 108
ThomasBNL 43:fb69ef657f30 109 const double mT_f_a1=-1.965293372622690e+00; // Motor Turn derivative filter constants
ThomasBNL 41:424264a4c39c 110 const double mT_f_a2=9.658854605688177e-01;
ThomasBNL 41:424264a4c39c 111 const double mT_f_b0=1.480219865318266e-04;
ThomasBNL 41:424264a4c39c 112 const double mT_f_b1=2.960439730636533e-04;
ThomasBNL 43:fb69ef657f30 113 const double mT_f_b2=1.480219865318266e-04;
ThomasBNL 41:424264a4c39c 114
ThomasBNL 43:fb69ef657f30 115 biquadFilter Lowpassfilter_derivative(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
ThomasBNL 8:50d6e2323d3b 116
ThomasBNL 34:c672f5c0763f 117 // ___________________________
ThomasBNL 34:c672f5c0763f 118 // // \\
ThomasBNL 34:c672f5c0763f 119 // || [FUNCTIONS USED] ||
ThomasBNL 34:c672f5c0763f 120 // \\___________________________//
ThomasBNL 34:c672f5c0763f 121 //
ThomasBNL 43:fb69ef657f30 122 void keep_in_range(double * in, double min, double max); // Put in a value and makes sure that the value stays between assigned boundary's
ThomasBNL 43:fb69ef657f30 123 void setlooptimerflag(void); // Sets looptimerflag volatile bool to true
ThomasBNL 8:50d6e2323d3b 124
ThomasBNL 43:fb69ef657f30 125 void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain); // STRIKE/TURN: Deactivate PID Controller
ThomasBNL 39:104a038f7b92 126
ThomasBNL 43:fb69ef657f30 127 void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain); // STRIKE: Activate PID Controller
ThomasBNL 8:50d6e2323d3b 128
ThomasBNL 43:fb69ef657f30 129 void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain); // TURN: Activate PID Controller
ThomasBNL 43:fb69ef657f30 130 void Change_Turn_Position_Left (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the left
ThomasBNL 43:fb69ef657f30 131 void Change_Turn_Position_Right (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the right
ThomasBNL 0:40052f5ca77b 132
ThomasBNL 39:104a038f7b92 133
ThomasBNL 34:c672f5c0763f 134 ///////////////////////////////
ThomasBNL 34:c672f5c0763f 135 // //
ThomasBNL 34:c672f5c0763f 136 /////////////////////////////////////////////////////// [MAIN FUNCTION] ////////////////////////////////////////////////////////////////////////////
ThomasBNL 34:c672f5c0763f 137 // // //
ThomasBNL 34:c672f5c0763f 138 /////////////////////////////// //
ThomasBNL 43:fb69ef657f30 139
ThomasBNL 0:40052f5ca77b 140 int main() {
ThomasBNL 43:fb69ef657f30 141
ThomasBNL 36:da07b5c2984d 142 debug_led_red=off;
ThomasBNL 36:da07b5c2984d 143 debug_led_blue=off;
ThomasBNL 36:da07b5c2984d 144 debug_led_green=off;
ThomasBNL 36:da07b5c2984d 145
ThomasBNL 45:359df0594588 146 const double change_one_bottle=45;
ThomasBNL 45:359df0594588 147 double position_turn; // TURN: Set variable to store current position of the motor
ThomasBNL 45:359df0594588 148 double error_turn; // TURN: Set variable to store the current error of the motor
ThomasBNL 45:359df0594588 149
ThomasBNL 45:359df0594588 150 double pwm_motor_turn_P; // TURN: variable that stores the P action part of the error
ThomasBNL 45:359df0594588 151 double pwm_motor_turn_I; // TURN: variable that stores the I action part of the error
ThomasBNL 45:359df0594588 152 double pwm_motor_turn_D; // TURN: variable that stores the D action part of the error
ThomasBNL 45:359df0594588 153 double pwm_to_motor_turn; // TURN: variable that is constructed by summing the values of the P, I and D action
ThomasBNL 45:359df0594588 154
ThomasBNL 45:359df0594588 155 double P_gain_turn; // TURN: this gain gets multiplied with the error inside the P action of the PID controller
ThomasBNL 45:359df0594588 156 double I_gain_turn; // TURN: this gain gets multiplied with the error inside the I action of the PID controller
ThomasBNL 45:359df0594588 157 double D_gain_turn; // TURN: this gain gets multiplied with the error inside the D action of the PID controller
ThomasBNL 44:5dd0a3d24662 158
ThomasBNL 45:359df0594588 159 double reference_turn; // TURN: reference position of the motor (position the motor 'likes' to get to)
ThomasBNL 45:359df0594588 160
ThomasBNL 45:359df0594588 161 //double position_strike; // TURN: Set variable to store current position of the motor
ThomasBNL 45:359df0594588 162 //double error_strike; // TURN: Set variable to store the current error of the motor
ThomasBNL 45:359df0594588 163
ThomasBNL 45:359df0594588 164 //double pwm_motor_strike_P; // STRIKE: variable that stores the P action part of the error
ThomasBNL 45:359df0594588 165 //double pwm_motor_strike_I; // STRIKE: variable that stores the I action part of the error
ThomasBNL 45:359df0594588 166 //double pwm_motor_strike_D; // STRIKE: variable that stores the D action part of the error
ThomasBNL 45:359df0594588 167 //double pwm_to_motor_strike; // STRIKE: variable that is constructed by summing the values of the P, I and D action
ThomasBNL 45:359df0594588 168 //double reference_strike;
ThomasBNL 44:5dd0a3d24662 169
ThomasBNL 45:359df0594588 170 //double P_gain_turn; // STRIKE: this gain gets multiplied with the error inside the P action of the PID controller
ThomasBNL 45:359df0594588 171 //double I_gain_turn; // STRIKE: this gain gets multiplied with the error inside the I action of the PID controller
ThomasBNL 45:359df0594588 172 //double D_gain_turn; // STRIKE: this gain gets multiplied with the error inside the D action of the PID controller
ThomasBNL 45:359df0594588 173
ThomasBNL 45:359df0594588 174 //double reference_strike; // STRIKE: reference position of the motor (position the motor 'likes' to get to)
ThomasBNL 45:359df0594588 175
ThomasBNL 43:fb69ef657f30 176 // ___________________________
ThomasBNL 43:fb69ef657f30 177 // // \\
ThomasBNL 43:fb69ef657f30 178 // || [START OF CODE] ||
ThomasBNL 43:fb69ef657f30 179 // \\___________________________//
ThomasBNL 43:fb69ef657f30 180 // START OF CODE
ThomasBNL 43:fb69ef657f30 181 pc.printf("Start of code \n\r");
ThomasBNL 43:fb69ef657f30 182
ThomasBNL 43:fb69ef657f30 183 pc.baud(115200); // Set the baudrate
ThomasBNL 0:40052f5ca77b 184
ThomasBNL 43:fb69ef657f30 185 // ___________________________
ThomasBNL 43:fb69ef657f30 186 // // \\
ThomasBNL 43:fb69ef657f30 187 // || [CALIBRATE] ||
ThomasBNL 43:fb69ef657f30 188 // \\___________________________//
ThomasBNL 43:fb69ef657f30 189 // Calibrate starting postion (RED LED)
ThomasBNL 43:fb69ef657f30 190
ThomasBNL 43:fb69ef657f30 191 pc.printf("Button calibration \n\r");
ThomasBNL 43:fb69ef657f30 192 while(1)
ThomasBNL 43:fb69ef657f30 193 {
ThomasBNL 43:fb69ef657f30 194 debug_led_red=on; // TURN: LOW buttons
ThomasBNL 43:fb69ef657f30 195
ThomasBNL 43:fb69ef657f30 196 if (buttonL2.read() < 0.5){ // TURN: turn the motor clockwise with pwm of 0.2
ThomasBNL 43:fb69ef657f30 197 motordirection_turn = cw;
ThomasBNL 43:fb69ef657f30 198 pwm_motor_turn = 0.2f;}
ThomasBNL 43:fb69ef657f30 199
ThomasBNL 43:fb69ef657f30 200 if (buttonL1.read() < 0.5){ // TURN: turn the motor counterclockwise with pwm of 0.2
ThomasBNL 43:fb69ef657f30 201 motordirection_turn = ccw;
ThomasBNL 43:fb69ef657f30 202 pwm_motor_turn = 0.2f;}
ThomasBNL 43:fb69ef657f30 203 // STRIKE: HIGH buttons
ThomasBNL 43:fb69ef657f30 204
ThomasBNL 44:5dd0a3d24662 205 // if (buttonH2.read() < 0.5){ // STRIKE: turn the motor clockwise with pwm of 0.2
ThomasBNL 44:5dd0a3d24662 206 // motordirection_strike = cw;
ThomasBNL 44:5dd0a3d24662 207 // pwm_motor_turn = 0.2f;}
ThomasBNL 44:5dd0a3d24662 208 //
ThomasBNL 44:5dd0a3d24662 209 // if (buttonH1.read() < 0.5){ // STRIKE: turn the motor clockwise with pwm of 0.2
ThomasBNL 44:5dd0a3d24662 210 // motordirection_strike = ccw;
ThomasBNL 44:5dd0a3d24662 211 // pwm_motor_turn = 0.2f;}
ThomasBNL 44:5dd0a3d24662 212 //
ThomasBNL 45:359df0594588 213 if ((buttonL2.read() < 0.5) && (buttonL1.read() < 0.5)) // Save current TURN and STRIKE positions as starting position
ThomasBNL 43:fb69ef657f30 214 {
ThomasBNL 43:fb69ef657f30 215 motor_turn.reset(); // TURN: Starting Position
ThomasBNL 44:5dd0a3d24662 216 reference_turn=0; // TURN: Set reference position to zero
ThomasBNL 44:5dd0a3d24662 217 // motor_strike.reset(); // STRIKE: Starting Position
ThomasBNL 44:5dd0a3d24662 218 // double reference_strike=0; // STRIKE: Set reference position to zero
ThomasBNL 43:fb69ef657f30 219 goto calibration_complete; // Calibration complete
ThomasBNL 43:fb69ef657f30 220 }
ThomasBNL 43:fb69ef657f30 221 }
ThomasBNL 43:fb69ef657f30 222
ThomasBNL 44:5dd0a3d24662 223
ThomasBNL 43:fb69ef657f30 224 calibration_complete:
ThomasBNL 44:5dd0a3d24662 225 debug_led_red=off; // Calibration end => RED LED off
ThomasBNL 40:bbe7922723df 226
ThomasBNL 40:bbe7922723df 227
ThomasBNL 45:359df0594588 228 // ___________________________
ThomasBNL 45:359df0594588 229 // // \\
ThomasBNL 45:359df0594588 230 // || [ATTACH TICKER] ||
ThomasBNL 45:359df0594588 231 // \\___________________________//
ThomasBNL 45:359df0594588 232 // Attach Ticker to looptimerflag
ThomasBNL 45:359df0594588 233
ThomasBNL 45:359df0594588 234 looptimer.attach(setlooptimerflag,(float)1/Fs); // calls the looptimer flag every sample
ThomasBNL 7:ddd7fb357786 235 pc.printf("Start infinite loop \n\r");
ThomasBNL 45:359df0594588 236
ThomasBNL 45:359df0594588 237 wait (5); // Wait 5 seconds before starting system
ThomasBNL 0:40052f5ca77b 238
ThomasBNL 44:5dd0a3d24662 239 activate_PID_Controller_strike(P_gain_turn, I_gain_turn, D_gain_turn);
ThomasBNL 34:c672f5c0763f 240
ThomasBNL 44:5dd0a3d24662 241 // ___________________________
ThomasBNL 44:5dd0a3d24662 242 // // \\
ThomasBNL 44:5dd0a3d24662 243 // || [START INFINTTE LOOP] ||
ThomasBNL 44:5dd0a3d24662 244 // \\___________________________//
ThomasBNL 44:5dd0a3d24662 245 //
ThomasBNL 44:5dd0a3d24662 246
ThomasBNL 5:8fb74a22fe3c 247 while(1)
ThomasBNL 8:50d6e2323d3b 248 { // Start while loop
ThomasBNL 34:c672f5c0763f 249
ThomasBNL 34:c672f5c0763f 250 // ___________________________
ThomasBNL 34:c672f5c0763f 251 // // \\
ThomasBNL 34:c672f5c0763f 252 // || [DEBUG BUTTONS] ||
ThomasBNL 34:c672f5c0763f 253 // \\___________________________//
ThomasBNL 34:c672f5c0763f 254 // interrupt button Disbalances the current motor position
ThomasBNL 34:c672f5c0763f 255
ThomasBNL 44:5dd0a3d24662 256 //if button L2 pressed => disbalance motor
ThomasBNL 34:c672f5c0763f 257 if (buttonL2.read() < 0.5){
ThomasBNL 8:50d6e2323d3b 258 motordirection_turn = cw;
ThomasBNL 34:c672f5c0763f 259 pwm_motor_turn = 0.5f;
ThomasBNL 8:50d6e2323d3b 260 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 34:c672f5c0763f 261
ThomasBNL 8:50d6e2323d3b 262
ThomasBNL 44:5dd0a3d24662 263 // if button L1 pressed => shut down motor for 1000 seconds
ThomasBNL 34:c672f5c0763f 264 if (buttonL1.read() < 0.5){
ThomasBNL 22:eaf4cbd1dcec 265 motordirection_turn = cw;
ThomasBNL 22:eaf4cbd1dcec 266 pwm_motor_turn = 0;
ThomasBNL 22:eaf4cbd1dcec 267 wait(1000);
ThomasBNL 22:eaf4cbd1dcec 268 pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
ThomasBNL 17:aa167ab3cf75 269
ThomasBNL 34:c672f5c0763f 270 else
ThomasBNL 8:50d6e2323d3b 271 {
ThomasBNL 34:c672f5c0763f 272 // ___________________________
ThomasBNL 34:c672f5c0763f 273 // // \\
ThomasBNL 34:c672f5c0763f 274 // || [Wait for go signal] ||
ThomasBNL 34:c672f5c0763f 275 // \\___________________________//
ThomasBNL 36:da07b5c2984d 276 // // Wait until looptimer flag is true then execute PID controller loop.
ThomasBNL 34:c672f5c0763f 277
ThomasBNL 0:40052f5ca77b 278 while(looptimerflag != true);
ThomasBNL 0:40052f5ca77b 279
ThomasBNL 0:40052f5ca77b 280 looptimerflag = false;
ThomasBNL 44:5dd0a3d24662 281
ThomasBNL 44:5dd0a3d24662 282
ThomasBNL 34:c672f5c0763f 283 // ___________________________
ThomasBNL 34:c672f5c0763f 284 // // \\
ThomasBNL 34:c672f5c0763f 285 // || [Calibrate position] ||
ThomasBNL 34:c672f5c0763f 286 // \\___________________________//
ThomasBNL 36:da07b5c2984d 287 // // Keep motor position between -4200 and 4200 counts
ThomasBNL 34:c672f5c0763f 288
ThomasBNL 8:50d6e2323d3b 289 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 290 {
ThomasBNL 8:50d6e2323d3b 291 motor_turn.reset();
ThomasBNL 3:11a7da46e093 292 pc.printf("RESET \n\r");
ThomasBNL 3:11a7da46e093 293 }
ThomasBNL 3:11a7da46e093 294
ThomasBNL 34:c672f5c0763f 295 // Convert position to degrees \\
ThomasBNL 34:c672f5c0763f 296
ThomasBNL 8:50d6e2323d3b 297 position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
ThomasBNL 0:40052f5ca77b 298
ThomasBNL 8:50d6e2323d3b 299 pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
ThomasBNL 3:11a7da46e093 300
ThomasBNL 0:40052f5ca77b 301
ThomasBNL 34:c672f5c0763f 302 // ___________________________
ThomasBNL 34:c672f5c0763f 303 // // \\
ThomasBNL 34:c672f5c0763f 304 // || [PID CONTROLLER] ||
ThomasBNL 34:c672f5c0763f 305 // \\___________________________//
ThomasBNL 34:c672f5c0763f 306 // // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
ThomasBNL 34:c672f5c0763f 307
ThomasBNL 42:0a7898cb3e94 308 error_turn=(reference_turn - position_turn); // TURN: Current error (input controller)
ThomasBNL 44:5dd0a3d24662 309 integrate_error_turn=integrate_error_turn + sample_time*error_turn; // TURN: Integral error output
ThomasBNL 44:5dd0a3d24662 310 // overwrite previous integrate error by adding the current error
ThomasBNL 34:c672f5c0763f 311 // multiplied by the sample time.
ThomasBNL 8:50d6e2323d3b 312 //
ThomasBNL 44:5dd0a3d24662 313 double error_derivative_turn=(error_turn - previous_error_turn)/sample_time; // TURN: Derivative error output
ThomasBNL 44:5dd0a3d24662 314 error_derivative_turn=Lowpassfilter_derivative.step(error_derivative_turn); // TURN: Filter
ThomasBNL 3:11a7da46e093 315
ThomasBNL 44:5dd0a3d24662 316 previous_error_turn=error_turn; // current error is saved to memory constant to be used in
ThomasBNL 44:5dd0a3d24662 317 // the next loop for calculating the derivative error
ThomasBNL 44:5dd0a3d24662 318
ThomasBNL 44:5dd0a3d24662 319 pwm_motor_turn_P = error_turn*P_gain_turn; // Output P controller to pwm
ThomasBNL 44:5dd0a3d24662 320 pwm_motor_turn_I = integrate_error_turn*I_gain_turn; // Output I controller to pwm
ThomasBNL 44:5dd0a3d24662 321 pwm_motor_turn_D = error_derivative_turn*D_gain_turn; // Output D controller to pwm
ThomasBNL 10:09ba965045a7 322
ThomasBNL 13:bcf8ec7120ab 323 pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D;
ThomasBNL 20:bdc62ee49197 324
ThomasBNL 34:c672f5c0763f 325 // ___________________________
ThomasBNL 34:c672f5c0763f 326 // // \\
ThomasBNL 34:c672f5c0763f 327 // || [PID error -> pwm motor] ||
ThomasBNL 34:c672f5c0763f 328 // \\___________________________//
ThomasBNL 36:da07b5c2984d 329 // // Keep Pwm between -1 and 1 -> and decide the direction of the motor to compensate the error
ThomasBNL 34:c672f5c0763f 330
ThomasBNL 10:09ba965045a7 331 keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 10:09ba965045a7 332 pc.printf("pwm %f \n\r", pwm_to_motor_turn);
ThomasBNL 0:40052f5ca77b 333
ThomasBNL 8:50d6e2323d3b 334 // Check error and decide direction to turn
ThomasBNL 10:09ba965045a7 335 if(pwm_to_motor_turn > 0)
ThomasBNL 3:11a7da46e093 336 {
ThomasBNL 8:50d6e2323d3b 337 motordirection_turn=ccw;
ThomasBNL 15:f029351f1f3a 338 pc.printf("if loop pwm > 0 \n\r");
ThomasBNL 3:11a7da46e093 339 }
ThomasBNL 0:40052f5ca77b 340 else
ThomasBNL 3:11a7da46e093 341 {
ThomasBNL 8:50d6e2323d3b 342 motordirection_turn=cw;
ThomasBNL 15:f029351f1f3a 343 pc.printf("else loop pwm < 0 \n\r");
ThomasBNL 3:11a7da46e093 344 }
ThomasBNL 8:50d6e2323d3b 345
ThomasBNL 34:c672f5c0763f 346 // ___________________________
ThomasBNL 34:c672f5c0763f 347 // // \\
ThomasBNL 34:c672f5c0763f 348 // || [pwm -> Plant] ||
ThomasBNL 34:c672f5c0763f 349 // \\___________________________//
ThomasBNL 34:c672f5c0763f 350 // // Put pwm to the motor \\
ThomasBNL 34:c672f5c0763f 351
ThomasBNL 40:bbe7922723df 352 pwm_motor_turn=(abs(pwm_to_motor_turn));
ThomasBNL 14:599896acf576 353
ThomasBNL 34:c672f5c0763f 354 // ___________________________
ThomasBNL 34:c672f5c0763f 355 // // \\
ThomasBNL 34:c672f5c0763f 356 // || [HIDSCOPE] ||
ThomasBNL 34:c672f5c0763f 357 // \\___________________________//
ThomasBNL 34:c672f5c0763f 358 // // Check signals inside HIDSCOPE \\
ThomasBNL 34:c672f5c0763f 359
ThomasBNL 42:0a7898cb3e94 360 scope.set(0,error_turn); // HIDSCOPE channel 0 : Current Error
ThomasBNL 36:da07b5c2984d 361 scope.set(1,position_turn); // HIDSCOPE channel 1 : Position_turn
ThomasBNL 36:da07b5c2984d 362 scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
ThomasBNL 34:c672f5c0763f 363 scope.send(); // Send channel info to HIDSCOPE
ThomasBNL 40:bbe7922723df 364
ThomasBNL 40:bbe7922723df 365 // ___________________________
ThomasBNL 40:bbe7922723df 366 // // \\
ThomasBNL 40:bbe7922723df 367 // || [Deactivate?] ||
ThomasBNL 40:bbe7922723df 368 // \\___________________________//
ThomasBNL 41:424264a4c39c 369 // // Check whether the motor has reached reference position and can be shut down
ThomasBNL 40:bbe7922723df 370
ThomasBNL 42:0a7898cb3e94 371 if(fabs(error_turn)<2) // Shut down if error is smaller than two degrees
ThomasBNL 42:0a7898cb3e94 372 {deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn);}
ThomasBNL 40:bbe7922723df 373 else
ThomasBNL 42:0a7898cb3e94 374 {activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);}
ThomasBNL 0:40052f5ca77b 375 }
ThomasBNL 0:40052f5ca77b 376 }
ThomasBNL 5:8fb74a22fe3c 377 }
ThomasBNL 0:40052f5ca77b 378
ThomasBNL 34:c672f5c0763f 379 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ThomasBNL 34:c672f5c0763f 380 // //
ThomasBNL 34:c672f5c0763f 381 // [Functions Described] //
ThomasBNL 34:c672f5c0763f 382 // //
ThomasBNL 34:c672f5c0763f 383 ////////////////////////////////////
ThomasBNL 34:c672f5c0763f 384
ThomasBNL 0:40052f5ca77b 385 // Keep in range function
ThomasBNL 0:40052f5ca77b 386 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 387 {
ThomasBNL 0:40052f5ca77b 388 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 389 }
ThomasBNL 0:40052f5ca77b 390
ThomasBNL 0:40052f5ca77b 391 // Looptimerflag function
ThomasBNL 0:40052f5ca77b 392 void setlooptimerflag(void)
ThomasBNL 0:40052f5ca77b 393 {
ThomasBNL 0:40052f5ca77b 394 looptimerflag = true;
ThomasBNL 1:dc683e88b44e 395 }
ThomasBNL 1:dc683e88b44e 396
ThomasBNL 39:104a038f7b92 397 // Change reference
ThomasBNL 39:104a038f7b92 398 void Change_Turn_Position_Left (double& reference, double change_one_bottle)
ThomasBNL 39:104a038f7b92 399 {
ThomasBNL 45:359df0594588 400 if(reference==90)
ThomasBNL 45:359df0594588 401 {
ThomasBNL 45:359df0594588 402 reference=-90;
ThomasBNL 45:359df0594588 403 }
ThomasBNL 45:359df0594588 404 else
ThomasBNL 45:359df0594588 405 {
ThomasBNL 45:359df0594588 406 reference = reference + change_one_bottle;
ThomasBNL 45:359df0594588 407 keep_in_range(&reference, -90, 90); // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
ThomasBNL 45:359df0594588 408 }
ThomasBNL 39:104a038f7b92 409 }
ThomasBNL 39:104a038f7b92 410
ThomasBNL 39:104a038f7b92 411 void Change_Turn_Position_Right (double& reference, double change_one_bottle)
ThomasBNL 39:104a038f7b92 412 {
ThomasBNL 45:359df0594588 413 if(reference==-90)
ThomasBNL 45:359df0594588 414 {
ThomasBNL 45:359df0594588 415 reference=90;
ThomasBNL 45:359df0594588 416 }
ThomasBNL 45:359df0594588 417 else
ThomasBNL 45:359df0594588 418 {
ThomasBNL 45:359df0594588 419 reference = reference - change_one_bottle;
ThomasBNL 45:359df0594588 420 keep_in_range(&reference, -90, 90);
ThomasBNL 45:359df0594588 421 }
ThomasBNL 39:104a038f7b92 422 }
ThomasBNL 39:104a038f7b92 423
ThomasBNL 42:0a7898cb3e94 424 // Deactivate or Activate PID_Controller
ThomasBNL 42:0a7898cb3e94 425 void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 426 {
ThomasBNL 42:0a7898cb3e94 427 P_gain=0;
ThomasBNL 42:0a7898cb3e94 428 I_gain=0;
ThomasBNL 42:0a7898cb3e94 429 D_gain=0;
ThomasBNL 42:0a7898cb3e94 430 }
ThomasBNL 42:0a7898cb3e94 431
ThomasBNL 42:0a7898cb3e94 432 void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 433 {
ThomasBNL 45:359df0594588 434 P_gain=0.01; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
ThomasBNL 45:359df0594588 435 I_gain=0.5;
ThomasBNL 45:359df0594588 436 D_gain=5;
ThomasBNL 42:0a7898cb3e94 437 }
ThomasBNL 42:0a7898cb3e94 438
ThomasBNL 42:0a7898cb3e94 439 void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
ThomasBNL 42:0a7898cb3e94 440 {
ThomasBNL 42:0a7898cb3e94 441 P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
ThomasBNL 42:0a7898cb3e94 442 I_gain=0;
ThomasBNL 42:0a7898cb3e94 443 D_gain=0;
ThomasBNL 42:0a7898cb3e94 444 }