Wouter Bos / Mbed 2 deprecated group29_x_y_control_dynamic_x-lims

presentatie versie met potmeters enabled

Dependencies:   Encoder HIDScope mbed

main.cpp@11:44b1c5b3b378, 2015-11-02 (annotated)

Committer:
Zeekat
Date:
Mon Nov 02 14:15:11 2015 +0000
Revision:
11:44b1c5b3b378
Parent:
10:93a76bd81eef 
add tekst;

Who changed what in which revision?

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