werkend x-y control
Dependencies: Encoder HIDScope MODSERIAL mbed
main.cpp@2:867a1eb33bbe, 2015-10-20 (annotated)
- Committer:
- Zeekat
- Date:
- Tue Oct 20 07:56:11 2015 +0000
- Revision:
- 2:867a1eb33bbe
- Parent:
- 1:f3910e46b831
- Child:
- 3:a73502236647
'working' x-y input. still some vibrations due to low tension belt
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Zeekat | 0:a643b1b38abe | 1 | #include "mbed.h" |
Zeekat | 0:a643b1b38abe | 2 | #include "MODSERIAL.h" |
Zeekat | 0:a643b1b38abe | 3 | #include "encoder.h" |
Zeekat | 0:a643b1b38abe | 4 | #include "HIDScope.h" |
Zeekat | 1:f3910e46b831 | 5 | #include "math.h" |
Zeekat | 0:a643b1b38abe | 6 | |
Zeekat | 0:a643b1b38abe | 7 | Serial pc(USBTX,USBRX); |
Zeekat | 2:867a1eb33bbe | 8 | HIDScope scope(4); // definieerd het aantal kanalen van de scope |
Zeekat | 0:a643b1b38abe | 9 | |
Zeekat | 0:a643b1b38abe | 10 | // Define Tickers and control frequencies |
Zeekat | 1:f3910e46b831 | 11 | Ticker controller1, controller2, main_filter, cartesian; // definieer de ticker die controler1 doet |
Zeekat | 0:a643b1b38abe | 12 | // Go flag variables |
Zeekat | 1:f3910e46b831 | 13 | volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false; |
Zeekat | 0:a643b1b38abe | 14 | |
Zeekat | 0:a643b1b38abe | 15 | // Frequency control |
Zeekat | 0:a643b1b38abe | 16 | double controlfreq = 50 ; // controlloops frequentie (Hz) |
Zeekat | 0:a643b1b38abe | 17 | double controlstep = 1/controlfreq; // timestep derived from controlfreq |
Zeekat | 0:a643b1b38abe | 18 | |
Zeekat | 0:a643b1b38abe | 19 | |
Zeekat | 0:a643b1b38abe | 20 | //MOTOR OUTPUTPINS |
Zeekat | 0:a643b1b38abe | 21 | // motor 1 |
Zeekat | 0:a643b1b38abe | 22 | PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets) |
Zeekat | 0:a643b1b38abe | 23 | DigitalOut motor1_rich(D7); // digitaal signaal voor richting |
Zeekat | 0:a643b1b38abe | 24 | // motor 2 |
Zeekat | 0:a643b1b38abe | 25 | PwmOut motor2_aan(D5); |
Zeekat | 0:a643b1b38abe | 26 | DigitalOut motor2_rich(D4); |
Zeekat | 0:a643b1b38abe | 27 | |
Zeekat | 0:a643b1b38abe | 28 | // ENCODER INPUTPINS |
Zeekat | 0:a643b1b38abe | 29 | Encoder motor1_enc(D12,D11); // encoder outputpins |
Zeekat | 0:a643b1b38abe | 30 | Encoder motor2_enc(D10,D9); |
Zeekat | 0:a643b1b38abe | 31 | |
Zeekat | 0:a643b1b38abe | 32 | int reference1 = 0; // set the reference position of the encoders (not used) |
Zeekat | 0:a643b1b38abe | 33 | int reference2 = 0; |
Zeekat | 0:a643b1b38abe | 34 | |
Zeekat | 1:f3910e46b831 | 35 | //////// |
Zeekat | 1:f3910e46b831 | 36 | // physical constants |
Zeekat | 1:f3910e46b831 | 37 | const double L = 36; // lenght of arms |
Zeekat | 1:f3910e46b831 | 38 | const double pi = 3.1415926535897; |
Zeekat | 1:f3910e46b831 | 39 | /////////////////////////// |
Zeekat | 1:f3910e46b831 | 40 | // Main control loop transfer variables |
Zeekat | 1:f3910e46b831 | 41 | // here all variables that transfer bewtween the primary control functions |
Zeekat | 1:f3910e46b831 | 42 | double output1; |
Zeekat | 1:f3910e46b831 | 43 | double output2; |
Zeekat | 1:f3910e46b831 | 44 | |
Zeekat | 2:867a1eb33bbe | 45 | double phi_one; |
Zeekat | 2:867a1eb33bbe | 46 | double phi_two; |
Zeekat | 1:f3910e46b831 | 47 | |
Zeekat | 0:a643b1b38abe | 48 | |
Zeekat | 0:a643b1b38abe | 49 | // EXTRA INPUTS AND REQUIRED VARIABLES |
Zeekat | 0:a643b1b38abe | 50 | //POTMETERS |
Zeekat | 0:a643b1b38abe | 51 | AnalogIn potright(A0); // define the potmeter outputpins |
Zeekat | 0:a643b1b38abe | 52 | AnalogIn potleft(A1); |
Zeekat | 1:f3910e46b831 | 53 | |
Zeekat | 1:f3910e46b831 | 54 | // Analoge input signalen defineren |
Zeekat | 1:f3910e46b831 | 55 | AnalogIn EMG_in(A2); // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen |
Zeekat | 1:f3910e46b831 | 56 | AnalogIn EMG_int(A3); // deze leest A3 uit |
Zeekat | 0:a643b1b38abe | 57 | |
Zeekat | 0:a643b1b38abe | 58 | // BUTTONS |
Zeekat | 0:a643b1b38abe | 59 | // control flow |
Zeekat | 0:a643b1b38abe | 60 | DigitalIn buttonlinks(PTA4); |
Zeekat | 0:a643b1b38abe | 61 | DigitalIn buttonrechts(PTC6); |
Zeekat | 0:a643b1b38abe | 62 | // init values |
Zeekat | 0:a643b1b38abe | 63 | bool loop_start = false; |
Zeekat | 0:a643b1b38abe | 64 | bool calib_start = false; |
Zeekat | 0:a643b1b38abe | 65 | |
Zeekat | 0:a643b1b38abe | 66 | // direction control |
Zeekat | 0:a643b1b38abe | 67 | DigitalIn reverse_button_links(D0); |
Zeekat | 0:a643b1b38abe | 68 | DigitalIn reverse_button_rechts(D1); |
Zeekat | 0:a643b1b38abe | 69 | // init values |
Zeekat | 0:a643b1b38abe | 70 | bool reverse_links = false; |
Zeekat | 0:a643b1b38abe | 71 | bool reverse_rechts = false; |
Zeekat | 0:a643b1b38abe | 72 | |
Zeekat | 0:a643b1b38abe | 73 | // LED |
Zeekat | 0:a643b1b38abe | 74 | DigitalOut ledred(LED1); |
Zeekat | 0:a643b1b38abe | 75 | DigitalOut ledgreen(LED2); |
Zeekat | 0:a643b1b38abe | 76 | DigitalOut ledblue(LED3); |
Zeekat | 0:a643b1b38abe | 77 | |
Zeekat | 0:a643b1b38abe | 78 | // REFERENCE SIGNAL SETTINGS |
Zeekat | 0:a643b1b38abe | 79 | double input_threshold = 0.25; // the minimum value the signal must have to change the reference. |
Zeekat | 0:a643b1b38abe | 80 | // Define signal amplification (needed with EMG, used in control loop, precise amp determination is a work in progress!!!!) ?? |
Zeekat | 0:a643b1b38abe | 81 | double signalamp1 = 1; |
Zeekat | 0:a643b1b38abe | 82 | double signalamp2 = 1; |
Zeekat | 0:a643b1b38abe | 83 | // Define storage variables for reference values |
Zeekat | 0:a643b1b38abe | 84 | double c_reference1 = 0; |
Zeekat | 0:a643b1b38abe | 85 | double c_reference2 = 0; |
Zeekat | 0:a643b1b38abe | 86 | // limit angles (in radians) |
Zeekat | 0:a643b1b38abe | 87 | // motor1 |
Zeekat | 0:a643b1b38abe | 88 | const double limlow1 = 0.5; // min height |
Zeekat | 0:a643b1b38abe | 89 | const double limhigh1 = 4.5; // max height |
Zeekat | 0:a643b1b38abe | 90 | // motor 2 |
Zeekat | 0:a643b1b38abe | 91 | const double limlow2 = -4.5; // maximum height, motor has been inverted due to transmission |
Zeekat | 0:a643b1b38abe | 92 | const double limhigh2 = 2; // minimum height |
Zeekat | 0:a643b1b38abe | 93 | |
Zeekat | 0:a643b1b38abe | 94 | // Define the maximum rate of change for the reference (velocity) |
Zeekat | 0:a643b1b38abe | 95 | double Vmax = 3; // rad/sec |
Zeekat | 0:a643b1b38abe | 96 | |
Zeekat | 0:a643b1b38abe | 97 | // CONTROLLER SETTINGS |
Zeekat | 0:a643b1b38abe | 98 | // motor 1 |
Zeekat | 0:a643b1b38abe | 99 | const double m1_Kp = 5; // Proportional constant |
Zeekat | 0:a643b1b38abe | 100 | const double m1_Ki = 0.5; // integration constant |
Zeekat | 0:a643b1b38abe | 101 | const double m1_Kd = 0.4; // differentiation constant |
Zeekat | 0:a643b1b38abe | 102 | // motor 2 |
Zeekat | 0:a643b1b38abe | 103 | const double m2_Kp = 2; |
Zeekat | 2:867a1eb33bbe | 104 | const double m2_Ki = 0.5; |
Zeekat | 0:a643b1b38abe | 105 | const double m2_Kd = 0.1; |
Zeekat | 0:a643b1b38abe | 106 | // storage variables |
Zeekat | 0:a643b1b38abe | 107 | // motor 1 |
Zeekat | 0:a643b1b38abe | 108 | double m1_err_int = 0; |
Zeekat | 0:a643b1b38abe | 109 | double m1_prev_err = 0; |
Zeekat | 0:a643b1b38abe | 110 | // motor 2 |
Zeekat | 0:a643b1b38abe | 111 | double m2_err_int = 0; |
Zeekat | 0:a643b1b38abe | 112 | double m2_prev_err = 0; |
Zeekat | 0:a643b1b38abe | 113 | |
Zeekat | 0:a643b1b38abe | 114 | |
Zeekat | 0:a643b1b38abe | 115 | //// FILTER VARIABLES |
Zeekat | 0:a643b1b38abe | 116 | // storage variables |
Zeekat | 0:a643b1b38abe | 117 | // differential action filter, same is used for both controllers |
Zeekat | 0:a643b1b38abe | 118 | double m_f_v1 = 0, m_f_v2 = 0; |
Zeekat | 0:a643b1b38abe | 119 | // input filter to smooth signal |
Zeekat | 0:a643b1b38abe | 120 | double r1_f_v1 = 0, r1_f_v2 = 0; |
Zeekat | 0:a643b1b38abe | 121 | double r2_f_v1 = 0, r2_f_v2 = 0; |
Zeekat | 0:a643b1b38abe | 122 | |
Zeekat | 0:a643b1b38abe | 123 | // Filter coefficients |
Zeekat | 0:a643b1b38abe | 124 | // differential action filter (lowpass 5Hz at 50samples) |
Zeekat | 0:a643b1b38abe | 125 | 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 | 0:a643b1b38abe | 126 | // input filter (lowpass 1Hz at 50samples) |
Zeekat | 0:a643b1b38abe | 127 | const double r1_f_a1 = -1.6475, r1_f_a2 = 0.7009, r1_f_b0 = 0.0134, r1_f_b1 = 0.0267, r1_f_b2 = 0.0134; |
Zeekat | 0:a643b1b38abe | 128 | |
Zeekat | 0:a643b1b38abe | 129 | // tweede orde notch filter 50 Hz |
Zeekat | 0:a643b1b38abe | 130 | // biquad 1 coefficienten |
Zeekat | 0:a643b1b38abe | 131 | const double numnotch50biq1_1 = 1; |
Zeekat | 0:a643b1b38abe | 132 | const double numnotch50biq1_2 = -1.61816178466632; |
Zeekat | 0:a643b1b38abe | 133 | const double numnotch50biq1_3 = 1.00000006127058; |
Zeekat | 0:a643b1b38abe | 134 | const double dennotch50biq1_2 = -1.59325742941798; |
Zeekat | 0:a643b1b38abe | 135 | const double dennotch50biq1_3 = 0.982171881701431; |
Zeekat | 0:a643b1b38abe | 136 | // biquad 2 coefficienten |
Zeekat | 0:a643b1b38abe | 137 | const double numnotch50biq2_1 = 1; |
Zeekat | 0:a643b1b38abe | 138 | const double numnotch50biq2_2 = -1.61816171933244; |
Zeekat | 0:a643b1b38abe | 139 | const double numnotch50biq2_3 = 0.999999938729428; |
Zeekat | 0:a643b1b38abe | 140 | const double dennotch50biq2_2 = -1.61431180968071; |
Zeekat | 0:a643b1b38abe | 141 | const double dennotch50biq2_3 = 0.982599066293075; |
Zeekat | 0:a643b1b38abe | 142 | // highpass filter 20 Hz coefficienten |
Zeekat | 0:a643b1b38abe | 143 | const double numhigh20_1 = 0.837089190566345; |
Zeekat | 0:a643b1b38abe | 144 | const double numhigh20_2 = -1.67417838113269; |
Zeekat | 0:a643b1b38abe | 145 | const double numhigh20_3 = 0.837089190566345; |
Zeekat | 0:a643b1b38abe | 146 | const double denhigh20_2 = -1.64745998107698; |
Zeekat | 0:a643b1b38abe | 147 | const double denhigh20_3 = 0.700896781188403; |
Zeekat | 0:a643b1b38abe | 148 | // lowpass 5 Hz coefficienten |
Zeekat | 0:a643b1b38abe | 149 | const double numlow5_1 =0.000944691843840162; |
Zeekat | 0:a643b1b38abe | 150 | const double numlow5_2 =0.00188938368768032; |
Zeekat | 0:a643b1b38abe | 151 | const double numlow5_3 =0.000944691843840162; |
Zeekat | 0:a643b1b38abe | 152 | const double denlow5_2 =-1.91119706742607; |
Zeekat | 0:a643b1b38abe | 153 | const double denlow5_3 =0.914975834801434; |
Zeekat | 0:a643b1b38abe | 154 | |
Zeekat | 1:f3910e46b831 | 155 | // Define the storage variables and filter coeficients for eight filters |
Zeekat | 1:f3910e46b831 | 156 | //filter 1 |
Zeekat | 1:f3910e46b831 | 157 | 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 | 1:f3910e46b831 | 158 | // filter2 |
Zeekat | 1:f3910e46b831 | 159 | double f1_v1t = 0, f1_v2t = 0, f2_v1t = 0, f2_v2t = 0, f3_v1t = 0, f3_v2t = 0,f4_v1t = 0, f4_v2t = 0; |
Zeekat | 0:a643b1b38abe | 160 | |
Zeekat | 0:a643b1b38abe | 161 | //////////////////////////////////////////////////////////////// |
Zeekat | 0:a643b1b38abe | 162 | /////////////////// START OF SIDE FUNCTIONS //////////////////// |
Zeekat | 0:a643b1b38abe | 163 | ////////////////////////////////////////////////////////////// |
Zeekat | 0:a643b1b38abe | 164 | // these functions are tailored to perform 1 specific function |
Zeekat | 0:a643b1b38abe | 165 | |
Zeekat | 0:a643b1b38abe | 166 | // this funtion flips leds on and off accordin to input with 0 being on |
Zeekat | 0:a643b1b38abe | 167 | void LED(int red,int green,int blue) |
Zeekat | 0:a643b1b38abe | 168 | { |
Zeekat | 0:a643b1b38abe | 169 | ledred.write(red); |
Zeekat | 0:a643b1b38abe | 170 | ledgreen.write(green); |
Zeekat | 0:a643b1b38abe | 171 | ledblue.write(blue); |
Zeekat | 0:a643b1b38abe | 172 | } |
Zeekat | 0:a643b1b38abe | 173 | |
Zeekat | 0:a643b1b38abe | 174 | // counts 2 radians |
Zeekat | 0:a643b1b38abe | 175 | // this function takes counts from the encoder and converts it to the amount of radians from the zero position. |
Zeekat | 0:a643b1b38abe | 176 | // It has been set up for standard 2X DECODING!!! |
Zeekat | 0:a643b1b38abe | 177 | double get_radians(double counts) |
Zeekat | 0:a643b1b38abe | 178 | { |
Zeekat | 0:a643b1b38abe | 179 | double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, last warning) |
Zeekat | 0:a643b1b38abe | 180 | return radians; |
Zeekat | 0:a643b1b38abe | 181 | } |
Zeekat | 0:a643b1b38abe | 182 | |
Zeekat | 0:a643b1b38abe | 183 | |
Zeekat | 0:a643b1b38abe | 184 | // This functions takes a 0->1 input, uses passing by reference (&c_reference) |
Zeekat | 0:a643b1b38abe | 185 | // to create a reference that moves with a variable speed. It is meant for 0->1 values |
Zeekat | 0:a643b1b38abe | 186 | double reference_f(double input, double &c_reference, double limlow, double limhigh) |
Zeekat | 0:a643b1b38abe | 187 | { |
Zeekat | 0:a643b1b38abe | 188 | double reference = c_reference + input * controlstep * Vmax ; |
Zeekat | 0:a643b1b38abe | 189 | // two if statements check if the reference exceeds the limits placed upon the arms |
Zeekat | 0:a643b1b38abe | 190 | if(reference < limlow){reference = limlow;} |
Zeekat | 0:a643b1b38abe | 191 | if(reference > limhigh){reference = limhigh;} |
Zeekat | 0:a643b1b38abe | 192 | c_reference = reference; // change the global variable to the latest location. |
Zeekat | 0:a643b1b38abe | 193 | return reference; |
Zeekat | 0:a643b1b38abe | 194 | } |
Zeekat | 0:a643b1b38abe | 195 | |
Zeekat | 0:a643b1b38abe | 196 | |
Zeekat | 0:a643b1b38abe | 197 | // This function takes the controller outputvalue and ensures it is between -1 and 1 |
Zeekat | 0:a643b1b38abe | 198 | // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction). |
Zeekat | 0:a643b1b38abe | 199 | // needs more work to use it for the wind-up prevention. |
Zeekat | 0:a643b1b38abe | 200 | double outputlimiter (double output, double limit) |
Zeekat | 0:a643b1b38abe | 201 | { |
Zeekat | 0:a643b1b38abe | 202 | if(output> limit) |
Zeekat | 0:a643b1b38abe | 203 | { |
Zeekat | 0:a643b1b38abe | 204 | output = 1; |
Zeekat | 0:a643b1b38abe | 205 | } |
Zeekat | 0:a643b1b38abe | 206 | else if(output < limit && output > 0) |
Zeekat | 0:a643b1b38abe | 207 | { |
Zeekat | 0:a643b1b38abe | 208 | output = output; |
Zeekat | 0:a643b1b38abe | 209 | } |
Zeekat | 0:a643b1b38abe | 210 | else if(output > -limit && output < 0) |
Zeekat | 0:a643b1b38abe | 211 | { |
Zeekat | 0:a643b1b38abe | 212 | output = output; |
Zeekat | 0:a643b1b38abe | 213 | } |
Zeekat | 0:a643b1b38abe | 214 | else if(output < -limit) |
Zeekat | 0:a643b1b38abe | 215 | { |
Zeekat | 0:a643b1b38abe | 216 | (output = -1); |
Zeekat | 0:a643b1b38abe | 217 | } |
Zeekat | 0:a643b1b38abe | 218 | return output; |
Zeekat | 0:a643b1b38abe | 219 | } |
Zeekat | 0:a643b1b38abe | 220 | |
Zeekat | 0:a643b1b38abe | 221 | |
Zeekat | 0:a643b1b38abe | 222 | // BIQUADFILTER CODE GIVEN IN SHEETS |
Zeekat | 0:a643b1b38abe | 223 | double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) |
Zeekat | 0:a643b1b38abe | 224 | { |
Zeekat | 0:a643b1b38abe | 225 | double v = u - a1*v1 - a2*v2; |
Zeekat | 0:a643b1b38abe | 226 | double y = b0*v + b1*v1 + b2*v2; |
Zeekat | 0:a643b1b38abe | 227 | v2 = v1; |
Zeekat | 0:a643b1b38abe | 228 | v1 = v; |
Zeekat | 0:a643b1b38abe | 229 | return y; |
Zeekat | 0:a643b1b38abe | 230 | } |
Zeekat | 0:a643b1b38abe | 231 | |
Zeekat | 1:f3910e46b831 | 232 | // biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee) |
Zeekat | 1:f3910e46b831 | 233 | double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t) |
Zeekat | 0:a643b1b38abe | 234 | { |
Zeekat | 1:f3910e46b831 | 235 | double vt = ut- a1t*v1t-a2t*v2t; |
Zeekat | 1:f3910e46b831 | 236 | double yt = b0t*vt+b1t*v1t+b2t*v2t; |
Zeekat | 1:f3910e46b831 | 237 | v2t = v1t; |
Zeekat | 1:f3910e46b831 | 238 | v1t = vt; |
Zeekat | 1:f3910e46b831 | 239 | return yt; |
Zeekat | 0:a643b1b38abe | 240 | } |
Zeekat | 0:a643b1b38abe | 241 | |
Zeekat | 0:a643b1b38abe | 242 | // PID Controller given in sheets |
Zeekat | 0:a643b1b38abe | 243 | // aangepast om zelfde filter te gebruiken en om de termen later gesplitst te kunnen limiteren (windup preventie!!) |
Zeekat | 0:a643b1b38abe | 244 | double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev) |
Zeekat | 0:a643b1b38abe | 245 | { |
Zeekat | 0:a643b1b38abe | 246 | // Proportional |
Zeekat | 0:a643b1b38abe | 247 | double P = Kp * e; |
Zeekat | 0:a643b1b38abe | 248 | // Integral |
Zeekat | 0:a643b1b38abe | 249 | e_int = e_int + Ts * e; |
Zeekat | 0:a643b1b38abe | 250 | double I = e_int * Ki; |
Zeekat | 0:a643b1b38abe | 251 | // Derivative |
Zeekat | 0:a643b1b38abe | 252 | double e_derr = (e - e_prev)/Ts; |
Zeekat | 0:a643b1b38abe | 253 | 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 | 0:a643b1b38abe | 254 | // |
Zeekat | 0:a643b1b38abe | 255 | e_prev = e; |
Zeekat | 0:a643b1b38abe | 256 | double D = Kd* e_derr; |
Zeekat | 0:a643b1b38abe | 257 | // PID |
Zeekat | 0:a643b1b38abe | 258 | double output = P + I + D; |
Zeekat | 0:a643b1b38abe | 259 | return output; |
Zeekat | 0:a643b1b38abe | 260 | } |
Zeekat | 0:a643b1b38abe | 261 | ///////////////////////////////////////////////////////////////////// |
Zeekat | 0:a643b1b38abe | 262 | ////////////////// PRIMARY CONTROL FUNCTIONS /////////////////////// |
Zeekat | 0:a643b1b38abe | 263 | /////////////////////////////////////////////////////////////////// |
Zeekat | 1:f3910e46b831 | 264 | // these functions are called by go-flags and are used to update main variables and send signals to motor |
Zeekat | 0:a643b1b38abe | 265 | |
Zeekat | 0:a643b1b38abe | 266 | // MOTOR 1 |
Zeekat | 0:a643b1b38abe | 267 | void motor1_control() |
Zeekat | 0:a643b1b38abe | 268 | { |
Zeekat | 2:867a1eb33bbe | 269 | //double m_input1 = potright.read(); // used for pot input |
Zeekat | 0:a643b1b38abe | 270 | //input1 = 0.4505; |
Zeekat | 0:a643b1b38abe | 271 | |
Zeekat | 0:a643b1b38abe | 272 | // first input edit (limit signal between threshold and 1, and reverse if wanted |
Zeekat | 2:867a1eb33bbe | 273 | //if(m_input1 < input_threshold) {m_input1 = 0;} |
Zeekat | 2:867a1eb33bbe | 274 | //if(m_input1 > 1) {m_input1 = 1;} |
Zeekat | 2:867a1eb33bbe | 275 | //if(reverse_rechts == true) {m_input1 = -m_input1;} |
Zeekat | 2:867a1eb33bbe | 276 | // m_input1 = biquadfilter(m_input1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); //biquad with diff-filter coefficients to smooth input |
Zeekat | 0:a643b1b38abe | 277 | |
Zeekat | 0:a643b1b38abe | 278 | |
Zeekat | 2:867a1eb33bbe | 279 | //double reference1 = reference_f(m_input1, c_reference1,limlow1,limhigh1); // determine the reference that has been set by the inputsignal |
Zeekat | 2:867a1eb33bbe | 280 | //scope.set(0,reference1); |
Zeekat | 2:867a1eb33bbe | 281 | double reference1 = phi_one; |
Zeekat | 0:a643b1b38abe | 282 | scope.set(0,reference1); |
Zeekat | 2:867a1eb33bbe | 283 | pc.printf("ref1 = %f",reference1); |
Zeekat | 0:a643b1b38abe | 284 | double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor |
Zeekat | 2:867a1eb33bbe | 285 | //scope.set(1,rads1); |
Zeekat | 2:867a1eb33bbe | 286 | pc.printf("rads1 = %f ",rads1); |
Zeekat | 2:867a1eb33bbe | 287 | //scope.send(); |
Zeekat | 0:a643b1b38abe | 288 | double error1 = (reference1 - rads1); // determine the error (reference - position) |
Zeekat | 1:f3910e46b831 | 289 | double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err); |
Zeekat | 0:a643b1b38abe | 290 | |
Zeekat | 1:f3910e46b831 | 291 | m_output1 = outputlimiter(m_output1,1); // relimit the output for safety |
Zeekat | 1:f3910e46b831 | 292 | if(m_output1 > 0) { // uses the calculated output to determine the direction of the motor |
Zeekat | 0:a643b1b38abe | 293 | motor1_rich.write(0); |
Zeekat | 1:f3910e46b831 | 294 | motor1_aan.write(m_output1); |
Zeekat | 1:f3910e46b831 | 295 | } else if(m_output1 < 0) { |
Zeekat | 0:a643b1b38abe | 296 | motor1_rich.write(1); |
Zeekat | 1:f3910e46b831 | 297 | motor1_aan.write(abs(m_output1)); |
Zeekat | 0:a643b1b38abe | 298 | } |
Zeekat | 0:a643b1b38abe | 299 | } |
Zeekat | 0:a643b1b38abe | 300 | |
Zeekat | 0:a643b1b38abe | 301 | // MOTOR 2 |
Zeekat | 0:a643b1b38abe | 302 | void motor2_control() |
Zeekat | 0:a643b1b38abe | 303 | { |
Zeekat | 2:867a1eb33bbe | 304 | //double m_input2 = potleft.read()*signalamp2; // replace potleft with filter |
Zeekat | 1:f3910e46b831 | 305 | |
Zeekat | 0:a643b1b38abe | 306 | // first input limiter |
Zeekat | 2:867a1eb33bbe | 307 | //if(m_input2 < input_threshold) {m_input2 = 0;} |
Zeekat | 2:867a1eb33bbe | 308 | //if(m_input2 > 1) {m_input2 = 1;} |
Zeekat | 2:867a1eb33bbe | 309 | //if(reverse_links == false) {m_input2 = -m_input2;} |
Zeekat | 2:867a1eb33bbe | 310 | //m_input2 = biquadfilter(m_input2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2); |
Zeekat | 0:a643b1b38abe | 311 | |
Zeekat | 0:a643b1b38abe | 312 | |
Zeekat | 2:867a1eb33bbe | 313 | //double reference2 = reference_f(m_input2, c_reference2,limlow2,limhigh2); // determine the reference that has been set by the clamped inputsignal |
Zeekat | 2:867a1eb33bbe | 314 | |
Zeekat | 2:867a1eb33bbe | 315 | double reference2 = phi_two; |
Zeekat | 2:867a1eb33bbe | 316 | scope.set(1,reference2); |
Zeekat | 2:867a1eb33bbe | 317 | scope.send(); |
Zeekat | 0:a643b1b38abe | 318 | double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor |
Zeekat | 2:867a1eb33bbe | 319 | pc.printf("rads2 = %f ",rads2); |
Zeekat | 0:a643b1b38abe | 320 | double error2 = (reference2 - rads2); // determine the error (reference - position) |
Zeekat | 1:f3910e46b831 | 321 | double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err); |
Zeekat | 0:a643b1b38abe | 322 | |
Zeekat | 1:f3910e46b831 | 323 | m_output2 = outputlimiter(m_output2,1); |
Zeekat | 1:f3910e46b831 | 324 | if(m_output2 > 0) { // uses the calculated output to determine the direction of the motor |
Zeekat | 0:a643b1b38abe | 325 | motor2_rich.write(0); |
Zeekat | 1:f3910e46b831 | 326 | motor2_aan.write(m_output2); |
Zeekat | 1:f3910e46b831 | 327 | } else if(m_output2 < 0) { |
Zeekat | 0:a643b1b38abe | 328 | motor2_rich.write(1); |
Zeekat | 1:f3910e46b831 | 329 | motor2_aan.write(abs(m_output2)); |
Zeekat | 0:a643b1b38abe | 330 | } |
Zeekat | 0:a643b1b38abe | 331 | } |
Zeekat | 0:a643b1b38abe | 332 | |
Zeekat | 0:a643b1b38abe | 333 | |
Zeekat | 1:f3910e46b831 | 334 | // function that updates the inputs |
Zeekat | 1:f3910e46b831 | 335 | void EMG_filter() |
Zeekat | 1:f3910e46b831 | 336 | { |
Zeekat | 1:f3910e46b831 | 337 | // filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass |
Zeekat | 1:f3910e46b831 | 338 | double u1 = EMG_in.read(); |
Zeekat | 1:f3910e46b831 | 339 | double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); |
Zeekat | 1:f3910e46b831 | 340 | double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); |
Zeekat | 1:f3910e46b831 | 341 | double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); |
Zeekat | 1:f3910e46b831 | 342 | double y4 = abs(y3); |
Zeekat | 1:f3910e46b831 | 343 | double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); |
Zeekat | 1:f3910e46b831 | 344 | output1 = y5; // update global variable |
Zeekat | 1:f3910e46b831 | 345 | // versturen van het input signaal u1 en het gefilterde signaal y5 naar HIDScope channel 0 en 1 |
Zeekat | 1:f3910e46b831 | 346 | //scope.set(0,u1); |
Zeekat | 1:f3910e46b831 | 347 | //scope.set(1,y5); |
Zeekat | 1:f3910e46b831 | 348 | // filteren van EMG signaal 2 (A2), zelfde proces als signaal 1 |
Zeekat | 1:f3910e46b831 | 349 | double u1t = EMG_int.read(); |
Zeekat | 1:f3910e46b831 | 350 | double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3); |
Zeekat | 1:f3910e46b831 | 351 | double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3); |
Zeekat | 1:f3910e46b831 | 352 | double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3); |
Zeekat | 1:f3910e46b831 | 353 | double y4t = abs(y3t); |
Zeekat | 1:f3910e46b831 | 354 | double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3); |
Zeekat | 1:f3910e46b831 | 355 | output2 = y5t; // update global variable |
Zeekat | 1:f3910e46b831 | 356 | |
Zeekat | 1:f3910e46b831 | 357 | // versturen van input signaal2; u1t en het gefilterde signaal; y5t naar HIDScope channel 2 en 3 |
Zeekat | 1:f3910e46b831 | 358 | //scope.set(2,u1t); |
Zeekat | 1:f3910e46b831 | 359 | //scope.set(3,y5t); |
Zeekat | 1:f3910e46b831 | 360 | // verzenden van de verstuurde signalen |
Zeekat | 1:f3910e46b831 | 361 | //scope.send(); |
Zeekat | 1:f3910e46b831 | 362 | } |
Zeekat | 1:f3910e46b831 | 363 | |
Zeekat | 1:f3910e46b831 | 364 | // function that updates the required motor angles |
Zeekat | 1:f3910e46b831 | 365 | void det_angles() |
Zeekat | 1:f3910e46b831 | 366 | { |
Zeekat | 2:867a1eb33bbe | 367 | output1 = potright.read(); |
Zeekat | 2:867a1eb33bbe | 368 | output2 = potleft.read(); |
Zeekat | 2:867a1eb33bbe | 369 | double xx = 66+output1*4; |
Zeekat | 2:867a1eb33bbe | 370 | double yy = -16+output2*32; |
Zeekat | 1:f3910e46b831 | 371 | double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector |
Zeekat | 1:f3910e46b831 | 372 | double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm |
Zeekat | 1:f3910e46b831 | 373 | double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r |
Zeekat | 1:f3910e46b831 | 374 | // hoeken berekenen |
Zeekat | 2:867a1eb33bbe | 375 | //pc.printf("xx = %f ",xx); |
Zeekat | 2:867a1eb33bbe | 376 | //pc.printf("yy = %f ",yy); |
Zeekat | 2:867a1eb33bbe | 377 | //scope.set(0,xx); |
Zeekat | 2:867a1eb33bbe | 378 | //scope.set(1,yy); |
Zeekat | 2:867a1eb33bbe | 379 | |
Zeekat | 2:867a1eb33bbe | 380 | double theta_one = (atan2(yy,xx)+beta); |
Zeekat | 2:867a1eb33bbe | 381 | double theta_two = (-pi + alfa); |
Zeekat | 2:867a1eb33bbe | 382 | |
Zeekat | 2:867a1eb33bbe | 383 | double phi1 = 4*(theta_one) + 2.8; |
Zeekat | 2:867a1eb33bbe | 384 | double phi2 = 4*(theta_one+theta_two) + 1.85; |
Zeekat | 2:867a1eb33bbe | 385 | phi2 = -phi2; |
Zeekat | 2:867a1eb33bbe | 386 | if(phi1 < limlow1){phi1 = limlow1;} |
Zeekat | 2:867a1eb33bbe | 387 | if(phi1 > limhigh1){phi1 = limhigh1;} |
Zeekat | 1:f3910e46b831 | 388 | |
Zeekat | 2:867a1eb33bbe | 389 | if(phi2 < limlow2){phi2 = limlow2;} // reverse limlow2 limhigh2 becasue they are inverse due to transmission |
Zeekat | 2:867a1eb33bbe | 390 | if(phi2 > limhigh2){phi2 = limhigh2;} |
Zeekat | 2:867a1eb33bbe | 391 | |
Zeekat | 2:867a1eb33bbe | 392 | phi_one = phi1; |
Zeekat | 2:867a1eb33bbe | 393 | phi_two = phi2; |
Zeekat | 2:867a1eb33bbe | 394 | |
Zeekat | 2:867a1eb33bbe | 395 | //theta_one = theta_one*4 ; |
Zeekat | 2:867a1eb33bbe | 396 | //theta_two = theta_two*4 ; |
Zeekat | 2:867a1eb33bbe | 397 | |
Zeekat | 2:867a1eb33bbe | 398 | |
Zeekat | 2:867a1eb33bbe | 399 | |
Zeekat | 2:867a1eb33bbe | 400 | //pc.printf("theta1 = %f ",theta_one); |
Zeekat | 2:867a1eb33bbe | 401 | //pc.printf("theta2 = %f \n",theta_two); |
Zeekat | 2:867a1eb33bbe | 402 | //scope.set(2,phi1); |
Zeekat | 2:867a1eb33bbe | 403 | //scope.set(3,phi2); |
Zeekat | 2:867a1eb33bbe | 404 | |
Zeekat | 2:867a1eb33bbe | 405 | //scope.send(); |
Zeekat | 1:f3910e46b831 | 406 | // testen van de functie |
Zeekat | 1:f3910e46b831 | 407 | double xt = L*cos(theta_one)+L*cos(theta_one+theta_two); |
Zeekat | 1:f3910e46b831 | 408 | double yt = L*sin(theta_one)+L*sin(theta_one+theta_two); |
Zeekat | 2:867a1eb33bbe | 409 | |
Zeekat | 1:f3910e46b831 | 410 | } |
Zeekat | 0:a643b1b38abe | 411 | ////////////////////////////////////////////////////////////////// |
Zeekat | 0:a643b1b38abe | 412 | //////////// DEFINE GO-FLAG FUNCTIONS /////////////////////////// |
Zeekat | 0:a643b1b38abe | 413 | //////////////////////////////////////////////////////////////// |
Zeekat | 0:a643b1b38abe | 414 | |
Zeekat | 1:f3910e46b831 | 415 | |
Zeekat | 1:f3910e46b831 | 416 | void EMG_activate() |
Zeekat | 1:f3910e46b831 | 417 | { |
Zeekat | 1:f3910e46b831 | 418 | emg_go = true; |
Zeekat | 1:f3910e46b831 | 419 | } |
Zeekat | 1:f3910e46b831 | 420 | void angle_activate() |
Zeekat | 1:f3910e46b831 | 421 | { |
Zeekat | 1:f3910e46b831 | 422 | cart_go = true; |
Zeekat | 1:f3910e46b831 | 423 | } |
Zeekat | 0:a643b1b38abe | 424 | void motor1_activate() |
Zeekat | 0:a643b1b38abe | 425 | { |
Zeekat | 0:a643b1b38abe | 426 | motor1_go = true; |
Zeekat | 0:a643b1b38abe | 427 | } |
Zeekat | 0:a643b1b38abe | 428 | void motor2_activate() |
Zeekat | 0:a643b1b38abe | 429 | { |
Zeekat | 0:a643b1b38abe | 430 | motor2_go = true; |
Zeekat | 0:a643b1b38abe | 431 | } |
Zeekat | 0:a643b1b38abe | 432 | |
Zeekat | 0:a643b1b38abe | 433 | int main() |
Zeekat | 0:a643b1b38abe | 434 | { |
Zeekat | 0:a643b1b38abe | 435 | pc.baud(115200); |
Zeekat | 2:867a1eb33bbe | 436 | // main_filter.attach(&EMG_activate, controlstep); |
Zeekat | 1:f3910e46b831 | 437 | cartesian.attach(&angle_activate, controlstep); |
Zeekat | 0:a643b1b38abe | 438 | controller1.attach(&motor1_activate, controlstep); // call a go-flag |
Zeekat | 0:a643b1b38abe | 439 | controller2.attach(&motor2_activate, controlstep); |
Zeekat | 0:a643b1b38abe | 440 | while(true) |
Zeekat | 0:a643b1b38abe | 441 | { |
Zeekat | 0:a643b1b38abe | 442 | // button press functions |
Zeekat | 0:a643b1b38abe | 443 | // flow buttons |
Zeekat | 0:a643b1b38abe | 444 | if(buttonlinks.read() == 0) |
Zeekat | 0:a643b1b38abe | 445 | { |
Zeekat | 0:a643b1b38abe | 446 | loop_start = !loop_start; |
Zeekat | 0:a643b1b38abe | 447 | wait(buttonlinks.read() == 1); |
Zeekat | 0:a643b1b38abe | 448 | wait(0.3); |
Zeekat | 0:a643b1b38abe | 449 | } |
Zeekat | 0:a643b1b38abe | 450 | if(buttonrechts.read() == 0) |
Zeekat | 0:a643b1b38abe | 451 | { |
Zeekat | 0:a643b1b38abe | 452 | calib_start = !calib_start; |
Zeekat | 0:a643b1b38abe | 453 | wait(buttonrechts.read() == 1); |
Zeekat | 0:a643b1b38abe | 454 | wait(0.3); |
Zeekat | 0:a643b1b38abe | 455 | } |
Zeekat | 0:a643b1b38abe | 456 | // reverse buttons |
Zeekat | 0:a643b1b38abe | 457 | if(reverse_button_links.read() == 0) |
Zeekat | 0:a643b1b38abe | 458 | { |
Zeekat | 0:a643b1b38abe | 459 | reverse_links = !reverse_links; |
Zeekat | 0:a643b1b38abe | 460 | wait(reverse_button_links.read() == 1); |
Zeekat | 0:a643b1b38abe | 461 | wait(0.3); |
Zeekat | 0:a643b1b38abe | 462 | } |
Zeekat | 0:a643b1b38abe | 463 | if(reverse_button_rechts.read() == 0) |
Zeekat | 0:a643b1b38abe | 464 | { |
Zeekat | 0:a643b1b38abe | 465 | reverse_rechts = !reverse_rechts; |
Zeekat | 0:a643b1b38abe | 466 | wait(reverse_button_rechts.read() == 1); |
Zeekat | 0:a643b1b38abe | 467 | wait(0.3); |
Zeekat | 0:a643b1b38abe | 468 | } |
Zeekat | 0:a643b1b38abe | 469 | ////////////////////////////////////////////////// |
Zeekat | 0:a643b1b38abe | 470 | // Main Control stuff and options |
Zeekat | 0:a643b1b38abe | 471 | if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops |
Zeekat | 0:a643b1b38abe | 472 | { |
Zeekat | 0:a643b1b38abe | 473 | LED(1,1,0); // turn blue led on |
Zeekat | 1:f3910e46b831 | 474 | if(cart_go) { cart_go = false; det_angles();} |
Zeekat | 1:f3910e46b831 | 475 | if(emg_go) { emg_go = false; EMG_filter();} |
Zeekat | 0:a643b1b38abe | 476 | if(motor1_go) { motor1_go = false; motor1_control();} |
Zeekat | 0:a643b1b38abe | 477 | if(motor2_go) { motor2_go = false; motor2_control();} |
Zeekat | 0:a643b1b38abe | 478 | } |
Zeekat | 0:a643b1b38abe | 479 | // shut off both motors |
Zeekat | 0:a643b1b38abe | 480 | if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);} |
Zeekat | 0:a643b1b38abe | 481 | |
Zeekat | 0:a643b1b38abe | 482 | // turn green led on // start calibration procedures |
Zeekat | 0:a643b1b38abe | 483 | if(loop_start == false && calib_start == true) { LED(1,0,1); motor1_aan.write(0); motor2_aan.write(0);} |
Zeekat | 0:a643b1b38abe | 484 | |
Zeekat | 0:a643b1b38abe | 485 | // turn red led on |
Zeekat | 0:a643b1b38abe | 486 | if(loop_start == true && calib_start == true) { LED(0,1,1); motor1_aan.write(0); motor2_aan.write(0);} |
Zeekat | 0:a643b1b38abe | 487 | |
Zeekat | 0:a643b1b38abe | 488 | // turn leds off (both buttons false) |
Zeekat | 0:a643b1b38abe | 489 | else { LED(1,1,1);} |
Zeekat | 0:a643b1b38abe | 490 | } |
Zeekat | 0:a643b1b38abe | 491 | } |