Wouter Bos / Mbed 2 deprecated group29_x_y_control_dynamic_x-lims

presentatie versie met potmeters enabled

Dependencies:   Encoder HIDScope mbed

main.cpp@0:b0e2b38ab272, 2015-10-27 (annotated)

Committer:
Zeekat
Date:
Tue Oct 27 12:20:03 2015 +0000
Revision:
0:b0e2b38ab272
Child:
1:4c9994ac229c
goed smooth. 200Hz EMG.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:b0e2b38ab272 1 #include "mbed.h"
Zeekat 0:b0e2b38ab272 2 #include "MODSERIAL.h"
Zeekat 0:b0e2b38ab272 3 #include "encoder.h"
Zeekat 0:b0e2b38ab272 4 #include "HIDScope.h"
Zeekat 0:b0e2b38ab272 5 #include "math.h"
Zeekat 0:b0e2b38ab272 6
Zeekat 0:b0e2b38ab272 7 Serial pc(USBTX,USBRX); // MODSERIAL output
Zeekat 0:b0e2b38ab272 8 HIDScope scope(4); // definieerd het aantal kanalen van de scope
Zeekat 0:b0e2b38ab272 9
Zeekat 0:b0e2b38ab272 10 // Define Tickers and control frequencies
Zeekat 0:b0e2b38ab272 11 Ticker controller1, controller2, main_filter, cartesian, send;
Zeekat 0:b0e2b38ab272 12 // Go flag variables belonging to Tickers
Zeekat 0:b0e2b38ab272 13 volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;
Zeekat 0:b0e2b38ab272 14
Zeekat 0:b0e2b38ab272 15 // Frequency control
Zeekat 0:b0e2b38ab272 16 double controlfreq = 50 ; // controlloops frequentie (Hz)
Zeekat 0:b0e2b38ab272 17 double controlstep = 1/controlfreq; // timestep derived from controlfreq
Zeekat 0:b0e2b38ab272 18
Zeekat 0:b0e2b38ab272 19 double EMG_freq = 200;
Zeekat 0:b0e2b38ab272 20 double EMG_step = 1/EMG_freq;
Zeekat 0:b0e2b38ab272 21
Zeekat 0:b0e2b38ab272 22 //////////////////////// IN-OUTPUT /////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 23 //MOTOR OUTPUTPINS
Zeekat 0:b0e2b38ab272 24 PwmOut motor1_aan(D6), motor2_aan(D5); // PWM signaal motor 2 (uit sheets)
Zeekat 0:b0e2b38ab272 25 DigitalOut motor1_rich(D7), motor2_rich(D4); // digitaal signaal voor richting
Zeekat 0:b0e2b38ab272 26
Zeekat 0:b0e2b38ab272 27 // ENCODER INPUTPINS
Zeekat 0:b0e2b38ab272 28 Encoder motor1_enc(D12,D11), motor2_enc(D10,D9); // encoder outputpins
Zeekat 0:b0e2b38ab272 29
Zeekat 0:b0e2b38ab272 30 // EXTRA INPUTS AND REQUIRED VARIABLES
Zeekat 0:b0e2b38ab272 31 //POTMETERS (used for debugging purposes, not reliable anymore)
Zeekat 0:b0e2b38ab272 32 AnalogIn potright(A4); // define the potmeter outputpins
Zeekat 0:b0e2b38ab272 33 AnalogIn potleft(A5);
Zeekat 0:b0e2b38ab272 34
Zeekat 0:b0e2b38ab272 35 // Analoge input signalen defineren
Zeekat 0:b0e2b38ab272 36 AnalogIn EMG_in(A0); // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
Zeekat 0:b0e2b38ab272 37 AnalogIn EMG_int(A2); // deze leest A3 uit
Zeekat 0:b0e2b38ab272 38
Zeekat 0:b0e2b38ab272 39 // BUTTONS
Zeekat 0:b0e2b38ab272 40 // control flow
Zeekat 0:b0e2b38ab272 41 DigitalIn buttonlinks(PTA4);
Zeekat 0:b0e2b38ab272 42 DigitalIn buttonrechts(PTC6);
Zeekat 0:b0e2b38ab272 43 DigitalIn reset_button(D1);
Zeekat 0:b0e2b38ab272 44 // init values
Zeekat 0:b0e2b38ab272 45 bool loop_start = false;
Zeekat 0:b0e2b38ab272 46 bool calib_start = false;
Zeekat 0:b0e2b38ab272 47 bool reset = false;
Zeekat 0:b0e2b38ab272 48
Zeekat 0:b0e2b38ab272 49 // axis control
Zeekat 0:b0e2b38ab272 50 DigitalIn switch_xy_button(D0);
Zeekat 0:b0e2b38ab272 51 // init values
Zeekat 0:b0e2b38ab272 52 bool switch_xy = false;
Zeekat 0:b0e2b38ab272 53 // LED outputs on bioshield
Zeekat 0:b0e2b38ab272 54 DigitalOut led_right(D2);
Zeekat 0:b0e2b38ab272 55 // LED outputs on dev-board
Zeekat 0:b0e2b38ab272 56 DigitalOut ledred(LED1);
Zeekat 0:b0e2b38ab272 57 DigitalOut ledgreen(LED2);
Zeekat 0:b0e2b38ab272 58 DigitalOut ledblue(LED3);
Zeekat 0:b0e2b38ab272 59
Zeekat 0:b0e2b38ab272 60
Zeekat 0:b0e2b38ab272 61 //////////////////////////////////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 62 ////////////////////////// GLOBAL VARIABLES ///////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 63 ////////////////////////////////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 64
Zeekat 0:b0e2b38ab272 65 double t = 0; // used for circle movement
Zeekat 0:b0e2b38ab272 66 // physical constants
Zeekat 0:b0e2b38ab272 67 const double L = 36; // lenght of arms
Zeekat 0:b0e2b38ab272 68 const double pi = 3.1415926535897; // pi
Zeekat 0:b0e2b38ab272 69
Zeekat 0:b0e2b38ab272 70 // angle limitations (in radians)
Zeekat 0:b0e2b38ab272 71 // motor1
Zeekat 0:b0e2b38ab272 72 const double limlow1 = 0.5; // min height in rad
Zeekat 0:b0e2b38ab272 73 const double limhigh1 = 6.3; // max height in rad
Zeekat 0:b0e2b38ab272 74 // motor 2
Zeekat 0:b0e2b38ab272 75 const double limlow2 = -4.0; // maximum height, motor has been inverted due to transmission
Zeekat 0:b0e2b38ab272 76 const double limhigh2 = 2.5; // minimum height in rad
Zeekat 0:b0e2b38ab272 77
Zeekat 0:b0e2b38ab272 78 // offset angle (radians needed to change the arms to horizontal position from reset position)
Zeekat 0:b0e2b38ab272 79 const double phi_one_offset = 2.8;
Zeekat 0:b0e2b38ab272 80 const double phi_two_offset = 1.85;
Zeekat 0:b0e2b38ab272 81
Zeekat 0:b0e2b38ab272 82 ///////////////////////////
Zeekat 0:b0e2b38ab272 83 // Main control loop transfer variables
Zeekat 0:b0e2b38ab272 84 // here all variables that transfer bewtween the primary control functions
Zeekat 0:b0e2b38ab272 85
Zeekat 0:b0e2b38ab272 86 // filter to calibration
Zeekat 0:b0e2b38ab272 87 double output1;
Zeekat 0:b0e2b38ab272 88 double output2;
Zeekat 0:b0e2b38ab272 89 // filter to x-y
Zeekat 0:b0e2b38ab272 90 double output1_amp;
Zeekat 0:b0e2b38ab272 91 double output2_amp;
Zeekat 0:b0e2b38ab272 92 // x-y to motor control
Zeekat 0:b0e2b38ab272 93 double phi_one;
Zeekat 0:b0e2b38ab272 94 double phi_two;
Zeekat 0:b0e2b38ab272 95
Zeekat 0:b0e2b38ab272 96 ////////////////////////////
Zeekat 0:b0e2b38ab272 97 // NOTE: start position is [x,y] = [60,0], reset position is [phi1,phi2] = [0,0]
Zeekat 0:b0e2b38ab272 98 // define smooth variables (is to create a slowed movement instead of going to ref value inmediately)
Zeekat 0:b0e2b38ab272 99 double start_up_time = 3;
Zeekat 0:b0e2b38ab272 100 double start_loops = start_up_time*controlfreq;
Zeekat 0:b0e2b38ab272 101 int rc1 = 0; // counters in function to enable relatively smooth movement
Zeekat 0:b0e2b38ab272 102 int rc2 = 0;
Zeekat 0:b0e2b38ab272 103
Zeekat 0:b0e2b38ab272 104 // define return to start variables
Zeekat 0:b0e2b38ab272 105 double reset_phi_one = 0;
Zeekat 0:b0e2b38ab272 106 double reset_phi_two = 0;
Zeekat 0:b0e2b38ab272 107
Zeekat 0:b0e2b38ab272 108 // storage variables to store the location at the beginning of the smoothed movement (0 on startup)
Zeekat 0:b0e2b38ab272 109 double phi_one_curr = 0;
Zeekat 0:b0e2b38ab272 110 double phi_two_curr = 0;
Zeekat 0:b0e2b38ab272 111
Zeekat 0:b0e2b38ab272 112 // REFERENCE SETTINGS
Zeekat 0:b0e2b38ab272 113 double input_threshold = 0.25; // the minimum value the signal must have in order to change the reference.
Zeekat 0:b0e2b38ab272 114 // Define storage variables for reference values (also start position)
Zeekat 0:b0e2b38ab272 115 double c_reference_x = 60, c_reference_y = 0;
Zeekat 0:b0e2b38ab272 116 // x-settings (no y-settings because these are calculated from the current x-position)
Zeekat 0:b0e2b38ab272 117 double x_min = 47, x_max = 70, y_min_max = -40;
Zeekat 0:b0e2b38ab272 118 double xx,yy,y_min,y_max;
Zeekat 0:b0e2b38ab272 119 // Define the maximum rate of change for the x and y reference signals (velocity)
Zeekat 0:b0e2b38ab272 120 double Vmax_x = 15, Vmax_y = 15; // [cm/s]
Zeekat 0:b0e2b38ab272 121
Zeekat 0:b0e2b38ab272 122
Zeekat 0:b0e2b38ab272 123 // CONTROLLER SETTINGS
Zeekat 0:b0e2b38ab272 124 // motor 1
Zeekat 0:b0e2b38ab272 125 const double m1_Kp = 5; // Proportional constant
Zeekat 0:b0e2b38ab272 126 const double m1_Ki = 0.5; // integration constant
Zeekat 0:b0e2b38ab272 127 const double m1_Kd = 0.4; // differentiation constant
Zeekat 0:b0e2b38ab272 128 // motor 2
Zeekat 0:b0e2b38ab272 129 const double m2_Kp = 3;
Zeekat 0:b0e2b38ab272 130 const double m2_Ki = 0.5;
Zeekat 0:b0e2b38ab272 131 const double m2_Kd = 0.3;
Zeekat 0:b0e2b38ab272 132 // storage variables. these variables are used to store data between controller iterations
Zeekat 0:b0e2b38ab272 133 // motor 1
Zeekat 0:b0e2b38ab272 134 double m1_err_int = 0;
Zeekat 0:b0e2b38ab272 135 double m1_prev_err = 0;
Zeekat 0:b0e2b38ab272 136 // motor 2
Zeekat 0:b0e2b38ab272 137 double m2_err_int = 0;
Zeekat 0:b0e2b38ab272 138 double m2_prev_err = 0;
Zeekat 0:b0e2b38ab272 139
Zeekat 0:b0e2b38ab272 140 // EMG calibration variables
Zeekat 0:b0e2b38ab272 141 double emg_gain1 = 7;
Zeekat 0:b0e2b38ab272 142 double emg_gain2 = 7;
Zeekat 0:b0e2b38ab272 143
Zeekat 0:b0e2b38ab272 144 double cal_time = 2.5; // amount of time calibration should take (seconds)
Zeekat 0:b0e2b38ab272 145 double cal_samples = controlfreq*cal_time; // amount of samples that is used (dependent on the frequency of the filter)
Zeekat 0:b0e2b38ab272 146 double normalize_emg_value = 1; // set the desired value to calibrate the signal to (eg max signal = 1)
Zeekat 0:b0e2b38ab272 147
Zeekat 0:b0e2b38ab272 148 // FILTER VARIABLES
Zeekat 0:b0e2b38ab272 149 // storage variables
Zeekat 0:b0e2b38ab272 150 // differential action filter, same is used for both PID controllers
Zeekat 0:b0e2b38ab272 151 double m_f_v1 = 0, m_f_v2 = 0;
Zeekat 0:b0e2b38ab272 152 // input filter to smooth angle reference signals
Zeekat 0:b0e2b38ab272 153 double r1_f_v1 = 0, r1_f_v2 = 0, r2_f_v1 = 0, r2_f_v2 = 0;
Zeekat 0:b0e2b38ab272 154 // Define the storage variables and filter coeficients for eight filters
Zeekat 0:b0e2b38ab272 155 // EMG filter 1
Zeekat 0:b0e2b38ab272 156 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:b0e2b38ab272 157 // EMG filter 2
Zeekat 0:b0e2b38ab272 158 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:b0e2b38ab272 159
Zeekat 0:b0e2b38ab272 160 // Filter coefficients
Zeekat 0:b0e2b38ab272 161 // differential action filter (lowpass 5Hz at 50Hz)
Zeekat 0:b0e2b38ab272 162 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;
Zeekat 0:b0e2b38ab272 163 // input filter (lowpass 1Hz at 50samples) (used to make the angle signals smooth)
Zeekat 0:b0e2b38ab272 164 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:b0e2b38ab272 165
Zeekat 0:b0e2b38ab272 166 // EMG-Filter (calculated for 200)
Zeekat 0:b0e2b38ab272 167
Zeekat 0:b0e2b38ab272 168 const double numnotch50biq1_1 = 1;
Zeekat 0:b0e2b38ab272 169 const double numnotch50biq1_2 = 1.11568254634875e-11;
Zeekat 0:b0e2b38ab272 170 const double numnotch50biq1_3 = 1.00000002278002;
Zeekat 0:b0e2b38ab272 171 const double dennotch50biq1_2 = -0.0434777721916752;
Zeekat 0:b0e2b38ab272 172 const double dennotch50biq1_3 = 0.956543692050407;
Zeekat 0:b0e2b38ab272 173 // biquad 2 coefficienten
Zeekat 0:b0e2b38ab272 174 const double numnotch50biq2_1 = 1;
Zeekat 0:b0e2b38ab272 175 const double numnotch50biq2_2 = -1.11571030192437e-11;
Zeekat 0:b0e2b38ab272 176 const double numnotch50biq2_3 = 0.999999977219980;
Zeekat 0:b0e2b38ab272 177 const double dennotch50biq2_2 = 0.0434777721916751;
Zeekat 0:b0e2b38ab272 178 const double dennotch50biq2_3 = 0.956543692050417;
Zeekat 0:b0e2b38ab272 179
Zeekat 0:b0e2b38ab272 180 // highpass filter 20 Hz coefficienten
Zeekat 0:b0e2b38ab272 181 const double numhigh20_1 = 0.638945525159022;
Zeekat 0:b0e2b38ab272 182 const double numhigh20_2 = -1.27789105031804;
Zeekat 0:b0e2b38ab272 183 const double numhigh20_3 = 0.638945525159022;
Zeekat 0:b0e2b38ab272 184 const double denhigh20_2 = -1.14298050253990;
Zeekat 0:b0e2b38ab272 185 const double denhigh20_3 = 0.412801598096189;
Zeekat 0:b0e2b38ab272 186
Zeekat 0:b0e2b38ab272 187 // lowpass 5 Hz coefficienten
Zeekat 0:b0e2b38ab272 188 const double numlow5_1 =0.000241359049041961;
Zeekat 0:b0e2b38ab272 189 const double numlow5_2 =0.000482718098083923;
Zeekat 0:b0e2b38ab272 190 const double numlow5_3 =0.000241359049041961;
Zeekat 0:b0e2b38ab272 191 const double denlow5_2 =-1.95557824031504;
Zeekat 0:b0e2b38ab272 192 const double denlow5_3 =0.956543676511203;
Zeekat 0:b0e2b38ab272 193
Zeekat 0:b0e2b38ab272 194 ////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 195 /////////////////// START OF SIDE FUNCTIONS ////////////////////
Zeekat 0:b0e2b38ab272 196 //////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 197 // these functions are tailored to perform 1 specific function
Zeekat 0:b0e2b38ab272 198
Zeekat 0:b0e2b38ab272 199 // this funtion flips leds on and off accordin to input with 0 being on
Zeekat 0:b0e2b38ab272 200 void LED(int red,int green,int blue)
Zeekat 0:b0e2b38ab272 201 {
Zeekat 0:b0e2b38ab272 202 ledred.write(red);
Zeekat 0:b0e2b38ab272 203 ledgreen.write(green);
Zeekat 0:b0e2b38ab272 204 ledblue.write(blue);
Zeekat 0:b0e2b38ab272 205 }
Zeekat 0:b0e2b38ab272 206
Zeekat 0:b0e2b38ab272 207 // counts 2 radians
Zeekat 0:b0e2b38ab272 208 // this function takes counts from the encoder and converts it to the amount of radians from the zero position.
Zeekat 0:b0e2b38ab272 209 // It has been set up for standard 2X DECODING!!!
Zeekat 0:b0e2b38ab272 210 double get_radians(double counts)
Zeekat 0:b0e2b38ab272 211 {
Zeekat 0:b0e2b38ab272 212 double radians = (counts/4200)*2*pi; // 2X DECODING!!!!! ((32 counts/rotation, last warning)
Zeekat 0:b0e2b38ab272 213 return radians;
Zeekat 0:b0e2b38ab272 214 }
Zeekat 0:b0e2b38ab272 215
Zeekat 0:b0e2b38ab272 216
Zeekat 0:b0e2b38ab272 217 // This functions takes a 0->1 input, uses passing by reference (&c_reference)
Zeekat 0:b0e2b38ab272 218 // to create a reference that moves with a variable speed. It is meant for -1->1 values
Zeekat 0:b0e2b38ab272 219 double reference_f(double input, double &c_reference, double limlow, double limhigh, double Vmax)
Zeekat 0:b0e2b38ab272 220 {
Zeekat 0:b0e2b38ab272 221 double reference = c_reference + input * controlstep * Vmax ;
Zeekat 0:b0e2b38ab272 222 // two if statements check if the reference exceeds the limits placed upon the arms
Zeekat 0:b0e2b38ab272 223 if(reference < limlow){reference = limlow;}
Zeekat 0:b0e2b38ab272 224 if(reference > limhigh){reference = limhigh;}
Zeekat 0:b0e2b38ab272 225 c_reference = reference; // change the global variable to the latest location.
Zeekat 0:b0e2b38ab272 226 return reference;
Zeekat 0:b0e2b38ab272 227 }
Zeekat 0:b0e2b38ab272 228
Zeekat 0:b0e2b38ab272 229
Zeekat 0:b0e2b38ab272 230 // This function takes the controller outputvalue and ensures it is between -1 and 1
Zeekat 0:b0e2b38ab272 231 // 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:b0e2b38ab272 232 double outputlimiter (double output, double limit)
Zeekat 0:b0e2b38ab272 233 {
Zeekat 0:b0e2b38ab272 234 if(output> limit)
Zeekat 0:b0e2b38ab272 235 {
Zeekat 0:b0e2b38ab272 236 output = 1;
Zeekat 0:b0e2b38ab272 237 }
Zeekat 0:b0e2b38ab272 238 else if(output < limit && output > 0)
Zeekat 0:b0e2b38ab272 239 {
Zeekat 0:b0e2b38ab272 240 output = output;
Zeekat 0:b0e2b38ab272 241 }
Zeekat 0:b0e2b38ab272 242 else if(output > -limit && output < 0)
Zeekat 0:b0e2b38ab272 243 {
Zeekat 0:b0e2b38ab272 244 output = output;
Zeekat 0:b0e2b38ab272 245 }
Zeekat 0:b0e2b38ab272 246 else if(output < -limit)
Zeekat 0:b0e2b38ab272 247 {
Zeekat 0:b0e2b38ab272 248 (output = -1);
Zeekat 0:b0e2b38ab272 249 }
Zeekat 0:b0e2b38ab272 250 return output;
Zeekat 0:b0e2b38ab272 251 }
Zeekat 0:b0e2b38ab272 252
Zeekat 0:b0e2b38ab272 253
Zeekat 0:b0e2b38ab272 254 // BIQUADFILTER CODE GIVEN IN SHEETS (input format: den, den, nom, nom, nom)
Zeekat 0:b0e2b38ab272 255 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:b0e2b38ab272 256 {
Zeekat 0:b0e2b38ab272 257 double v = u - a1*v1 - a2*v2;
Zeekat 0:b0e2b38ab272 258 double y = b0*v + b1*v1 + b2*v2;
Zeekat 0:b0e2b38ab272 259 v2 = v1;
Zeekat 0:b0e2b38ab272 260 v1 = v;
Zeekat 0:b0e2b38ab272 261 return y;
Zeekat 0:b0e2b38ab272 262 }
Zeekat 0:b0e2b38ab272 263
Zeekat 0:b0e2b38ab272 264 // PID Controller given in sheets
Zeekat 0:b0e2b38ab272 265 // adapted to use the same differential filter, and to split the different terms
Zeekat 0:b0e2b38ab272 266 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
Zeekat 0:b0e2b38ab272 267 {
Zeekat 0:b0e2b38ab272 268 // Proportional
Zeekat 0:b0e2b38ab272 269 double P = Kp * e;
Zeekat 0:b0e2b38ab272 270 // Integral
Zeekat 0:b0e2b38ab272 271 e_int = e_int + Ts * e;
Zeekat 0:b0e2b38ab272 272 double I = e_int * Ki;
Zeekat 0:b0e2b38ab272 273 // Derivative
Zeekat 0:b0e2b38ab272 274 double e_derr = (e - e_prev)/Ts;
Zeekat 0:b0e2b38ab272 275 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:b0e2b38ab272 276 //
Zeekat 0:b0e2b38ab272 277 e_prev = e;
Zeekat 0:b0e2b38ab272 278 double D = Kd* e_derr;
Zeekat 0:b0e2b38ab272 279 // PID
Zeekat 0:b0e2b38ab272 280 double output = P + I + D;
Zeekat 0:b0e2b38ab272 281 return output;
Zeekat 0:b0e2b38ab272 282 }
Zeekat 0:b0e2b38ab272 283
Zeekat 0:b0e2b38ab272 284
Zeekat 0:b0e2b38ab272 285 // function that limits the angles that can be used in the motor reference signal
Zeekat 0:b0e2b38ab272 286 double angle_limits(double phi, double limlow, double limhigh)
Zeekat 0:b0e2b38ab272 287 {
Zeekat 0:b0e2b38ab272 288 if(phi < limlow)
Zeekat 0:b0e2b38ab272 289 {
Zeekat 0:b0e2b38ab272 290 phi = limlow;
Zeekat 0:b0e2b38ab272 291 }
Zeekat 0:b0e2b38ab272 292 if(phi > limhigh)
Zeekat 0:b0e2b38ab272 293 {
Zeekat 0:b0e2b38ab272 294 phi = limhigh;
Zeekat 0:b0e2b38ab272 295 }
Zeekat 0:b0e2b38ab272 296 return phi;
Zeekat 0:b0e2b38ab272 297 }
Zeekat 0:b0e2b38ab272 298
Zeekat 0:b0e2b38ab272 299 // this function adapts the filtered emg signal for use in the reference generation
Zeekat 0:b0e2b38ab272 300 // adds threshold value and normalizes between 0 and 1
Zeekat 0:b0e2b38ab272 301 double adapt_signal(double input)
Zeekat 0:b0e2b38ab272 302 {
Zeekat 0:b0e2b38ab272 303 // add threshold value for outputs
Zeekat 0:b0e2b38ab272 304 if(input < input_threshold){input = 0;}
Zeekat 0:b0e2b38ab272 305
Zeekat 0:b0e2b38ab272 306 // return the input to a value between 0 and 1 (otherwise you will get jumps in input)
Zeekat 0:b0e2b38ab272 307 input = (input-input_threshold) * (1/(1-input_threshold));
Zeekat 0:b0e2b38ab272 308
Zeekat 0:b0e2b38ab272 309 // if below 0 = 0 (otherwise values like -input_threshold start popping up)
Zeekat 0:b0e2b38ab272 310 if(input < 0){input = 0;}
Zeekat 0:b0e2b38ab272 311
Zeekat 0:b0e2b38ab272 312 // limit signal maximum to 1
Zeekat 0:b0e2b38ab272 313 if(input > 1){input = 1;}
Zeekat 0:b0e2b38ab272 314 return input;
Zeekat 0:b0e2b38ab272 315 }
Zeekat 0:b0e2b38ab272 316
Zeekat 0:b0e2b38ab272 317 // send stuff to putty to check things
Zeekat 0:b0e2b38ab272 318 void mod_send()
Zeekat 0:b0e2b38ab272 319 {
Zeekat 0:b0e2b38ab272 320 pc.printf("xx = %f, yy = %f, phi1 = %f, phi2 = %f \n",xx,yy,phi_one,phi_two);
Zeekat 0:b0e2b38ab272 321 }
Zeekat 0:b0e2b38ab272 322 /////////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 323 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
Zeekat 0:b0e2b38ab272 324 ///////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 325 // these functions are called by go-flags and are used to update main variables and send signals to motor
Zeekat 0:b0e2b38ab272 326
Zeekat 0:b0e2b38ab272 327 // function that updates the values of the filtered emg-signal
Zeekat 0:b0e2b38ab272 328 void EMG_filter()
Zeekat 0:b0e2b38ab272 329 {
Zeekat 0:b0e2b38ab272 330 // filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
Zeekat 0:b0e2b38ab272 331 double u1 = EMG_in.read();
Zeekat 0:b0e2b38ab272 332 double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 0:b0e2b38ab272 333 double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 0:b0e2b38ab272 334 double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 0:b0e2b38ab272 335 double y4 = abs(y3);
Zeekat 0:b0e2b38ab272 336 double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 0:b0e2b38ab272 337 // update global variables
Zeekat 0:b0e2b38ab272 338 output1 = y5;
Zeekat 0:b0e2b38ab272 339 output1_amp = y5*emg_gain1; // update global variable
Zeekat 0:b0e2b38ab272 340
Zeekat 0:b0e2b38ab272 341 // filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
Zeekat 0:b0e2b38ab272 342 double u1t = EMG_int.read();
Zeekat 0:b0e2b38ab272 343 double y1t = biquadfilter( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
Zeekat 0:b0e2b38ab272 344 double y2t = biquadfilter( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
Zeekat 0:b0e2b38ab272 345 double y3t = biquadfilter( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
Zeekat 0:b0e2b38ab272 346 double y4t = abs(y3t);
Zeekat 0:b0e2b38ab272 347 double y5t = biquadfilter( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
Zeekat 0:b0e2b38ab272 348 // update global variables
Zeekat 0:b0e2b38ab272 349 output2 = y5t;
Zeekat 0:b0e2b38ab272 350 output2_amp = y5t*emg_gain2;
Zeekat 0:b0e2b38ab272 351 }
Zeekat 0:b0e2b38ab272 352
Zeekat 0:b0e2b38ab272 353
Zeekat 0:b0e2b38ab272 354 // function that updates the required motor angles from the current filtered emg
Zeekat 0:b0e2b38ab272 355 void det_angles()
Zeekat 0:b0e2b38ab272 356 {
Zeekat 0:b0e2b38ab272 357 // convert global to local variable
Zeekat 0:b0e2b38ab272 358 double xy_input1 = output1_amp;
Zeekat 0:b0e2b38ab272 359 double xy_input2 = output2_amp;
Zeekat 0:b0e2b38ab272 360
Zeekat 0:b0e2b38ab272 361 // use potmeter for debugging purposes (note: does not give a smooth signal due to mechanical breakdown)
Zeekat 0:b0e2b38ab272 362 xy_input1 = potright.read();
Zeekat 0:b0e2b38ab272 363 xy_input2 = potleft.read();
Zeekat 0:b0e2b38ab272 364
Zeekat 0:b0e2b38ab272 365 xy_input1 = adapt_signal(xy_input1);
Zeekat 0:b0e2b38ab272 366 xy_input2 = adapt_signal(xy_input2);
Zeekat 0:b0e2b38ab272 367
Zeekat 0:b0e2b38ab272 368 double xy_main_input = xy_input1 - xy_input2 ; // subtract inputs to create a signal that can go from -1 to 1
Zeekat 0:b0e2b38ab272 369
Zeekat 0:b0e2b38ab272 370 //scope.set(0,xy_main_input);
Zeekat 0:b0e2b38ab272 371 // limit the output between -1 and 1 (signal is not supposed to be able to go above but last check)
Zeekat 0:b0e2b38ab272 372 if(xy_main_input>1) {xy_main_input = 1;}
Zeekat 0:b0e2b38ab272 373 if(xy_main_input<-1) {xy_main_input = -1;}
Zeekat 0:b0e2b38ab272 374
Zeekat 0:b0e2b38ab272 375 if(switch_xy == false) // use the signal to change the x-reference
Zeekat 0:b0e2b38ab272 376 {
Zeekat 0:b0e2b38ab272 377 xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x); // change the global x-reference
Zeekat 0:b0e2b38ab272 378 // calculate the y limits belonging to that particular x coordinate and update global variables
Zeekat 0:b0e2b38ab272 379 y_min = - sqrt(4900 - pow(xx,2));
Zeekat 0:b0e2b38ab272 380 if(y_min<y_min_max){y_min = y_min_max;} // make sure the arm cannot hit the table (may later be removed)
Zeekat 0:b0e2b38ab272 381 y_max = sqrt(4900 - pow(xx,2));
Zeekat 0:b0e2b38ab272 382
Zeekat 0:b0e2b38ab272 383 }
Zeekat 0:b0e2b38ab272 384 if(switch_xy == true) // use the signal to change the y-reference
Zeekat 0:b0e2b38ab272 385 {
Zeekat 0:b0e2b38ab272 386 yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y); // change the y-reference
Zeekat 0:b0e2b38ab272 387
Zeekat 0:b0e2b38ab272 388 }
Zeekat 0:b0e2b38ab272 389 // check the y-reference (otherwise if x is controlled after y has been controlled, the circle is not followed).
Zeekat 0:b0e2b38ab272 390 if(yy < y_min){yy = y_min;}
Zeekat 0:b0e2b38ab272 391 if(yy > y_max){yy = y_max;}
Zeekat 0:b0e2b38ab272 392
Zeekat 0:b0e2b38ab272 393 // let the arm make a circle (testing)
Zeekat 0:b0e2b38ab272 394 // xx = 60 + 5*cos(t);
Zeekat 0:b0e2b38ab272 395 // yy = 5*sin(t);
Zeekat 0:b0e2b38ab272 396 // t = t + 0.01;
Zeekat 0:b0e2b38ab272 397
Zeekat 0:b0e2b38ab272 398 // x-y to arm-angles math
Zeekat 0:b0e2b38ab272 399 double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
Zeekat 0:b0e2b38ab272 400 double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
Zeekat 0:b0e2b38ab272 401 double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
Zeekat 0:b0e2b38ab272 402 double theta_one = (atan2(yy,xx)+beta);
Zeekat 0:b0e2b38ab272 403 double theta_two = (-pi + alfa);
Zeekat 0:b0e2b38ab272 404
Zeekat 0:b0e2b38ab272 405 // convert arm-angles to motor angles( (x transmission) and offset (+ offset) to account for reset position)
Zeekat 0:b0e2b38ab272 406 double phi1 = 4*(theta_one) + phi_one_offset;
Zeekat 0:b0e2b38ab272 407 double phi2 = 4*(theta_one+theta_two) + phi_two_offset; // math assumes angle relative to first arm. motor does not change relative orientation, so angle wrt horizontal position is needed.
Zeekat 0:b0e2b38ab272 408 phi2 = -phi2; // reverse angle because of transmission.
Zeekat 0:b0e2b38ab272 409
Zeekat 0:b0e2b38ab272 410 // check the angles and apply the limits
Zeekat 0:b0e2b38ab272 411 phi1 = angle_limits(phi1,limlow1,limhigh1);
Zeekat 0:b0e2b38ab272 412 phi2 = angle_limits(phi2,limlow2,limhigh2);
Zeekat 0:b0e2b38ab272 413
Zeekat 0:b0e2b38ab272 414 // smooth the input signal (lowpass 1Hz). (to reduce the freq content after reaching limits and to make the signal less jittery)
Zeekat 0:b0e2b38ab272 415 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 0:b0e2b38ab272 416 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 0:b0e2b38ab272 417
Zeekat 0:b0e2b38ab272 418 // write into global variables
Zeekat 0:b0e2b38ab272 419 phi_one = phi1;
Zeekat 0:b0e2b38ab272 420 phi_two = phi2;
Zeekat 0:b0e2b38ab272 421
Zeekat 0:b0e2b38ab272 422 // if the reset button has been pressed, continiously write the start position into the global variables to reset the arm
Zeekat 0:b0e2b38ab272 423 if(reset == true)
Zeekat 0:b0e2b38ab272 424 {
Zeekat 0:b0e2b38ab272 425 phi_one = reset_phi_one;
Zeekat 0:b0e2b38ab272 426 phi_two = reset_phi_two;
Zeekat 0:b0e2b38ab272 427 }
Zeekat 0:b0e2b38ab272 428 }
Zeekat 0:b0e2b38ab272 429
Zeekat 0:b0e2b38ab272 430
Zeekat 0:b0e2b38ab272 431 // MOTOR 1
Zeekat 0:b0e2b38ab272 432 void motor1_control()
Zeekat 0:b0e2b38ab272 433 {
Zeekat 0:b0e2b38ab272 434 // change global into local variable
Zeekat 0:b0e2b38ab272 435 double reference1 = phi_one;
Zeekat 0:b0e2b38ab272 436
Zeekat 0:b0e2b38ab272 437 // add smooth start up
Zeekat 0:b0e2b38ab272 438 // for a certain amount of function iterations slowly add the delta phi between positions
Zeekat 0:b0e2b38ab272 439 // (used to gently move to start position or move to reset position)
Zeekat 0:b0e2b38ab272 440 if(rc1 < start_loops)
Zeekat 0:b0e2b38ab272 441 {
Zeekat 0:b0e2b38ab272 442 rc1++;
Zeekat 0:b0e2b38ab272 443 reference1 = phi_one_curr + ((double) rc1/start_loops)*(reference1-phi_one_curr);
Zeekat 0:b0e2b38ab272 444 }
Zeekat 0:b0e2b38ab272 445
Zeekat 0:b0e2b38ab272 446 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 0:b0e2b38ab272 447 double error1 = (reference1 - rads1); // determine the error (reference - position)
Zeekat 0:b0e2b38ab272 448 double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
Zeekat 0:b0e2b38ab272 449 // check the real angles and stop if exceeded (NaN protection) // doesnt work because start angle is lower so output stays 0
Zeekat 0:b0e2b38ab272 450 // if(rads1 < (limlow1 - 0.05)){m_output1 = 0;}
Zeekat 0:b0e2b38ab272 451 // if(rads1 > (limhigh1 + 0.05)){m_output1 = 0;}
Zeekat 0:b0e2b38ab272 452 m_output1 = outputlimiter(m_output1,1); // relimit the output between -1 and 1 for safety
Zeekat 0:b0e2b38ab272 453 if(m_output1 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:b0e2b38ab272 454 motor1_rich.write(0);
Zeekat 0:b0e2b38ab272 455 motor1_aan.write(m_output1);
Zeekat 0:b0e2b38ab272 456 } else if(m_output1 < 0) {
Zeekat 0:b0e2b38ab272 457 motor1_rich.write(1);
Zeekat 0:b0e2b38ab272 458 motor1_aan.write(abs(m_output1));
Zeekat 0:b0e2b38ab272 459 }
Zeekat 0:b0e2b38ab272 460 }
Zeekat 0:b0e2b38ab272 461
Zeekat 0:b0e2b38ab272 462 // MOTOR 2
Zeekat 0:b0e2b38ab272 463 void motor2_control()
Zeekat 0:b0e2b38ab272 464 {
Zeekat 0:b0e2b38ab272 465 double reference2 = phi_two;
Zeekat 0:b0e2b38ab272 466
Zeekat 0:b0e2b38ab272 467 // add smooth start up
Zeekat 0:b0e2b38ab272 468 // for a certain amount of function iterations slowly add the delta phi between positions
Zeekat 0:b0e2b38ab272 469 // (used to gently move to start position [x,y] = [60,0] or move to the reset position [phi1,phi2] = (0,0)
Zeekat 0:b0e2b38ab272 470 if(rc2 < start_loops)
Zeekat 0:b0e2b38ab272 471 {
Zeekat 0:b0e2b38ab272 472 rc2++;
Zeekat 0:b0e2b38ab272 473 reference2 = phi_two_curr + ((double) rc2/start_loops)*(reference2-phi_two_curr);
Zeekat 0:b0e2b38ab272 474 }
Zeekat 0:b0e2b38ab272 475
Zeekat 0:b0e2b38ab272 476 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 0:b0e2b38ab272 477 double error2 = (reference2 - rads2); // determine the error (reference - position)
Zeekat 0:b0e2b38ab272 478 double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
Zeekat 0:b0e2b38ab272 479 // check the real angles and stop if exceeded
Zeekat 0:b0e2b38ab272 480 // if(rads2 < limlow2 - 0.05){m_output2 = 0;}
Zeekat 0:b0e2b38ab272 481 // if(rads2 > limhigh2 + 0.05){m_output2 = 0;}
Zeekat 0:b0e2b38ab272 482 m_output2 = outputlimiter(m_output2,1); // final output limit (not really needed, is for safety)
Zeekat 0:b0e2b38ab272 483 if(m_output2 > 0) { // uses the calculated output to determine the direction of the motor
Zeekat 0:b0e2b38ab272 484 motor2_rich.write(0);
Zeekat 0:b0e2b38ab272 485 motor2_aan.write(m_output2);
Zeekat 0:b0e2b38ab272 486 } else if(m_output2 < 0) {
Zeekat 0:b0e2b38ab272 487 motor2_rich.write(1);
Zeekat 0:b0e2b38ab272 488 motor2_aan.write(abs(m_output2));
Zeekat 0:b0e2b38ab272 489 }
Zeekat 0:b0e2b38ab272 490 }
Zeekat 0:b0e2b38ab272 491
Zeekat 0:b0e2b38ab272 492 // calibrate the emg-signal
Zeekat 0:b0e2b38ab272 493 // works bij taking a certain amount of samples taking the max then normalize to the desired value
Zeekat 0:b0e2b38ab272 494 // went to max-value type. must be tested.!
Zeekat 0:b0e2b38ab272 495 void calibrate_amp()
Zeekat 0:b0e2b38ab272 496 {
Zeekat 0:b0e2b38ab272 497 double max1 = 0;
Zeekat 0:b0e2b38ab272 498 double max2 = 0;
Zeekat 0:b0e2b38ab272 499 for(int i = 0; i<cal_samples; i++)
Zeekat 0:b0e2b38ab272 500 {
Zeekat 0:b0e2b38ab272 501 EMG_filter(); // run filter
Zeekat 0:b0e2b38ab272 502 double input1 = output1; // take data from global variable
Zeekat 0:b0e2b38ab272 503 if(input1>max1){max1 = input1;} // take max input
Zeekat 0:b0e2b38ab272 504 double input2 = output2;
Zeekat 0:b0e2b38ab272 505 if(input2>max2){max2 = input2;} // take max input
Zeekat 0:b0e2b38ab272 506 wait(controlstep); // !! has to run at same interval as filter in main loop !! otherwise a 'different' signal will be used for calibration
Zeekat 0:b0e2b38ab272 507 }
Zeekat 0:b0e2b38ab272 508 emg_gain1 = normalize_emg_value/max1; // normalize the amplification so that the maximum signal hits the desired one
Zeekat 0:b0e2b38ab272 509 emg_gain2 = normalize_emg_value/max2;
Zeekat 0:b0e2b38ab272 510 pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2); // print the calculated gains to putty
Zeekat 0:b0e2b38ab272 511
Zeekat 0:b0e2b38ab272 512 }
Zeekat 0:b0e2b38ab272 513 //////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 514 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
Zeekat 0:b0e2b38ab272 515 ////////////////////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 516
Zeekat 0:b0e2b38ab272 517
Zeekat 0:b0e2b38ab272 518 void EMG_activate(){emg_go = true;}
Zeekat 0:b0e2b38ab272 519 void angle_activate(){cart_go = true;}
Zeekat 0:b0e2b38ab272 520 void motor1_activate(){motor1_go = true;}
Zeekat 0:b0e2b38ab272 521 void motor2_activate(){motor2_go = true;}
Zeekat 0:b0e2b38ab272 522
Zeekat 0:b0e2b38ab272 523 int main()
Zeekat 0:b0e2b38ab272 524 {
Zeekat 0:b0e2b38ab272 525 pc.baud(115200);
Zeekat 0:b0e2b38ab272 526 main_filter.attach(&EMG_activate, EMG_step);
Zeekat 0:b0e2b38ab272 527 cartesian.attach(&angle_activate, controlstep);
Zeekat 0:b0e2b38ab272 528 controller1.attach(&motor1_activate, controlstep); // call a go-flag
Zeekat 0:b0e2b38ab272 529 controller2.attach(&motor2_activate, controlstep);
Zeekat 0:b0e2b38ab272 530 send.attach(&mod_send, 1); // send data (only global variables) to putty
Zeekat 0:b0e2b38ab272 531 while(true)
Zeekat 0:b0e2b38ab272 532 {
Zeekat 0:b0e2b38ab272 533 // button press functions
Zeekat 0:b0e2b38ab272 534 // flow buttons
Zeekat 0:b0e2b38ab272 535 if(buttonlinks.read() == 0)
Zeekat 0:b0e2b38ab272 536 {
Zeekat 0:b0e2b38ab272 537 loop_start = !loop_start;
Zeekat 0:b0e2b38ab272 538 wait(buttonlinks.read() == 1);
Zeekat 0:b0e2b38ab272 539 wait(0.2);
Zeekat 0:b0e2b38ab272 540 }
Zeekat 0:b0e2b38ab272 541 if(buttonrechts.read() == 0)
Zeekat 0:b0e2b38ab272 542 {
Zeekat 0:b0e2b38ab272 543 calib_start = !calib_start;
Zeekat 0:b0e2b38ab272 544 wait(buttonrechts.read() == 1);
Zeekat 0:b0e2b38ab272 545 wait(0.2);
Zeekat 0:b0e2b38ab272 546 }
Zeekat 0:b0e2b38ab272 547 // reverse buttons
Zeekat 0:b0e2b38ab272 548 if(switch_xy_button.read() == 0)
Zeekat 0:b0e2b38ab272 549 {
Zeekat 0:b0e2b38ab272 550 switch_xy = !switch_xy;
Zeekat 0:b0e2b38ab272 551 wait(switch_xy_button.read() == 1);
Zeekat 0:b0e2b38ab272 552 led_right.write(!led_right.read()); // turn on led when switched to y control
Zeekat 0:b0e2b38ab272 553 wait(0.2);
Zeekat 0:b0e2b38ab272 554 }
Zeekat 0:b0e2b38ab272 555 if(reset_button.read() == 0)
Zeekat 0:b0e2b38ab272 556 {
Zeekat 0:b0e2b38ab272 557 reset = !reset;
Zeekat 0:b0e2b38ab272 558 wait(reset_button.read() == 1);
Zeekat 0:b0e2b38ab272 559 phi_one_curr = phi_one;
Zeekat 0:b0e2b38ab272 560 phi_two_curr = phi_two;
Zeekat 0:b0e2b38ab272 561 rc1 = 0;
Zeekat 0:b0e2b38ab272 562 rc2 = 0;
Zeekat 0:b0e2b38ab272 563 wait(0.2);
Zeekat 0:b0e2b38ab272 564
Zeekat 0:b0e2b38ab272 565 }
Zeekat 0:b0e2b38ab272 566 //////////////////////////////////////////////////
Zeekat 0:b0e2b38ab272 567 // Main Control stuff and options
Zeekat 0:b0e2b38ab272 568 if(loop_start == true && calib_start == false) // check if start button = true then start the main control loops
Zeekat 0:b0e2b38ab272 569 {
Zeekat 0:b0e2b38ab272 570 LED(1,1,0); // turn blue led on
Zeekat 0:b0e2b38ab272 571 if(cart_go) { cart_go = false; det_angles();}
Zeekat 0:b0e2b38ab272 572 if(emg_go) { emg_go = false; EMG_filter();}
Zeekat 0:b0e2b38ab272 573 if(motor1_go) { motor1_go = false; motor1_control();}
Zeekat 0:b0e2b38ab272 574 if(motor2_go) { motor2_go = false; motor2_control();}
Zeekat 0:b0e2b38ab272 575 }
Zeekat 0:b0e2b38ab272 576 // shut off both motors
Zeekat 0:b0e2b38ab272 577 if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);}
Zeekat 0:b0e2b38ab272 578
Zeekat 0:b0e2b38ab272 579 // turn green led on // start calibration procedures
Zeekat 0:b0e2b38ab272 580 if(loop_start == false && calib_start == true)
Zeekat 0:b0e2b38ab272 581 { LED(1,0,1);
Zeekat 0:b0e2b38ab272 582 motor1_aan.write(0);
Zeekat 0:b0e2b38ab272 583 motor2_aan.write(0);
Zeekat 0:b0e2b38ab272 584 calibrate_amp(); // 10 second calibration
Zeekat 0:b0e2b38ab272 585 calib_start = false; // turn calibration mode off
Zeekat 0:b0e2b38ab272 586 }
Zeekat 0:b0e2b38ab272 587
Zeekat 0:b0e2b38ab272 588 // turn red led on (show both buttons have been pressed)
Zeekat 0:b0e2b38ab272 589 if(loop_start == true && calib_start == true)
Zeekat 0:b0e2b38ab272 590 {
Zeekat 0:b0e2b38ab272 591 LED(0,1,1);
Zeekat 0:b0e2b38ab272 592 motor1_aan.write(0);
Zeekat 0:b0e2b38ab272 593 motor2_aan.write(0);
Zeekat 0:b0e2b38ab272 594 }
Zeekat 0:b0e2b38ab272 595
Zeekat 0:b0e2b38ab272 596 // turn leds off (both buttons false)
Zeekat 0:b0e2b38ab272 597 else { LED(1,1,1);}
Zeekat 0:b0e2b38ab272 598 }
Zeekat 0:b0e2b38ab272 599 }