control flow doet nog niks
Dependencies: Encoder HIDScope MODSERIAL mbed
main.cpp@8:00c3992af9f4, 2015-10-14 (annotated)
- Committer:
- Zeekat
- Date:
- Wed Oct 14 13:19:32 2015 +0000
- Revision:
- 8:00c3992af9f4
- Parent:
- 7:4df9f6ebe744
werkende versie. PID wordt getuned
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Zeekat | 0:f6306e179750 | 1 | #include "mbed.h" |
Zeekat | 0:f6306e179750 | 2 | #include "MODSERIAL.h" |
Zeekat | 0:f6306e179750 | 3 | #include "encoder.h" |
Zeekat | 2:2563df4ee829 | 4 | #include "HIDScope.h" |
Zeekat | 0:f6306e179750 | 5 | |
Zeekat | 0:f6306e179750 | 6 | Serial pc(USBTX,USBRX); |
Zeekat | 8:00c3992af9f4 | 7 | HIDScope scope(3); // definieerd het aantal kanalen van de scope |
Zeekat | 0:f6306e179750 | 8 | |
Zeekat | 2:2563df4ee829 | 9 | // Define Tickers and control frequencies |
Zeekat | 0:f6306e179750 | 10 | Ticker controller1, controller2; // definieer de ticker die controler1 doet |
Zeekat | 2:2563df4ee829 | 11 | // Go flag variables |
Zeekat | 2:2563df4ee829 | 12 | volatile bool motor1_go = false, motor2_go = false; |
Zeekat | 2:2563df4ee829 | 13 | |
Zeekat | 2:2563df4ee829 | 14 | // Frequency control |
Zeekat | 2:2563df4ee829 | 15 | double controlfreq = 50 ; // controlloops frequentie (Hz) |
Zeekat | 2:2563df4ee829 | 16 | double controlstep = 1/controlfreq; // timestep derived from controlfreq |
Zeekat | 0:f6306e179750 | 17 | |
Zeekat | 2:2563df4ee829 | 18 | |
Zeekat | 2:2563df4ee829 | 19 | //MOTOR OUTPUTPINS |
Zeekat | 0:f6306e179750 | 20 | // motor 1 |
Zeekat | 2:2563df4ee829 | 21 | PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets) |
Zeekat | 2:2563df4ee829 | 22 | DigitalOut motor1_rich(D7); // digitaal signaal voor richting |
Zeekat | 0:f6306e179750 | 23 | // motor 2 |
Zeekat | 2:2563df4ee829 | 24 | PwmOut motor2_aan(D5); |
Zeekat | 2:2563df4ee829 | 25 | DigitalOut motor2_rich(D4); |
Zeekat | 0:f6306e179750 | 26 | |
Zeekat | 2:2563df4ee829 | 27 | // ENCODER INPUTPINS |
Zeekat | 0:f6306e179750 | 28 | Encoder motor1_enc(D12,D11); // encoder outputpins |
Zeekat | 0:f6306e179750 | 29 | Encoder motor2_enc(D10,D9); |
Zeekat | 0:f6306e179750 | 30 | |
Zeekat | 0:f6306e179750 | 31 | int reference1 = 0; // set the reference position of the encoders |
Zeekat | 0:f6306e179750 | 32 | int reference2 = 0; |
Zeekat | 0:f6306e179750 | 33 | |
Zeekat | 2:2563df4ee829 | 34 | |
Zeekat | 2:2563df4ee829 | 35 | // EXTRA INPUTS AND REQUIRED VARIABLES |
Zeekat | 2:2563df4ee829 | 36 | //POTMETERS |
Zeekat | 2:2563df4ee829 | 37 | AnalogIn potright(A0); // define the potmeter outputpins |
Zeekat | 2:2563df4ee829 | 38 | AnalogIn potleft(A1); |
Zeekat | 2:2563df4ee829 | 39 | |
Zeekat | 6:9c8e91bb1316 | 40 | // BUTTONS |
Zeekat | 6:9c8e91bb1316 | 41 | // control flow |
Zeekat | 4:072b99947fc6 | 42 | DigitalIn buttonlinks(PTA4); |
Zeekat | 4:072b99947fc6 | 43 | DigitalIn buttonrechts(PTC6); |
Zeekat | 4:072b99947fc6 | 44 | // init values |
Zeekat | 4:072b99947fc6 | 45 | bool loop_start = false; |
Zeekat | 4:072b99947fc6 | 46 | bool calib_start = false; |
Zeekat | 6:9c8e91bb1316 | 47 | |
Zeekat | 6:9c8e91bb1316 | 48 | // direction control |
Zeekat | 6:9c8e91bb1316 | 49 | DigitalIn reverse_button_links(D0); |
Zeekat | 6:9c8e91bb1316 | 50 | DigitalIn reverse_button_rechts(D1); |
Zeekat | 6:9c8e91bb1316 | 51 | // init values |
Zeekat | 6:9c8e91bb1316 | 52 | bool reverse_links = false; |
Zeekat | 6:9c8e91bb1316 | 53 | bool reverse_rechts = false; |
Zeekat | 2:2563df4ee829 | 54 | |
Zeekat | 4:072b99947fc6 | 55 | // LED |
Zeekat | 4:072b99947fc6 | 56 | DigitalOut ledred(LED1); |
Zeekat | 4:072b99947fc6 | 57 | DigitalOut ledgreen(LED2); |
Zeekat | 4:072b99947fc6 | 58 | DigitalOut ledblue(LED3); |
Zeekat | 2:2563df4ee829 | 59 | |
Zeekat | 3:7273bbe6aa02 | 60 | // REFERENCE SIGNAL SETTINGS |
Zeekat | 7:4df9f6ebe744 | 61 | double input_threshold = 0.25; // the minimum value the signal must have to change the reference. |
Zeekat | 3:7273bbe6aa02 | 62 | // Define signal amplification (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!) ?? |
Zeekat | 3:7273bbe6aa02 | 63 | double signalamp1 = 1; |
Zeekat | 3:7273bbe6aa02 | 64 | double signalamp2 = 1; |
Zeekat | 6:9c8e91bb1316 | 65 | // Define storage variables for reference values |
Zeekat | 3:7273bbe6aa02 | 66 | double c_reference1 = 0; |
Zeekat | 3:7273bbe6aa02 | 67 | double c_reference2 = 0; |
Zeekat | 8:00c3992af9f4 | 68 | // limit angles |
Zeekat | 8:00c3992af9f4 | 69 | // motor1 |
Zeekat | 8:00c3992af9f4 | 70 | const double limlow1 = 0.5; // min height |
Zeekat | 8:00c3992af9f4 | 71 | const double limhigh1 = 4.5; // max height |
Zeekat | 8:00c3992af9f4 | 72 | // motor 2 |
Zeekat | 8:00c3992af9f4 | 73 | const double limlow2 = -4.5; // maximum height, motor has been inversed due to transmission |
Zeekat | 8:00c3992af9f4 | 74 | const double limhigh2 = 2; // minimum height |
Zeekat | 8:00c3992af9f4 | 75 | |
Zeekat | 3:7273bbe6aa02 | 76 | // Define the maximum rate of change for the reference (velocity) |
Zeekat | 8:00c3992af9f4 | 77 | double Vmax = 3; // rad/sec |
Zeekat | 2:2563df4ee829 | 78 | |
Zeekat | 0:f6306e179750 | 79 | // CONTROLLER SETTINGS |
Zeekat | 1:2f23368e8638 | 80 | // motor 1 |
Zeekat | 8:00c3992af9f4 | 81 | const double m1_Kp = 2; // Proportional constant |
Zeekat | 8:00c3992af9f4 | 82 | const double m1_Ki = 0.05; // integration constant |
Zeekat | 1:2f23368e8638 | 83 | const double m1_Kd = 0; // differentiation constant |
Zeekat | 1:2f23368e8638 | 84 | // motor 2 |
Zeekat | 8:00c3992af9f4 | 85 | const double m2_Kp = 0.25; |
Zeekat | 8:00c3992af9f4 | 86 | const double m2_Ki = 0.01; |
Zeekat | 1:2f23368e8638 | 87 | const double m2_Kd = 0; |
Zeekat | 1:2f23368e8638 | 88 | // storage variables |
Zeekat | 1:2f23368e8638 | 89 | // motor 1 |
Zeekat | 1:2f23368e8638 | 90 | double m1_err_int = 0; |
Zeekat | 1:2f23368e8638 | 91 | double m1_prev_err = 0; |
Zeekat | 1:2f23368e8638 | 92 | // motor 2 |
Zeekat | 1:2f23368e8638 | 93 | double m2_err_int = 0; |
Zeekat | 1:2f23368e8638 | 94 | double m2_prev_err = 0; |
Zeekat | 0:f6306e179750 | 95 | |
Zeekat | 0:f6306e179750 | 96 | |
Zeekat | 1:2f23368e8638 | 97 | //// FILTER VARIABLES |
Zeekat | 1:2f23368e8638 | 98 | // storage variables |
Zeekat | 1:2f23368e8638 | 99 | // differential action filter, same is used for both controllers |
Zeekat | 1:2f23368e8638 | 100 | double m_f_v1 = 0; |
Zeekat | 1:2f23368e8638 | 101 | double m_f_v2 = 0; |
Zeekat | 1:2f23368e8638 | 102 | |
Zeekat | 1:2f23368e8638 | 103 | // Filter coefficients |
Zeekat | 6:9c8e91bb1316 | 104 | // differential action filter |
Zeekat | 8:00c3992af9f4 | 105 | const double m_f_a1 = -1.1430, m_f_a2 = 0.4128, m_f_b0 = 0.0675, m_f_b1 = 0.1349, m_f_b2 = 0.0675; // coefficients from sheets are used as first test. |
Zeekat | 5:67afe491a1e3 | 106 | |
Zeekat | 5:67afe491a1e3 | 107 | // tweede orde notch filter 50 Hz |
Zeekat | 5:67afe491a1e3 | 108 | // biquad 1 coefficienten |
Zeekat | 5:67afe491a1e3 | 109 | const double numnotch50biq1_1 = 1; |
Zeekat | 5:67afe491a1e3 | 110 | const double numnotch50biq1_2 = -1.61816178466632; |
Zeekat | 5:67afe491a1e3 | 111 | const double numnotch50biq1_3 = 1.00000006127058; |
Zeekat | 5:67afe491a1e3 | 112 | const double dennotch50biq1_2 = -1.59325742941798; |
Zeekat | 5:67afe491a1e3 | 113 | const double dennotch50biq1_3 = 0.982171881701431; |
Zeekat | 5:67afe491a1e3 | 114 | // biquad 2 coefficienten |
Zeekat | 5:67afe491a1e3 | 115 | const double numnotch50biq2_1 = 1; |
Zeekat | 5:67afe491a1e3 | 116 | const double numnotch50biq2_2 = -1.61816171933244; |
Zeekat | 5:67afe491a1e3 | 117 | const double numnotch50biq2_3 = 0.999999938729428; |
Zeekat | 5:67afe491a1e3 | 118 | const double dennotch50biq2_2 = -1.61431180968071; |
Zeekat | 5:67afe491a1e3 | 119 | const double dennotch50biq2_3 = 0.982599066293075; |
Zeekat | 5:67afe491a1e3 | 120 | // highpass filter 20 Hz coefficienten |
Zeekat | 5:67afe491a1e3 | 121 | const double numhigh20_1 = 0.837089190566345; |
Zeekat | 5:67afe491a1e3 | 122 | const double numhigh20_2 = -1.67417838113269; |
Zeekat | 5:67afe491a1e3 | 123 | const double numhigh20_3 = 0.837089190566345; |
Zeekat | 5:67afe491a1e3 | 124 | const double denhigh20_2 = -1.64745998107698; |
Zeekat | 5:67afe491a1e3 | 125 | const double denhigh20_3 = 0.700896781188403; |
Zeekat | 5:67afe491a1e3 | 126 | // lowpass 5 Hz coefficienten |
Zeekat | 5:67afe491a1e3 | 127 | const double numlow5_1 =0.000944691843840162; |
Zeekat | 5:67afe491a1e3 | 128 | const double numlow5_2 =0.00188938368768032; |
Zeekat | 5:67afe491a1e3 | 129 | const double numlow5_3 =0.000944691843840162; |
Zeekat | 5:67afe491a1e3 | 130 | const double denlow5_2 =-1.91119706742607; |
Zeekat | 5:67afe491a1e3 | 131 | const double denlow5_3 =0.914975834801434; |
Zeekat | 5:67afe491a1e3 | 132 | |
Zeekat | 5:67afe491a1e3 | 133 | // Define the storage variables and filter coeicients for two filters |
Zeekat | 5:67afe491a1e3 | 134 | double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0; |
Zeekat | 0:f6306e179750 | 135 | |
Zeekat | 0:f6306e179750 | 136 | |
Zeekat | 0:f6306e179750 | 137 | |
Zeekat | 0:f6306e179750 | 138 | //////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 139 | /////////////////// START OF SIDE FUNCTIONS //////////////////// |
Zeekat | 0:f6306e179750 | 140 | ////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 141 | // these functions are tailored to perform 1 specific function |
Zeekat | 0:f6306e179750 | 142 | |
Zeekat | 5:67afe491a1e3 | 143 | // this funtion flips leds on and off accordin to input with 0 being on |
Zeekat | 5:67afe491a1e3 | 144 | void LED(int red,int green,int blue) |
Zeekat | 5:67afe491a1e3 | 145 | { |
Zeekat | 5:67afe491a1e3 | 146 | ledred.write(red); |
Zeekat | 5:67afe491a1e3 | 147 | ledgreen.write(green); |
Zeekat | 5:67afe491a1e3 | 148 | ledblue.write(blue); |
Zeekat | 5:67afe491a1e3 | 149 | } |
Zeekat | 5:67afe491a1e3 | 150 | |
Zeekat | 0:f6306e179750 | 151 | // counts 2 radians |
Zeekat | 0:f6306e179750 | 152 | // this function takes counts from the encoder and converts it to the amount of radians from the zero position. |
Zeekat | 0:f6306e179750 | 153 | // It has been set up for standard 2X DECODING!!!!!! |
Zeekat | 0:f6306e179750 | 154 | double get_radians(double counts) |
Zeekat | 0:f6306e179750 | 155 | { |
Zeekat | 0:f6306e179750 | 156 | double pi = 3.14159265359; |
Zeekat | 0:f6306e179750 | 157 | double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, extra warning) |
Zeekat | 0:f6306e179750 | 158 | return radians; |
Zeekat | 0:f6306e179750 | 159 | } |
Zeekat | 0:f6306e179750 | 160 | |
Zeekat | 0:f6306e179750 | 161 | |
Zeekat | 7:4df9f6ebe744 | 162 | // This functions takes a 0->1 input, uses passing by reference (&c_reference) |
Zeekat | 7:4df9f6ebe744 | 163 | // to create a reference that moves with a variable speed. It is now optimised for 0->1 values |
Zeekat | 8:00c3992af9f4 | 164 | double reference_f(double input, double &c_reference, double limlow, double limhigh) |
Zeekat | 0:f6306e179750 | 165 | { |
Zeekat | 6:9c8e91bb1316 | 166 | double reference = c_reference + input * controlstep * Vmax ; |
Zeekat | 8:00c3992af9f4 | 167 | if(reference < limlow){reference = limlow;} |
Zeekat | 8:00c3992af9f4 | 168 | if(reference > limhigh){reference = limhigh;} |
Zeekat | 3:7273bbe6aa02 | 169 | c_reference = reference; // change the global variable to the latest location. |
Zeekat | 3:7273bbe6aa02 | 170 | return reference; |
Zeekat | 0:f6306e179750 | 171 | } |
Zeekat | 0:f6306e179750 | 172 | |
Zeekat | 0:f6306e179750 | 173 | |
Zeekat | 3:7273bbe6aa02 | 174 | // This function takes the controller outputvalue and ensures it is between -1 and 1 |
Zeekat | 0:f6306e179750 | 175 | // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction). |
Zeekat | 3:7273bbe6aa02 | 176 | // needs more work to use it for the wind-up prevention. |
Zeekat | 2:2563df4ee829 | 177 | double outputlimiter (double output, double limit) |
Zeekat | 0:f6306e179750 | 178 | { |
Zeekat | 2:2563df4ee829 | 179 | if(output> limit) |
Zeekat | 0:f6306e179750 | 180 | { |
Zeekat | 0:f6306e179750 | 181 | output = 1; |
Zeekat | 0:f6306e179750 | 182 | } |
Zeekat | 2:2563df4ee829 | 183 | else if(output < limit && output > 0) |
Zeekat | 0:f6306e179750 | 184 | { |
Zeekat | 0:f6306e179750 | 185 | output = output; |
Zeekat | 0:f6306e179750 | 186 | } |
Zeekat | 2:2563df4ee829 | 187 | else if(output > -limit && output < 0) |
Zeekat | 0:f6306e179750 | 188 | { |
Zeekat | 0:f6306e179750 | 189 | output = output; |
Zeekat | 0:f6306e179750 | 190 | } |
Zeekat | 2:2563df4ee829 | 191 | else if(output < -limit) |
Zeekat | 0:f6306e179750 | 192 | { |
Zeekat | 0:f6306e179750 | 193 | (output = -1); |
Zeekat | 0:f6306e179750 | 194 | } |
Zeekat | 0:f6306e179750 | 195 | return output; |
Zeekat | 0:f6306e179750 | 196 | } |
Zeekat | 0:f6306e179750 | 197 | |
Zeekat | 0:f6306e179750 | 198 | |
Zeekat | 1:2f23368e8638 | 199 | // BIQUADFILTER CODE GIVEN IN SHEETS |
Zeekat | 1:2f23368e8638 | 200 | // used for d-control |
Zeekat | 1:2f23368e8638 | 201 | double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) |
Zeekat | 1:2f23368e8638 | 202 | { |
Zeekat | 3:7273bbe6aa02 | 203 | double v = u - a1*v1 - a2*v2; |
Zeekat | 3:7273bbe6aa02 | 204 | double y = b0*v + b1*v1 + b2*v2; |
Zeekat | 1:2f23368e8638 | 205 | v2 = v1; |
Zeekat | 1:2f23368e8638 | 206 | v1 = v; |
Zeekat | 1:2f23368e8638 | 207 | return y; |
Zeekat | 1:2f23368e8638 | 208 | } |
Zeekat | 1:2f23368e8638 | 209 | |
Zeekat | 5:67afe491a1e3 | 210 | double EMG_Filter(double u1) |
Zeekat | 5:67afe491a1e3 | 211 | { |
Zeekat | 5:67afe491a1e3 | 212 | // double u1 = potright.read(); |
Zeekat | 5:67afe491a1e3 | 213 | double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); |
Zeekat | 5:67afe491a1e3 | 214 | double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); |
Zeekat | 5:67afe491a1e3 | 215 | double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); |
Zeekat | 5:67afe491a1e3 | 216 | double y4 = abs(y3); |
Zeekat | 5:67afe491a1e3 | 217 | double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); |
Zeekat | 5:67afe491a1e3 | 218 | return y5; |
Zeekat | 5:67afe491a1e3 | 219 | } |
Zeekat | 1:2f23368e8638 | 220 | |
Zeekat | 1:2f23368e8638 | 221 | // PID Controller given in sheets |
Zeekat | 1:2f23368e8638 | 222 | // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!) |
Zeekat | 2:2563df4ee829 | 223 | double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev) |
Zeekat | 1:2f23368e8638 | 224 | { |
Zeekat | 1:2f23368e8638 | 225 | // Proportional |
Zeekat | 1:2f23368e8638 | 226 | double P = Kp * e; |
Zeekat | 8:00c3992af9f4 | 227 | // pc.printf("P = %f, ",P); |
Zeekat | 1:2f23368e8638 | 228 | // Integral |
Zeekat | 1:2f23368e8638 | 229 | e_int = e_int + Ts * e; |
Zeekat | 1:2f23368e8638 | 230 | double I = e_int * Ki; |
Zeekat | 8:00c3992af9f4 | 231 | // pc.printf("I = %f, ",I); |
Zeekat | 3:7273bbe6aa02 | 232 | // Derivative (Disabled for now because of NaN problem from filter |
Zeekat | 3:7273bbe6aa02 | 233 | double e_derr = (e - e_prev)/Ts; |
Zeekat | 8:00c3992af9f4 | 234 | // pc.printf("e_derr %f, ",e_derr); |
Zeekat | 3:7273bbe6aa02 | 235 | e_derr = biquadfilter(e_derr, m_f_v1, m_f_v2, m_f_a1, m_f_a2, m_f_b0, m_f_b1, m_f_b2); |
Zeekat | 8:00c3992af9f4 | 236 | // |
Zeekat | 3:7273bbe6aa02 | 237 | pc.printf("fil_e_derr %f, ",e_derr); // check for NaN ?? |
Zeekat | 3:7273bbe6aa02 | 238 | e_prev = e; |
Zeekat | 8:00c3992af9f4 | 239 | double D = Kd* e_derr; |
Zeekat | 1:2f23368e8638 | 240 | |
Zeekat | 1:2f23368e8638 | 241 | // PID |
Zeekat | 8:00c3992af9f4 | 242 | double output = P + I + D; |
Zeekat | 1:2f23368e8638 | 243 | return output; |
Zeekat | 1:2f23368e8638 | 244 | } |
Zeekat | 1:2f23368e8638 | 245 | |
Zeekat | 1:2f23368e8638 | 246 | |
Zeekat | 1:2f23368e8638 | 247 | |
Zeekat | 0:f6306e179750 | 248 | // this function is a simple K control called by the motor function |
Zeekat | 0:f6306e179750 | 249 | double K_control(double error,double K) |
Zeekat | 0:f6306e179750 | 250 | { |
Zeekat | 0:f6306e179750 | 251 | double output = K*error; // controller output K*e |
Zeekat | 2:2563df4ee829 | 252 | output = outputlimiter(output,1); // limit the output to -1->1 |
Zeekat | 0:f6306e179750 | 253 | return output; |
Zeekat | 0:f6306e179750 | 254 | } |
Zeekat | 0:f6306e179750 | 255 | |
Zeekat | 0:f6306e179750 | 256 | ///////////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 257 | ////////////////// PRIMARY CONTROL FUNCTIONS /////////////////////// |
Zeekat | 0:f6306e179750 | 258 | /////////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 259 | // these functions are used to control all aspects of a single electric motor and are called by the main function |
Zeekat | 0:f6306e179750 | 260 | |
Zeekat | 0:f6306e179750 | 261 | // MOTOR 1 |
Zeekat | 0:f6306e179750 | 262 | void motor1_control() |
Zeekat | 0:f6306e179750 | 263 | { |
Zeekat | 5:67afe491a1e3 | 264 | |
Zeekat | 7:4df9f6ebe744 | 265 | double input1 = potright.read()*signalamp1; // this line must be seperated for emg usage |
Zeekat | 8:00c3992af9f4 | 266 | //pc.printf("input = %f ",input1); |
Zeekat | 7:4df9f6ebe744 | 267 | // first data limiter |
Zeekat | 7:4df9f6ebe744 | 268 | if(input1 < input_threshold) {input1 = 0;} |
Zeekat | 6:9c8e91bb1316 | 269 | if(input1 > 1) {input1 = 1;} |
Zeekat | 7:4df9f6ebe744 | 270 | // reverse direction |
Zeekat | 6:9c8e91bb1316 | 271 | if(reverse_rechts == true) {input1 = -input1;} |
Zeekat | 8:00c3992af9f4 | 272 | |
Zeekat | 8:00c3992af9f4 | 273 | double reference1 = reference_f(input1, c_reference1,limlow1,limhigh1); // determine the reference that has been set by the inputsignal |
Zeekat | 8:00c3992af9f4 | 274 | scope.set(0,reference1); |
Zeekat | 0:f6306e179750 | 275 | double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor |
Zeekat | 8:00c3992af9f4 | 276 | scope.set(1,rads1); |
Zeekat | 3:7273bbe6aa02 | 277 | double error1 = (reference1 - rads1); // determine the error (reference - position) |
Zeekat | 1:2f23368e8638 | 278 | double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err); |
Zeekat | 8:00c3992af9f4 | 279 | //pc.printf("output 1 %f \n",output1); |
Zeekat | 8:00c3992af9f4 | 280 | scope.set(2,output1); |
Zeekat | 8:00c3992af9f4 | 281 | scope.send(); |
Zeekat | 8:00c3992af9f4 | 282 | output1 = outputlimiter(output1,1); |
Zeekat | 0:f6306e179750 | 283 | if(output1 > 0) { // uses the calculated output to determine the direction of the motor |
Zeekat | 0:f6306e179750 | 284 | motor1_rich.write(0); |
Zeekat | 0:f6306e179750 | 285 | motor1_aan.write(output1); |
Zeekat | 0:f6306e179750 | 286 | } else if(output1 < 0) { |
Zeekat | 0:f6306e179750 | 287 | motor1_rich.write(1); |
Zeekat | 0:f6306e179750 | 288 | motor1_aan.write(abs(output1)); |
Zeekat | 0:f6306e179750 | 289 | } |
Zeekat | 0:f6306e179750 | 290 | } |
Zeekat | 0:f6306e179750 | 291 | |
Zeekat | 0:f6306e179750 | 292 | // MOTOR 2 |
Zeekat | 0:f6306e179750 | 293 | void motor2_control() |
Zeekat | 0:f6306e179750 | 294 | { |
Zeekat | 5:67afe491a1e3 | 295 | double input2 = potleft.read()*signalamp2; // replace potleft with filter |
Zeekat | 7:4df9f6ebe744 | 296 | // first input limiter |
Zeekat | 7:4df9f6ebe744 | 297 | if(input2 < input_threshold) {input2 = 0;} |
Zeekat | 6:9c8e91bb1316 | 298 | if(input2 > 1) {input2 = 1;} |
Zeekat | 7:4df9f6ebe744 | 299 | // reverse direction |
Zeekat | 6:9c8e91bb1316 | 300 | if(reverse_links == true) {input2 = -input2;} |
Zeekat | 8:00c3992af9f4 | 301 | double reference2 = reference_f(input2, c_reference2,limlow2,limhigh2); // determine the reference that has been set by the clamped inputsignal |
Zeekat | 0:f6306e179750 | 302 | double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor |
Zeekat | 3:7273bbe6aa02 | 303 | double error2 = (reference2 - rads2); // determine the error (reference - position) |
Zeekat | 8:00c3992af9f4 | 304 | //scope.set(1,error2); |
Zeekat | 8:00c3992af9f4 | 305 | //scope.send(); |
Zeekat | 1:2f23368e8638 | 306 | double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err); |
Zeekat | 8:00c3992af9f4 | 307 | output2 = outputlimiter(output2,1); |
Zeekat | 0:f6306e179750 | 308 | if(output2 > 0) { // uses the calculated output to determine the direction of the motor |
Zeekat | 0:f6306e179750 | 309 | motor2_rich.write(0); |
Zeekat | 0:f6306e179750 | 310 | motor2_aan.write(output2); |
Zeekat | 0:f6306e179750 | 311 | } else if(output2 < 0) { |
Zeekat | 0:f6306e179750 | 312 | motor2_rich.write(1); |
Zeekat | 0:f6306e179750 | 313 | motor2_aan.write(abs(output2)); |
Zeekat | 0:f6306e179750 | 314 | } |
Zeekat | 0:f6306e179750 | 315 | } |
Zeekat | 0:f6306e179750 | 316 | |
Zeekat | 0:f6306e179750 | 317 | |
Zeekat | 2:2563df4ee829 | 318 | ////////////////////////////////////////////////////////////////// |
Zeekat | 2:2563df4ee829 | 319 | //////////// DEFINE GO-FLAG FUNCTIONS /////////////////////////// |
Zeekat | 2:2563df4ee829 | 320 | //////////////////////////////////////////////////////////////// |
Zeekat | 2:2563df4ee829 | 321 | |
Zeekat | 2:2563df4ee829 | 322 | void motor1_activate() |
Zeekat | 2:2563df4ee829 | 323 | { |
Zeekat | 2:2563df4ee829 | 324 | motor1_go = true; |
Zeekat | 2:2563df4ee829 | 325 | } |
Zeekat | 2:2563df4ee829 | 326 | |
Zeekat | 2:2563df4ee829 | 327 | void motor2_activate() |
Zeekat | 2:2563df4ee829 | 328 | { |
Zeekat | 2:2563df4ee829 | 329 | motor2_go = true; |
Zeekat | 2:2563df4ee829 | 330 | } |
Zeekat | 2:2563df4ee829 | 331 | |
Zeekat | 0:f6306e179750 | 332 | int main() |
Zeekat | 0:f6306e179750 | 333 | { |
Zeekat | 2:2563df4ee829 | 334 | pc.baud(115200); |
Zeekat | 4:072b99947fc6 | 335 | controller1.attach(&motor1_activate, controlstep); // call a go-flag |
Zeekat | 8:00c3992af9f4 | 336 | controller2.attach(&motor2_activate, controlstep); // Disabled while debugging PID-controller ?? |
Zeekat | 0:f6306e179750 | 337 | while(true) |
Zeekat | 0:f6306e179750 | 338 | { |
Zeekat | 6:9c8e91bb1316 | 339 | // button press functions |
Zeekat | 6:9c8e91bb1316 | 340 | // flow buttons |
Zeekat | 4:072b99947fc6 | 341 | if(buttonlinks.read() == 0) |
Zeekat | 2:2563df4ee829 | 342 | { |
Zeekat | 6:9c8e91bb1316 | 343 | loop_start = !loop_start; |
Zeekat | 4:072b99947fc6 | 344 | wait(buttonlinks.read() == 1); |
Zeekat | 6:9c8e91bb1316 | 345 | wait(0.3); |
Zeekat | 2:2563df4ee829 | 346 | } |
Zeekat | 4:072b99947fc6 | 347 | if(buttonrechts.read() == 0) |
Zeekat | 4:072b99947fc6 | 348 | { |
Zeekat | 6:9c8e91bb1316 | 349 | calib_start = !calib_start; |
Zeekat | 4:072b99947fc6 | 350 | wait(buttonrechts.read() == 1); |
Zeekat | 6:9c8e91bb1316 | 351 | wait(0.3); |
Zeekat | 4:072b99947fc6 | 352 | } |
Zeekat | 6:9c8e91bb1316 | 353 | // reverse buttons |
Zeekat | 6:9c8e91bb1316 | 354 | if(reverse_button_links.read() == 0) |
Zeekat | 6:9c8e91bb1316 | 355 | { |
Zeekat | 7:4df9f6ebe744 | 356 | reverse_links = !reverse_links; |
Zeekat | 7:4df9f6ebe744 | 357 | wait(reverse_button_links.read() == 1); |
Zeekat | 7:4df9f6ebe744 | 358 | wait(0.3); |
Zeekat | 6:9c8e91bb1316 | 359 | } |
Zeekat | 7:4df9f6ebe744 | 360 | if(reverse_button_rechts.read() == 0) |
Zeekat | 6:9c8e91bb1316 | 361 | { |
Zeekat | 6:9c8e91bb1316 | 362 | reverse_rechts = !reverse_rechts; |
Zeekat | 6:9c8e91bb1316 | 363 | wait(reverse_button_rechts.read() == 1); |
Zeekat | 6:9c8e91bb1316 | 364 | wait(0.3); |
Zeekat | 6:9c8e91bb1316 | 365 | } |
Zeekat | 4:072b99947fc6 | 366 | // Main Control stuff and options |
Zeekat | 4:072b99947fc6 | 367 | if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops |
Zeekat | 2:2563df4ee829 | 368 | { |
Zeekat | 5:67afe491a1e3 | 369 | LED(1,1,0); // turn blue led on |
Zeekat | 7:4df9f6ebe744 | 370 | if(motor1_go) { motor1_go = false; motor1_control();} |
Zeekat | 7:4df9f6ebe744 | 371 | if(motor2_go) { motor2_go = false; motor2_control();} |
Zeekat | 2:2563df4ee829 | 372 | } |
Zeekat | 7:4df9f6ebe744 | 373 | // turn green led on // start calibration procedures |
Zeekat | 8:00c3992af9f4 | 374 | if(loop_start == false && calib_start == true) { LED(1,0,1); motor1_aan.write(0); motor2_aan.write(0);} |
Zeekat | 7:4df9f6ebe744 | 375 | // turn red led on |
Zeekat | 7:4df9f6ebe744 | 376 | if(loop_start == true && calib_start == true) { LED(0,1,1);} |
Zeekat | 7:4df9f6ebe744 | 377 | // turn leds off (both buttons false) |
Zeekat | 7:4df9f6ebe744 | 378 | else { LED(1,1,1);} |
Zeekat | 0:f6306e179750 | 379 | } |
Zeekat | 0:f6306e179750 | 380 | } |