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