control flow doet nog niks
Dependencies: Encoder HIDScope MODSERIAL mbed
main.cpp@6:9c8e91bb1316, 2015-10-10 (annotated)
- Committer:
- Zeekat
- Date:
- Sat Oct 10 20:38:38 2015 +0000
- Revision:
- 6:9c8e91bb1316
- Parent:
- 5:67afe491a1e3
- Child:
- 7:4df9f6ebe744
added extra buttons to reverse motion, and removed the mirrored pot input. now pot can be directly replaced with filtered input without changing anything (i hope). the gain determination has not been implemented yet.
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 | 2:2563df4ee829 | 7 | HIDScope scope(2); // 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 | 3:7273bbe6aa02 | 61 | // Define signal amplification (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!) ?? |
Zeekat | 3:7273bbe6aa02 | 62 | double signalamp1 = 1; |
Zeekat | 3:7273bbe6aa02 | 63 | double signalamp2 = 1; |
Zeekat | 6:9c8e91bb1316 | 64 | // Define storage variables for reference values |
Zeekat | 3:7273bbe6aa02 | 65 | double c_reference1 = 0; |
Zeekat | 3:7273bbe6aa02 | 66 | double c_reference2 = 0; |
Zeekat | 3:7273bbe6aa02 | 67 | // Define the maximum rate of change for the reference (velocity) |
Zeekat | 2:2563df4ee829 | 68 | double Vmax = 5; // rad/sec |
Zeekat | 2:2563df4ee829 | 69 | |
Zeekat | 0:f6306e179750 | 70 | // CONTROLLER SETTINGS |
Zeekat | 1:2f23368e8638 | 71 | // motor 1 |
Zeekat | 1:2f23368e8638 | 72 | const double m1_Kp = 1; // Proportional constant |
Zeekat | 2:2563df4ee829 | 73 | const double m1_Ki = 1; // integration constant |
Zeekat | 1:2f23368e8638 | 74 | const double m1_Kd = 0; // differentiation constant |
Zeekat | 1:2f23368e8638 | 75 | // motor 2 |
Zeekat | 1:2f23368e8638 | 76 | const double m2_Kp = 1; |
Zeekat | 1:2f23368e8638 | 77 | const double m2_Ki = 0; |
Zeekat | 1:2f23368e8638 | 78 | const double m2_Kd = 0; |
Zeekat | 1:2f23368e8638 | 79 | // storage variables |
Zeekat | 1:2f23368e8638 | 80 | // motor 1 |
Zeekat | 1:2f23368e8638 | 81 | double m1_err_int = 0; |
Zeekat | 1:2f23368e8638 | 82 | double m1_prev_err = 0; |
Zeekat | 1:2f23368e8638 | 83 | // motor 2 |
Zeekat | 1:2f23368e8638 | 84 | double m2_err_int = 0; |
Zeekat | 1:2f23368e8638 | 85 | double m2_prev_err = 0; |
Zeekat | 0:f6306e179750 | 86 | |
Zeekat | 0:f6306e179750 | 87 | |
Zeekat | 1:2f23368e8638 | 88 | //// FILTER VARIABLES |
Zeekat | 1:2f23368e8638 | 89 | // storage variables |
Zeekat | 1:2f23368e8638 | 90 | // differential action filter, same is used for both controllers |
Zeekat | 1:2f23368e8638 | 91 | double m_f_v1 = 0; |
Zeekat | 1:2f23368e8638 | 92 | double m_f_v2 = 0; |
Zeekat | 1:2f23368e8638 | 93 | |
Zeekat | 1:2f23368e8638 | 94 | // Filter coefficients |
Zeekat | 6:9c8e91bb1316 | 95 | // differential action filter |
Zeekat | 6:9c8e91bb1316 | 96 | const double m_f_a1 = 1.0, m_f_a2 = 2.0, m_f_b0 = 1.0, m_f_b1 = 3.0, m_f_b2 = 4.0; // coefficients from sheets are used as first test. |
Zeekat | 5:67afe491a1e3 | 97 | |
Zeekat | 5:67afe491a1e3 | 98 | // tweede orde notch filter 50 Hz |
Zeekat | 5:67afe491a1e3 | 99 | // biquad 1 coefficienten |
Zeekat | 5:67afe491a1e3 | 100 | const double numnotch50biq1_1 = 1; |
Zeekat | 5:67afe491a1e3 | 101 | const double numnotch50biq1_2 = -1.61816178466632; |
Zeekat | 5:67afe491a1e3 | 102 | const double numnotch50biq1_3 = 1.00000006127058; |
Zeekat | 5:67afe491a1e3 | 103 | const double dennotch50biq1_2 = -1.59325742941798; |
Zeekat | 5:67afe491a1e3 | 104 | const double dennotch50biq1_3 = 0.982171881701431; |
Zeekat | 5:67afe491a1e3 | 105 | // biquad 2 coefficienten |
Zeekat | 5:67afe491a1e3 | 106 | const double numnotch50biq2_1 = 1; |
Zeekat | 5:67afe491a1e3 | 107 | const double numnotch50biq2_2 = -1.61816171933244; |
Zeekat | 5:67afe491a1e3 | 108 | const double numnotch50biq2_3 = 0.999999938729428; |
Zeekat | 5:67afe491a1e3 | 109 | const double dennotch50biq2_2 = -1.61431180968071; |
Zeekat | 5:67afe491a1e3 | 110 | const double dennotch50biq2_3 = 0.982599066293075; |
Zeekat | 5:67afe491a1e3 | 111 | // highpass filter 20 Hz coefficienten |
Zeekat | 5:67afe491a1e3 | 112 | const double numhigh20_1 = 0.837089190566345; |
Zeekat | 5:67afe491a1e3 | 113 | const double numhigh20_2 = -1.67417838113269; |
Zeekat | 5:67afe491a1e3 | 114 | const double numhigh20_3 = 0.837089190566345; |
Zeekat | 5:67afe491a1e3 | 115 | const double denhigh20_2 = -1.64745998107698; |
Zeekat | 5:67afe491a1e3 | 116 | const double denhigh20_3 = 0.700896781188403; |
Zeekat | 5:67afe491a1e3 | 117 | // lowpass 5 Hz coefficienten |
Zeekat | 5:67afe491a1e3 | 118 | const double numlow5_1 =0.000944691843840162; |
Zeekat | 5:67afe491a1e3 | 119 | const double numlow5_2 =0.00188938368768032; |
Zeekat | 5:67afe491a1e3 | 120 | const double numlow5_3 =0.000944691843840162; |
Zeekat | 5:67afe491a1e3 | 121 | const double denlow5_2 =-1.91119706742607; |
Zeekat | 5:67afe491a1e3 | 122 | const double denlow5_3 =0.914975834801434; |
Zeekat | 5:67afe491a1e3 | 123 | |
Zeekat | 5:67afe491a1e3 | 124 | // Define the storage variables and filter coeicients for two filters |
Zeekat | 5:67afe491a1e3 | 125 | 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 | 126 | |
Zeekat | 0:f6306e179750 | 127 | |
Zeekat | 0:f6306e179750 | 128 | |
Zeekat | 0:f6306e179750 | 129 | //////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 130 | /////////////////// START OF SIDE FUNCTIONS //////////////////// |
Zeekat | 0:f6306e179750 | 131 | ////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 132 | // these functions are tailored to perform 1 specific function |
Zeekat | 0:f6306e179750 | 133 | |
Zeekat | 5:67afe491a1e3 | 134 | // this funtion flips leds on and off accordin to input with 0 being on |
Zeekat | 5:67afe491a1e3 | 135 | void LED(int red,int green,int blue) |
Zeekat | 5:67afe491a1e3 | 136 | { |
Zeekat | 5:67afe491a1e3 | 137 | ledred.write(red); |
Zeekat | 5:67afe491a1e3 | 138 | ledgreen.write(green); |
Zeekat | 5:67afe491a1e3 | 139 | ledblue.write(blue); |
Zeekat | 5:67afe491a1e3 | 140 | } |
Zeekat | 5:67afe491a1e3 | 141 | |
Zeekat | 0:f6306e179750 | 142 | // counts 2 radians |
Zeekat | 0:f6306e179750 | 143 | // this function takes counts from the encoder and converts it to the amount of radians from the zero position. |
Zeekat | 0:f6306e179750 | 144 | // It has been set up for standard 2X DECODING!!!!!! |
Zeekat | 0:f6306e179750 | 145 | double get_radians(double counts) |
Zeekat | 0:f6306e179750 | 146 | { |
Zeekat | 0:f6306e179750 | 147 | double pi = 3.14159265359; |
Zeekat | 0:f6306e179750 | 148 | double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, extra warning) |
Zeekat | 0:f6306e179750 | 149 | return radians; |
Zeekat | 0:f6306e179750 | 150 | } |
Zeekat | 0:f6306e179750 | 151 | |
Zeekat | 0:f6306e179750 | 152 | |
Zeekat | 3:7273bbe6aa02 | 153 | // This functions takes a 0->1 input converts it to -1->1 and uses passing by reference (&c_reference) |
Zeekat | 0:f6306e179750 | 154 | // to create a reference that moves with a variable speed. It is now set up for potmeter values. |
Zeekat | 3:7273bbe6aa02 | 155 | double reference_f(double input, double &c_reference) |
Zeekat | 0:f6306e179750 | 156 | { |
Zeekat | 6:9c8e91bb1316 | 157 | double reference = c_reference + input * controlstep * Vmax ; |
Zeekat | 3:7273bbe6aa02 | 158 | c_reference = reference; // change the global variable to the latest location. |
Zeekat | 3:7273bbe6aa02 | 159 | return reference; |
Zeekat | 0:f6306e179750 | 160 | } |
Zeekat | 0:f6306e179750 | 161 | |
Zeekat | 0:f6306e179750 | 162 | |
Zeekat | 3:7273bbe6aa02 | 163 | // This function takes the controller outputvalue and ensures it is between -1 and 1 |
Zeekat | 0:f6306e179750 | 164 | // 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 | 165 | // needs more work to use it for the wind-up prevention. |
Zeekat | 2:2563df4ee829 | 166 | double outputlimiter (double output, double limit) |
Zeekat | 0:f6306e179750 | 167 | { |
Zeekat | 2:2563df4ee829 | 168 | if(output> limit) |
Zeekat | 0:f6306e179750 | 169 | { |
Zeekat | 0:f6306e179750 | 170 | output = 1; |
Zeekat | 0:f6306e179750 | 171 | } |
Zeekat | 2:2563df4ee829 | 172 | else if(output < limit && output > 0) |
Zeekat | 0:f6306e179750 | 173 | { |
Zeekat | 0:f6306e179750 | 174 | output = output; |
Zeekat | 0:f6306e179750 | 175 | } |
Zeekat | 2:2563df4ee829 | 176 | else if(output > -limit && output < 0) |
Zeekat | 0:f6306e179750 | 177 | { |
Zeekat | 0:f6306e179750 | 178 | output = output; |
Zeekat | 0:f6306e179750 | 179 | } |
Zeekat | 2:2563df4ee829 | 180 | else if(output < -limit) |
Zeekat | 0:f6306e179750 | 181 | { |
Zeekat | 0:f6306e179750 | 182 | (output = -1); |
Zeekat | 0:f6306e179750 | 183 | } |
Zeekat | 0:f6306e179750 | 184 | return output; |
Zeekat | 0:f6306e179750 | 185 | } |
Zeekat | 0:f6306e179750 | 186 | |
Zeekat | 0:f6306e179750 | 187 | |
Zeekat | 1:2f23368e8638 | 188 | // BIQUADFILTER CODE GIVEN IN SHEETS |
Zeekat | 1:2f23368e8638 | 189 | // used for d-control |
Zeekat | 1:2f23368e8638 | 190 | 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 | 191 | { |
Zeekat | 3:7273bbe6aa02 | 192 | double v = u - a1*v1 - a2*v2; |
Zeekat | 3:7273bbe6aa02 | 193 | double y = b0*v + b1*v1 + b2*v2; |
Zeekat | 1:2f23368e8638 | 194 | v2 = v1; |
Zeekat | 1:2f23368e8638 | 195 | v1 = v; |
Zeekat | 1:2f23368e8638 | 196 | return y; |
Zeekat | 1:2f23368e8638 | 197 | } |
Zeekat | 1:2f23368e8638 | 198 | |
Zeekat | 5:67afe491a1e3 | 199 | double EMG_Filter(double u1) |
Zeekat | 5:67afe491a1e3 | 200 | { |
Zeekat | 5:67afe491a1e3 | 201 | // double u1 = potright.read(); |
Zeekat | 5:67afe491a1e3 | 202 | double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); |
Zeekat | 5:67afe491a1e3 | 203 | double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); |
Zeekat | 5:67afe491a1e3 | 204 | double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); |
Zeekat | 5:67afe491a1e3 | 205 | double y4 = abs(y3); |
Zeekat | 5:67afe491a1e3 | 206 | double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); |
Zeekat | 5:67afe491a1e3 | 207 | return y5; |
Zeekat | 5:67afe491a1e3 | 208 | } |
Zeekat | 1:2f23368e8638 | 209 | |
Zeekat | 1:2f23368e8638 | 210 | // PID Controller given in sheets |
Zeekat | 1:2f23368e8638 | 211 | // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!) |
Zeekat | 2:2563df4ee829 | 212 | double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev) |
Zeekat | 1:2f23368e8638 | 213 | { |
Zeekat | 1:2f23368e8638 | 214 | // Proportional |
Zeekat | 1:2f23368e8638 | 215 | double P = Kp * e; |
Zeekat | 3:7273bbe6aa02 | 216 | pc.printf("P = %f, ",P); |
Zeekat | 1:2f23368e8638 | 217 | // Integral |
Zeekat | 1:2f23368e8638 | 218 | e_int = e_int + Ts * e; |
Zeekat | 1:2f23368e8638 | 219 | double I = e_int * Ki; |
Zeekat | 3:7273bbe6aa02 | 220 | pc.printf("I = %f, ",I); |
Zeekat | 3:7273bbe6aa02 | 221 | // Derivative (Disabled for now because of NaN problem from filter |
Zeekat | 3:7273bbe6aa02 | 222 | double e_derr = (e - e_prev)/Ts; |
Zeekat | 3:7273bbe6aa02 | 223 | pc.printf("e_derr %f, ",e_derr); |
Zeekat | 3:7273bbe6aa02 | 224 | 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 | 3:7273bbe6aa02 | 225 | pc.printf("fil_e_derr %f, ",e_derr); // check for NaN ?? |
Zeekat | 3:7273bbe6aa02 | 226 | e_prev = e; |
Zeekat | 3:7273bbe6aa02 | 227 | // double D = Kd* e_derr; |
Zeekat | 1:2f23368e8638 | 228 | |
Zeekat | 1:2f23368e8638 | 229 | // PID |
Zeekat | 2:2563df4ee829 | 230 | double output = P + I ;// + D; |
Zeekat | 1:2f23368e8638 | 231 | return output; |
Zeekat | 1:2f23368e8638 | 232 | } |
Zeekat | 1:2f23368e8638 | 233 | |
Zeekat | 1:2f23368e8638 | 234 | |
Zeekat | 1:2f23368e8638 | 235 | |
Zeekat | 0:f6306e179750 | 236 | // this function is a simple K control called by the motor function |
Zeekat | 0:f6306e179750 | 237 | double K_control(double error,double K) |
Zeekat | 0:f6306e179750 | 238 | { |
Zeekat | 0:f6306e179750 | 239 | double output = K*error; // controller output K*e |
Zeekat | 2:2563df4ee829 | 240 | output = outputlimiter(output,1); // limit the output to -1->1 |
Zeekat | 0:f6306e179750 | 241 | return output; |
Zeekat | 0:f6306e179750 | 242 | } |
Zeekat | 0:f6306e179750 | 243 | |
Zeekat | 0:f6306e179750 | 244 | ///////////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 245 | ////////////////// PRIMARY CONTROL FUNCTIONS /////////////////////// |
Zeekat | 0:f6306e179750 | 246 | /////////////////////////////////////////////////////////////////// |
Zeekat | 0:f6306e179750 | 247 | // these functions are used to control all aspects of a single electric motor and are called by the main function |
Zeekat | 0:f6306e179750 | 248 | |
Zeekat | 0:f6306e179750 | 249 | // MOTOR 1 |
Zeekat | 0:f6306e179750 | 250 | void motor1_control() |
Zeekat | 0:f6306e179750 | 251 | { |
Zeekat | 5:67afe491a1e3 | 252 | |
Zeekat | 3:7273bbe6aa02 | 253 | double input1 = potright.read()*signalamp1; |
Zeekat | 6:9c8e91bb1316 | 254 | if(input1 < 0.25) {input1 = 0;} |
Zeekat | 6:9c8e91bb1316 | 255 | if(input1 > 1) {input1 = 1;} |
Zeekat | 6:9c8e91bb1316 | 256 | if(reverse_rechts == true) {input1 = -input1;} |
Zeekat | 3:7273bbe6aa02 | 257 | double reference1 = reference_f(input1, c_reference1); // determine the reference that has been set by the inputsignal |
Zeekat | 0:f6306e179750 | 258 | double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor |
Zeekat | 3:7273bbe6aa02 | 259 | double error1 = (reference1 - rads1); // determine the error (reference - position) |
Zeekat | 1:2f23368e8638 | 260 | double output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err); |
Zeekat | 2:2563df4ee829 | 261 | pc.printf("output 1 %f \n",output1); |
Zeekat | 0:f6306e179750 | 262 | if(output1 > 0) { // uses the calculated output to determine the direction of the motor |
Zeekat | 0:f6306e179750 | 263 | motor1_rich.write(0); |
Zeekat | 0:f6306e179750 | 264 | motor1_aan.write(output1); |
Zeekat | 0:f6306e179750 | 265 | } else if(output1 < 0) { |
Zeekat | 0:f6306e179750 | 266 | motor1_rich.write(1); |
Zeekat | 0:f6306e179750 | 267 | motor1_aan.write(abs(output1)); |
Zeekat | 0:f6306e179750 | 268 | } |
Zeekat | 0:f6306e179750 | 269 | } |
Zeekat | 0:f6306e179750 | 270 | |
Zeekat | 0:f6306e179750 | 271 | // MOTOR 2 |
Zeekat | 0:f6306e179750 | 272 | void motor2_control() |
Zeekat | 0:f6306e179750 | 273 | { |
Zeekat | 5:67afe491a1e3 | 274 | double input2 = potleft.read()*signalamp2; // replace potleft with filter |
Zeekat | 6:9c8e91bb1316 | 275 | if(input2 < 0.25) {input2 = 0;} |
Zeekat | 6:9c8e91bb1316 | 276 | if(input2 > 1) {input2 = 1;} |
Zeekat | 6:9c8e91bb1316 | 277 | if(reverse_links == true) {input2 = -input2;} |
Zeekat | 6:9c8e91bb1316 | 278 | double reference2 = reference_f(input2, c_reference2); // determine the reference that has been set by the clamped inputsignal |
Zeekat | 0:f6306e179750 | 279 | double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor |
Zeekat | 3:7273bbe6aa02 | 280 | double error2 = (reference2 - rads2); // determine the error (reference - position) |
Zeekat | 1:2f23368e8638 | 281 | double output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err); |
Zeekat | 0:f6306e179750 | 282 | if(output2 > 0) { // uses the calculated output to determine the direction of the motor |
Zeekat | 0:f6306e179750 | 283 | motor2_rich.write(0); |
Zeekat | 0:f6306e179750 | 284 | motor2_aan.write(output2); |
Zeekat | 0:f6306e179750 | 285 | } else if(output2 < 0) { |
Zeekat | 0:f6306e179750 | 286 | motor2_rich.write(1); |
Zeekat | 0:f6306e179750 | 287 | motor2_aan.write(abs(output2)); |
Zeekat | 0:f6306e179750 | 288 | } |
Zeekat | 0:f6306e179750 | 289 | } |
Zeekat | 0:f6306e179750 | 290 | |
Zeekat | 0:f6306e179750 | 291 | |
Zeekat | 2:2563df4ee829 | 292 | ////////////////////////////////////////////////////////////////// |
Zeekat | 2:2563df4ee829 | 293 | //////////// DEFINE GO-FLAG FUNCTIONS /////////////////////////// |
Zeekat | 2:2563df4ee829 | 294 | //////////////////////////////////////////////////////////////// |
Zeekat | 2:2563df4ee829 | 295 | |
Zeekat | 2:2563df4ee829 | 296 | void motor1_activate() |
Zeekat | 2:2563df4ee829 | 297 | { |
Zeekat | 2:2563df4ee829 | 298 | motor1_go = true; |
Zeekat | 2:2563df4ee829 | 299 | } |
Zeekat | 2:2563df4ee829 | 300 | |
Zeekat | 2:2563df4ee829 | 301 | void motor2_activate() |
Zeekat | 2:2563df4ee829 | 302 | { |
Zeekat | 2:2563df4ee829 | 303 | motor2_go = true; |
Zeekat | 2:2563df4ee829 | 304 | } |
Zeekat | 2:2563df4ee829 | 305 | |
Zeekat | 0:f6306e179750 | 306 | int main() |
Zeekat | 0:f6306e179750 | 307 | { |
Zeekat | 2:2563df4ee829 | 308 | pc.baud(115200); |
Zeekat | 4:072b99947fc6 | 309 | controller1.attach(&motor1_activate, controlstep); // call a go-flag |
Zeekat | 4:072b99947fc6 | 310 | // controller2.attach(&motor2_activate, controlstep); Disabled while debugging PID-controller ?? |
Zeekat | 0:f6306e179750 | 311 | while(true) |
Zeekat | 0:f6306e179750 | 312 | { |
Zeekat | 6:9c8e91bb1316 | 313 | // button press functions |
Zeekat | 6:9c8e91bb1316 | 314 | // flow buttons |
Zeekat | 4:072b99947fc6 | 315 | if(buttonlinks.read() == 0) |
Zeekat | 2:2563df4ee829 | 316 | { |
Zeekat | 6:9c8e91bb1316 | 317 | loop_start = !loop_start; |
Zeekat | 4:072b99947fc6 | 318 | wait(buttonlinks.read() == 1); |
Zeekat | 6:9c8e91bb1316 | 319 | wait(0.3); |
Zeekat | 2:2563df4ee829 | 320 | } |
Zeekat | 4:072b99947fc6 | 321 | if(buttonrechts.read() == 0) |
Zeekat | 4:072b99947fc6 | 322 | { |
Zeekat | 6:9c8e91bb1316 | 323 | calib_start = !calib_start; |
Zeekat | 4:072b99947fc6 | 324 | wait(buttonrechts.read() == 1); |
Zeekat | 6:9c8e91bb1316 | 325 | wait(0.3); |
Zeekat | 4:072b99947fc6 | 326 | } |
Zeekat | 6:9c8e91bb1316 | 327 | // reverse buttons |
Zeekat | 6:9c8e91bb1316 | 328 | if(reverse_button_links.read() == 0) |
Zeekat | 6:9c8e91bb1316 | 329 | { |
Zeekat | 6:9c8e91bb1316 | 330 | reverse_links = !reverse_links; |
Zeekat | 6:9c8e91bb1316 | 331 | wait(reverse_button_links.read() == 1); |
Zeekat | 6:9c8e91bb1316 | 332 | wait(0.3); |
Zeekat | 6:9c8e91bb1316 | 333 | } |
Zeekat | 6:9c8e91bb1316 | 334 | if(reverse_button_rechts.read() == 0) |
Zeekat | 6:9c8e91bb1316 | 335 | { |
Zeekat | 6:9c8e91bb1316 | 336 | reverse_rechts = !reverse_rechts; |
Zeekat | 6:9c8e91bb1316 | 337 | wait(reverse_button_rechts.read() == 1); |
Zeekat | 6:9c8e91bb1316 | 338 | wait(0.3); |
Zeekat | 6:9c8e91bb1316 | 339 | } |
Zeekat | 4:072b99947fc6 | 340 | // Main Control stuff and options |
Zeekat | 4:072b99947fc6 | 341 | if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops |
Zeekat | 2:2563df4ee829 | 342 | { |
Zeekat | 5:67afe491a1e3 | 343 | LED(1,1,0); // turn blue led on |
Zeekat | 4:072b99947fc6 | 344 | |
Zeekat | 4:072b99947fc6 | 345 | if(motor1_go) |
Zeekat | 4:072b99947fc6 | 346 | { |
Zeekat | 4:072b99947fc6 | 347 | motor1_go = false; |
Zeekat | 4:072b99947fc6 | 348 | motor1_control(); |
Zeekat | 4:072b99947fc6 | 349 | } |
Zeekat | 4:072b99947fc6 | 350 | if(motor2_go) |
Zeekat | 4:072b99947fc6 | 351 | { |
Zeekat | 4:072b99947fc6 | 352 | motor2_go = false; |
Zeekat | 4:072b99947fc6 | 353 | motor2_control(); |
Zeekat | 4:072b99947fc6 | 354 | } |
Zeekat | 2:2563df4ee829 | 355 | } |
Zeekat | 4:072b99947fc6 | 356 | if(loop_start == false && calib_start == true) // start calibration procedures |
Zeekat | 4:072b99947fc6 | 357 | { |
Zeekat | 5:67afe491a1e3 | 358 | LED(1,0,1); // turn green led on |
Zeekat | 4:072b99947fc6 | 359 | } |
Zeekat | 4:072b99947fc6 | 360 | if(loop_start == true && calib_start == true) // check if both buttons are enabled and stop everything |
Zeekat | 0:f6306e179750 | 361 | { |
Zeekat | 5:67afe491a1e3 | 362 | LED(0,1,1); // turn red led on |
Zeekat | 4:072b99947fc6 | 363 | } |
Zeekat | 4:072b99947fc6 | 364 | else |
Zeekat | 4:072b99947fc6 | 365 | { |
Zeekat | 5:67afe491a1e3 | 366 | LED(1,1,1); // turn leds off (both buttons false) |
Zeekat | 0:f6306e179750 | 367 | } |
Zeekat | 0:f6306e179750 | 368 | } |
Zeekat | 0:f6306e179750 | 369 | } |