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