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@38:17a1622a27f7, 2015-10-14 (annotated)

Committer:
ThomasBNL
Date:
Wed Oct 14 06:10:48 2015 +0000
Revision:
38:17a1622a27f7
Parent:
36:da07b5c2984d
Child:
39:104a038f7b92
PID CONTROLLER + WERKEND HIDSCOPE (VERSIE 2) Added function to activate and deavtivate the PID controller

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