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@37:23660d12d772, 2015-10-14 (annotated)
- Committer:
- ThomasBNL
- Date:
- Wed Oct 14 05:35:11 2015 +0000
- Revision:
- 37:23660d12d772
- Parent:
- 36:da07b5c2984d
TEST pid in FUNCTIE : getPulses lijkt niet te werken in de functie
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 | QEI motor_turn(D12,D13,NC,32); // Encoder for motor Turn |
ThomasBNL | 34:c672f5c0763f | 34 | PwmOut pwm_motor_turn(D5); // Pwm for motor Turn |
ThomasBNL | 34:c672f5c0763f | 35 | DigitalOut motordirection_turn(D4); // Direction of the motor Turn |
ThomasBNL | 34:c672f5c0763f | 36 | |
ThomasBNL | 34:c672f5c0763f | 37 | // ___________________________ |
ThomasBNL | 34:c672f5c0763f | 38 | // // \\ |
ThomasBNL | 34:c672f5c0763f | 39 | // || [HIDSCOPE] || |
ThomasBNL | 34:c672f5c0763f | 40 | // \\___________________________// |
ThomasBNL | 34:c672f5c0763f | 41 | // |
ThomasBNL | 37:23660d12d772 | 42 | //HIDScope scope(3); // HIDSCOPE declared |
ThomasBNL | 34:c672f5c0763f | 43 | // ___________________________ |
ThomasBNL | 34:c672f5c0763f | 44 | // // \\ |
ThomasBNL | 34:c672f5c0763f | 45 | // || [CONSTANTS] || |
ThomasBNL | 34:c672f5c0763f | 46 | // \\___________________________// |
ThomasBNL | 34:c672f5c0763f | 47 | // |
ThomasBNL | 37:23660d12d772 | 48 | const double off=1; //Led off |
ThomasBNL | 37:23660d12d772 | 49 | const double on=0; //Led on |
ThomasBNL | 37:23660d12d772 | 50 | const int Fs = 512; // sampling frequency (512 Hz) |
ThomasBNL | 37:23660d12d772 | 51 | |
ThomasBNL | 37:23660d12d772 | 52 | // ___________________________ |
ThomasBNL | 37:23660d12d772 | 53 | // // \\ |
ThomasBNL | 37:23660d12d772 | 54 | // || [FUNCTIONS USED] || |
ThomasBNL | 37:23660d12d772 | 55 | // \\___________________________// |
ThomasBNL | 37:23660d12d772 | 56 | // |
ThomasBNL | 0:40052f5ca77b | 57 | |
ThomasBNL | 37:23660d12d772 | 58 | void keep_in_range(double * in, double min, double max); |
ThomasBNL | 37:23660d12d772 | 59 | void setlooptimerflag(void); |
ThomasBNL | 37:23660d12d772 | 60 | |
ThomasBNL | 37:23660d12d772 | 61 | |
ThomasBNL | 37:23660d12d772 | 62 | |
ThomasBNL | 37:23660d12d772 | 63 | /////////////////////////////// |
ThomasBNL | 37:23660d12d772 | 64 | // // |
ThomasBNL | 37:23660d12d772 | 65 | /////////////////////////////////////////////////////// [MAIN FUNCTION] //////////////////////////////////////////////////////////////////////////// |
ThomasBNL | 37:23660d12d772 | 66 | // // // |
ThomasBNL | 37:23660d12d772 | 67 | /////////////////////////////// // |
ThomasBNL | 37:23660d12d772 | 68 | int main() { |
ThomasBNL | 37:23660d12d772 | 69 | debug_led_red=off; |
ThomasBNL | 37:23660d12d772 | 70 | debug_led_blue=off; |
ThomasBNL | 37:23660d12d772 | 71 | debug_led_green=off; |
ThomasBNL | 37:23660d12d772 | 72 | |
ThomasBNL | 37:23660d12d772 | 73 | |
ThomasBNL | 37:23660d12d772 | 74 | //START OF CODE |
ThomasBNL | 37:23660d12d772 | 75 | pc.printf("Start of code \n\r"); |
ThomasBNL | 37:23660d12d772 | 76 | |
ThomasBNL | 37:23660d12d772 | 77 | pc.baud(115200); // Set the baudrate |
ThomasBNL | 37:23660d12d772 | 78 | |
ThomasBNL | 37:23660d12d772 | 79 | // Tickers |
ThomasBNL | 37:23660d12d772 | 80 | Ticker looptimer; // Ticker called looptimer to set a looptimerflag |
ThomasBNL | 37:23660d12d772 | 81 | looptimer.attach(&setlooptimerflag,(float)1/Fs); // calls the looptimer flag every 0.01s |
ThomasBNL | 37:23660d12d772 | 82 | |
ThomasBNL | 37:23660d12d772 | 83 | pc.printf("Start infinite loop \n\r"); |
ThomasBNL | 37:23660d12d772 | 84 | wait (3); // Wait before starting system |
ThomasBNL | 37:23660d12d772 | 85 | |
ThomasBNL | 37:23660d12d772 | 86 | |
ThomasBNL | 37:23660d12d772 | 87 | //INFINITE LOOP |
ThomasBNL | 37:23660d12d772 | 88 | while(1) |
ThomasBNL | 37:23660d12d772 | 89 | { // Start while loop |
ThomasBNL | 37:23660d12d772 | 90 | |
ThomasBNL | 37:23660d12d772 | 91 | |
ThomasBNL | 37:23660d12d772 | 92 | // ___________________________ |
ThomasBNL | 37:23660d12d772 | 93 | // // \\ |
ThomasBNL | 37:23660d12d772 | 94 | // || [Wait for go signal] || |
ThomasBNL | 37:23660d12d772 | 95 | // \\___________________________// |
ThomasBNL | 37:23660d12d772 | 96 | // // Wait until looptimer flag is true then execute PID controller loop. |
ThomasBNL | 37:23660d12d772 | 97 | |
ThomasBNL | 37:23660d12d772 | 98 | |
ThomasBNL | 37:23660d12d772 | 99 | //reference = (potmeter.read()-0.5)*2000; // Potmeter bepaald reference (uitgeschakeld) |
ThomasBNL | 37:23660d12d772 | 100 | |
ThomasBNL | 37:23660d12d772 | 101 | |
ThomasBNL | 37:23660d12d772 | 102 | } |
ThomasBNL | 37:23660d12d772 | 103 | } |
ThomasBNL | 37:23660d12d772 | 104 | |
ThomasBNL | 37:23660d12d772 | 105 | //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
ThomasBNL | 37:23660d12d772 | 106 | // // |
ThomasBNL | 37:23660d12d772 | 107 | // [Functions Described] // |
ThomasBNL | 37:23660d12d772 | 108 | // // |
ThomasBNL | 37:23660d12d772 | 109 | //////////////////////////////////// |
ThomasBNL | 37:23660d12d772 | 110 | |
ThomasBNL | 37:23660d12d772 | 111 | // Keep in range function |
ThomasBNL | 37:23660d12d772 | 112 | void keep_in_range(double * in, double min, double max) |
ThomasBNL | 37:23660d12d772 | 113 | { |
ThomasBNL | 37:23660d12d772 | 114 | *in > min ? *in < max? : *in = max: *in = min; |
ThomasBNL | 37:23660d12d772 | 115 | } |
ThomasBNL | 37:23660d12d772 | 116 | |
ThomasBNL | 37:23660d12d772 | 117 | double reference_turn=20; // Set constant to store reference value of the Turn motor |
ThomasBNL | 37:23660d12d772 | 118 | volatile bool looptimerflag; |
ThomasBNL | 37:23660d12d772 | 119 | const double cw=0; // zero is clockwise (front view) |
ThomasBNL | 37:23660d12d772 | 120 | const double ccw=1; // one is counterclockwise (front view) |
ThomasBNL | 37:23660d12d772 | 121 | const double sample_time = 0.001953125; // sampling frequency (512 Hz) |
ThomasBNL | 34:c672f5c0763f | 122 | |
ThomasBNL | 34:c672f5c0763f | 123 | // PID motor constants |
ThomasBNL | 34:c672f5c0763f | 124 | double integrate_error_turn=0; // integration error turn motor |
ThomasBNL | 34:c672f5c0763f | 125 | double previous_error_turn=0; // previous error turn motor |
ThomasBNL | 8:50d6e2323d3b | 126 | |
ThomasBNL | 26:b3693f431d6f | 127 | const double Gain_P_turn=10; //0.0067; |
ThomasBNL | 8:50d6e2323d3b | 128 | // stel setpoint tussen (0 en 360) en position tussen (0 en 360) |
ThomasBNL | 8:50d6e2323d3b | 129 | // 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 | 130 | // dus 0.1=15*gain gain=0.0067 |
ThomasBNL | 14:599896acf576 | 131 | // Als 3 graden verschil 0.1 dan 0.1/3=gain=0.33 |
ThomasBNL | 13:bcf8ec7120ab | 132 | |
ThomasBNL | 34:c672f5c0763f | 133 | double Gain_I_turn=0.1; //0.025; //(1/2000) //0.00000134 |
ThomasBNL | 13:bcf8ec7120ab | 134 | // 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 | 135 | // 0.1 / (4.01) = Gain = 0.025 |
ThomasBNL | 13:bcf8ec7120ab | 136 | |
ThomasBNL | 34:c672f5c0763f | 137 | double Gain_D_turn=50; //0.01; |
ThomasBNL | 13:bcf8ec7120ab | 138 | // error_derivative_turn=(error - previous_error_turn)/sample_time |
ThomasBNL | 14:599896acf576 | 139 | // |
ThomasBNL | 37:23660d12d772 | 140 | |
ThomasBNL | 8:50d6e2323d3b | 141 | double conversion_counts_to_degrees=0.085877862594198; |
ThomasBNL | 8:50d6e2323d3b | 142 | // gear ratio motor = 131 |
ThomasBNL | 8:50d6e2323d3b | 143 | // ticks per magnet rotation = 32 (X2 Encoder) |
ThomasBNL | 8:50d6e2323d3b | 144 | // One revolution = 360 degrees |
ThomasBNL | 8:50d6e2323d3b | 145 | // degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198 |
ThomasBNL | 8:50d6e2323d3b | 146 | |
ThomasBNL | 37:23660d12d772 | 147 | // Looptimerflag function |
ThomasBNL | 37:23660d12d772 | 148 | void setlooptimerflag(void) |
ThomasBNL | 37:23660d12d772 | 149 | { |
ThomasBNL | 37:23660d12d772 | 150 | debug_led_green=on; |
ThomasBNL | 8:50d6e2323d3b | 151 | |
ThomasBNL | 0:40052f5ca77b | 152 | |
ThomasBNL | 34:c672f5c0763f | 153 | // ___________________________ |
ThomasBNL | 34:c672f5c0763f | 154 | // // \\ |
ThomasBNL | 34:c672f5c0763f | 155 | // || [DEBUG BUTTONS] || |
ThomasBNL | 34:c672f5c0763f | 156 | // \\___________________________// |
ThomasBNL | 34:c672f5c0763f | 157 | // interrupt button Disbalances the current motor position |
ThomasBNL | 34:c672f5c0763f | 158 | |
ThomasBNL | 36:da07b5c2984d | 159 | //if button L2 pressed => disbalance motor |
ThomasBNL | 34:c672f5c0763f | 160 | if (buttonL2.read() < 0.5){ |
ThomasBNL | 8:50d6e2323d3b | 161 | motordirection_turn = cw; |
ThomasBNL | 34:c672f5c0763f | 162 | pwm_motor_turn = 0.5f; |
ThomasBNL | 8:50d6e2323d3b | 163 | pc.printf("positie = %d \r\n", motor_turn.getPulses()); } |
ThomasBNL | 34:c672f5c0763f | 164 | |
ThomasBNL | 8:50d6e2323d3b | 165 | |
ThomasBNL | 36:da07b5c2984d | 166 | // if button L1 pressed => shut down motor for 1000 seconds |
ThomasBNL | 34:c672f5c0763f | 167 | if (buttonL1.read() < 0.5){ |
ThomasBNL | 22:eaf4cbd1dcec | 168 | motordirection_turn = cw; |
ThomasBNL | 22:eaf4cbd1dcec | 169 | pwm_motor_turn = 0; |
ThomasBNL | 22:eaf4cbd1dcec | 170 | wait(1000); |
ThomasBNL | 22:eaf4cbd1dcec | 171 | pc.printf("positie = %d \r\n", motor_turn.getPulses()); } |
ThomasBNL | 17:aa167ab3cf75 | 172 | |
ThomasBNL | 34:c672f5c0763f | 173 | else |
ThomasBNL | 8:50d6e2323d3b | 174 | { |
ThomasBNL | 37:23660d12d772 | 175 | |
ThomasBNL | 37:23660d12d772 | 176 | // ___________________________ |
ThomasBNL | 34:c672f5c0763f | 177 | // // \\ |
ThomasBNL | 34:c672f5c0763f | 178 | // || [Calibrate position] || |
ThomasBNL | 34:c672f5c0763f | 179 | // \\___________________________// |
ThomasBNL | 36:da07b5c2984d | 180 | // // Keep motor position between -4200 and 4200 counts |
ThomasBNL | 37:23660d12d772 | 181 | debug_led_green=on; |
ThomasBNL | 8:50d6e2323d3b | 182 | 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 | 183 | { |
ThomasBNL | 8:50d6e2323d3b | 184 | motor_turn.reset(); |
ThomasBNL | 3:11a7da46e093 | 185 | pc.printf("RESET \n\r"); |
ThomasBNL | 3:11a7da46e093 | 186 | } |
ThomasBNL | 3:11a7da46e093 | 187 | |
ThomasBNL | 34:c672f5c0763f | 188 | // Convert position to degrees \\ |
ThomasBNL | 34:c672f5c0763f | 189 | |
ThomasBNL | 37:23660d12d772 | 190 | double position_turn = conversion_counts_to_degrees * motor_turn.getPulses(); |
ThomasBNL | 0:40052f5ca77b | 191 | |
ThomasBNL | 8:50d6e2323d3b | 192 | pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn); |
ThomasBNL | 3:11a7da46e093 | 193 | |
ThomasBNL | 0:40052f5ca77b | 194 | |
ThomasBNL | 34:c672f5c0763f | 195 | // ___________________________ |
ThomasBNL | 34:c672f5c0763f | 196 | // // \\ |
ThomasBNL | 34:c672f5c0763f | 197 | // || [PID CONTROLLER] || |
ThomasBNL | 34:c672f5c0763f | 198 | // \\___________________________// |
ThomasBNL | 34:c672f5c0763f | 199 | // // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\ |
ThomasBNL | 34:c672f5c0763f | 200 | |
ThomasBNL | 37:23660d12d772 | 201 | double error_turn=(reference_turn - position_turn); // Current error (input controller) |
ThomasBNL | 8:50d6e2323d3b | 202 | |
ThomasBNL | 37:23660d12d772 | 203 | integrate_error_turn=integrate_error_turn + sample_time*error_turn; // integral error output |
ThomasBNL | 34:c672f5c0763f | 204 | // // overwrite previous integrate error by adding the current error |
ThomasBNL | 34:c672f5c0763f | 205 | // multiplied by the sample time. |
ThomasBNL | 8:50d6e2323d3b | 206 | // |
ThomasBNL | 37:23660d12d772 | 207 | double error_derivative_turn=(error_turn - previous_error_turn)/sample_time; // derivative error output |
ThomasBNL | 8:50d6e2323d3b | 208 | |
ThomasBNL | 37:23660d12d772 | 209 | // FILTER error_derivative_turn (lowpassfilter) (EMG LOWPASS FILTER MOMENTEEL!!!!!) |
ThomasBNL | 11:ecd83b303252 | 210 | |
ThomasBNL | 37:23660d12d772 | 211 | const double low_b1 = 1.480219865318266e-04; //filter coefficients |
ThomasBNL | 37:23660d12d772 | 212 | const double low_b2 = 2.960439730636533e-04; |
ThomasBNL | 37:23660d12d772 | 213 | const double low_b3 = 1.480219865318266e-04; |
ThomasBNL | 37:23660d12d772 | 214 | const double low_a2 = -1.965293372622690e+00; // a1 is normalized to 1 |
ThomasBNL | 37:23660d12d772 | 215 | const double low_a3 = 9.658854605688177e-01; |
ThomasBNL | 37:23660d12d772 | 216 | biquadFilter lowpassfilter_1(low_a2, low_a3, low_b1, low_b2, low_b3); |
ThomasBNL | 11:ecd83b303252 | 217 | |
ThomasBNL | 37:23660d12d772 | 218 | error_derivative_turn=lowpassfilter_1.step(error_derivative_turn); |
ThomasBNL | 3:11a7da46e093 | 219 | |
ThomasBNL | 37:23660d12d772 | 220 | previous_error_turn=error_turn; // current error is saved to memory constant to be used in |
ThomasBNL | 8:50d6e2323d3b | 221 | // the next loop for calculating the derivative error |
ThomasBNL | 8:50d6e2323d3b | 222 | |
ThomasBNL | 37:23660d12d772 | 223 | double pwm_to_motor_turn = error_turn*Gain_P_turn; // output P controller to pwm |
ThomasBNL | 8:50d6e2323d3b | 224 | |
ThomasBNL | 37:23660d12d772 | 225 | double pwm_motor_turn_P = error_turn*Gain_P_turn; // output P controller to pwm |
ThomasBNL | 37:23660d12d772 | 226 | double pwm_motor_turn_I = integrate_error_turn*Gain_I_turn; // output I controller to pwm |
ThomasBNL | 37:23660d12d772 | 227 | double pwm_motor_turn_D = error_derivative_turn*Gain_D_turn; // output D controller to pwm |
ThomasBNL | 10:09ba965045a7 | 228 | |
ThomasBNL | 13:bcf8ec7120ab | 229 | pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D; |
ThomasBNL | 20:bdc62ee49197 | 230 | |
ThomasBNL | 34:c672f5c0763f | 231 | // ___________________________ |
ThomasBNL | 34:c672f5c0763f | 232 | // // \\ |
ThomasBNL | 34:c672f5c0763f | 233 | // || [PID error -> pwm motor] || |
ThomasBNL | 34:c672f5c0763f | 234 | // \\___________________________// |
ThomasBNL | 36:da07b5c2984d | 235 | // // Keep Pwm between -1 and 1 -> and decide the direction of the motor to compensate the error |
ThomasBNL | 34:c672f5c0763f | 236 | |
ThomasBNL | 10:09ba965045a7 | 237 | keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to motor controller but keep it in range! |
ThomasBNL | 10:09ba965045a7 | 238 | pc.printf("pwm %f \n\r", pwm_to_motor_turn); |
ThomasBNL | 0:40052f5ca77b | 239 | |
ThomasBNL | 8:50d6e2323d3b | 240 | // Check error and decide direction to turn |
ThomasBNL | 10:09ba965045a7 | 241 | if(pwm_to_motor_turn > 0) |
ThomasBNL | 3:11a7da46e093 | 242 | { |
ThomasBNL | 8:50d6e2323d3b | 243 | motordirection_turn=ccw; |
ThomasBNL | 15:f029351f1f3a | 244 | pc.printf("if loop pwm > 0 \n\r"); |
ThomasBNL | 3:11a7da46e093 | 245 | } |
ThomasBNL | 0:40052f5ca77b | 246 | else |
ThomasBNL | 3:11a7da46e093 | 247 | { |
ThomasBNL | 8:50d6e2323d3b | 248 | motordirection_turn=cw; |
ThomasBNL | 15:f029351f1f3a | 249 | pc.printf("else loop pwm < 0 \n\r"); |
ThomasBNL | 3:11a7da46e093 | 250 | } |
ThomasBNL | 8:50d6e2323d3b | 251 | |
ThomasBNL | 34:c672f5c0763f | 252 | // ___________________________ |
ThomasBNL | 34:c672f5c0763f | 253 | // // \\ |
ThomasBNL | 34:c672f5c0763f | 254 | // || [pwm -> Plant] || |
ThomasBNL | 34:c672f5c0763f | 255 | // \\___________________________// |
ThomasBNL | 34:c672f5c0763f | 256 | // // Put pwm to the motor \\ |
ThomasBNL | 34:c672f5c0763f | 257 | |
ThomasBNL | 10:09ba965045a7 | 258 | pwm_motor_turn=(abs(pwm_to_motor_turn)); |
ThomasBNL | 14:599896acf576 | 259 | |
ThomasBNL | 34:c672f5c0763f | 260 | // ___________________________ |
ThomasBNL | 34:c672f5c0763f | 261 | // // \\ |
ThomasBNL | 34:c672f5c0763f | 262 | // || [HIDSCOPE] || |
ThomasBNL | 34:c672f5c0763f | 263 | // \\___________________________// |
ThomasBNL | 34:c672f5c0763f | 264 | // // Check signals inside HIDSCOPE \\ |
ThomasBNL | 34:c672f5c0763f | 265 | |
ThomasBNL | 37:23660d12d772 | 266 | // scope.set(0,error_turn); // HIDSCOPE channel 0 : Current Reference |
ThomasBNL | 37:23660d12d772 | 267 | // scope.set(1,position_turn); // HIDSCOPE channel 1 : Position_turn |
ThomasBNL | 37:23660d12d772 | 268 | // scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn |
ThomasBNL | 37:23660d12d772 | 269 | // scope.send(); // Send channel info to HIDSCOPE |
ThomasBNL | 5:8fb74a22fe3c | 270 | } |
ThomasBNL | 1:dc683e88b44e | 271 | } |
ThomasBNL | 1:dc683e88b44e | 272 | |
ThomasBNL | 8:50d6e2323d3b | 273 | // Get setpoint -> potmeter (MOMENTEEL UITGESCHAKELD) |
ThomasBNL | 8:50d6e2323d3b | 274 | double get_reference(double input) |
ThomasBNL | 1:dc683e88b44e | 275 | { |
ThomasBNL | 1:dc683e88b44e | 276 | const float offset = 0.5; |
ThomasBNL | 1:dc683e88b44e | 277 | const float gain = 4.0; |
ThomasBNL | 1:dc683e88b44e | 278 | return (input-offset)*gain; |
ThomasBNL | 5:8fb74a22fe3c | 279 | } |