Diff: main.cpp

Revision:

0:b0e2b38ab272

Child:

1:4c9994ac229c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 27 12:20:03 2015 +0000
@@ -0,0 +1,599 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "encoder.h"
+#include "HIDScope.h"
+#include "math.h"
+
+Serial pc(USBTX,USBRX);     // MODSERIAL output
+HIDScope scope(4);          // definieerd het aantal kanalen van de scope
+
+// Define Tickers and control frequencies
+Ticker          controller1, controller2, main_filter, cartesian, send;
+    // Go flag variables belonging to Tickers
+    volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;
+
+// Frequency control
+    double controlfreq = 50 ;    // controlloops frequentie (Hz)
+    double controlstep = 1/controlfreq; // timestep derived from controlfreq
+    
+    double EMG_freq = 200;
+    double EMG_step = 1/EMG_freq;
+
+//////////////////////// IN-OUTPUT /////////////////////////////////////////////
+//MOTOR OUTPUTPINS
+    PwmOut motor1_aan(D6), motor2_aan(D5);      // PWM signaal motor 2 (uit sheets)
+    DigitalOut motor1_rich(D7), motor2_rich(D4); // digitaal signaal voor richting
+
+// ENCODER INPUTPINS
+    Encoder motor1_enc(D12,D11), motor2_enc(D10,D9);        // encoder outputpins
+
+// EXTRA INPUTS AND REQUIRED VARIABLES
+//POTMETERS (used for debugging purposes, not reliable anymore)
+    AnalogIn potright(A4);      // define the potmeter outputpins
+    AnalogIn potleft(A5);
+    
+// Analoge input signalen defineren
+    AnalogIn    EMG_in(A0);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
+    AnalogIn    EMG_int(A2);                // deze leest A3 uit
+
+// BUTTONS 
+    // control flow             
+    DigitalIn   buttonlinks(PTA4);       
+    DigitalIn   buttonrechts(PTC6);
+    DigitalIn   reset_button(D1);
+        // init values
+        bool loop_start = false;
+        bool calib_start = false;
+        bool reset = false;
+        
+    // axis control
+    DigitalIn   switch_xy_button(D0);
+        // init values
+        bool switch_xy = false;
+// LED outputs on bioshield
+    DigitalOut led_right(D2);
+// LED outputs on dev-board
+    DigitalOut ledred(LED1);
+    DigitalOut ledgreen(LED2);
+    DigitalOut ledblue(LED3);
+    
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////// GLOBAL VARIABLES ///////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////
+
+double t = 0;       // used for circle movement
+// physical constants
+    const double L = 36;           // lenght of arms
+    const double pi = 3.1415926535897;      // pi
+
+    // angle limitations (in radians)
+    // motor1
+    const double limlow1 = 0.5;             // min height in rad
+    const double limhigh1 = 6.3;            // max height in rad
+    // motor 2
+    const double limlow2 = -4.0;            // maximum height, motor has been inverted due to transmission
+    const double limhigh2 = 2.5;              // minimum height in rad
+    
+    // offset angle (radians needed to change the arms to horizontal position from reset position)
+    const double phi_one_offset = 2.8;
+    const double phi_two_offset = 1.85;
+
+///////////////////////////
+// Main control loop transfer variables
+// here all variables that transfer bewtween the primary control functions
+
+// filter to calibration
+    double output1;
+    double output2;
+// filter to x-y
+    double output1_amp;
+    double output2_amp;
+// x-y to motor control
+    double phi_one;
+    double phi_two;
+
+////////////////////////////
+// NOTE: start position is [x,y] = [60,0], reset position is [phi1,phi2] = [0,0]
+// define smooth variables (is to create a slowed movement instead of going to ref value inmediately)
+    double start_up_time = 3;
+    double start_loops = start_up_time*controlfreq;
+    int rc1 = 0;            // counters in function to enable relatively smooth movement
+    int rc2 = 0;
+    
+// define return to start variables
+double reset_phi_one = 0;
+double reset_phi_two = 0;
+
+// storage variables to store the location at the beginning of the smoothed movement (0 on startup)
+double phi_one_curr = 0;
+double phi_two_curr = 0;
+
+// REFERENCE SETTINGS
+    double input_threshold = 0.25;   // the minimum value the signal must have in order to change the reference.
+ // Define storage variables for reference values (also start position)
+    double c_reference_x = 60, c_reference_y = 0;
+// x-settings   (no y-settings because these are calculated from the current x-position)
+    double x_min = 47, x_max = 70, y_min_max = -40;
+    double xx,yy,y_min,y_max;
+    // Define the maximum rate of change for the x and y reference signals (velocity)
+    double Vmax_x = 15, Vmax_y = 15;  // [cm/s]          
+
+
+// CONTROLLER SETTINGS
+    // motor 1
+    const double m1_Kp = 5;         // Proportional constant     
+    const double m1_Ki = 0.5;         // integration constant
+    const double m1_Kd = 0.4;         // differentiation constant
+    // motor 2
+    const double m2_Kp = 3;
+    const double m2_Ki = 0.5;
+    const double m2_Kd = 0.3;
+// storage variables.   these variables are used to store data between controller iterations
+    // motor 1
+    double m1_err_int = 0; 
+    double m1_prev_err = 0;
+    // motor 2
+    double m2_err_int = 0;
+    double m2_prev_err = 0;
+
+// EMG calibration variables
+double emg_gain1 = 7;       
+double emg_gain2 = 7;
+
+double cal_time = 2.5;       // amount of time calibration should take (seconds)
+double cal_samples = controlfreq*cal_time;  // amount of samples that is used (dependent on the frequency of the filter)
+double normalize_emg_value = 1;      // set the desired value to calibrate the signal to (eg max signal = 1)
+
+// FILTER VARIABLES
+    // storage variables
+        // differential action filter, same is used for both PID controllers
+            double m_f_v1 = 0, m_f_v2 = 0;
+        // input filter to smooth angle reference signals
+            double r1_f_v1 = 0, r1_f_v2 = 0, r2_f_v1 = 0, r2_f_v2 = 0;
+        // Define the storage variables and filter coeficients for eight filters
+        // EMG filter 1
+            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;
+        // EMG filter 2
+            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;
+        
+    // Filter coefficients
+        // differential action filter (lowpass 5Hz at 50Hz)
+            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;
+        // input filter (lowpass 1Hz at 50samples) (used to make the angle signals smooth)
+            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;    
+
+    // EMG-Filter (calculated for 200)
+    
+    const double numnotch50biq1_1 = 1;
+    const double numnotch50biq1_2 = 1.11568254634875e-11;
+    const double numnotch50biq1_3 = 1.00000002278002;
+    const double dennotch50biq1_2 = -0.0434777721916752;
+    const double dennotch50biq1_3 = 0.956543692050407;
+    // biquad 2 coefficienten
+    const double numnotch50biq2_1 = 1;
+    const double numnotch50biq2_2 = -1.11571030192437e-11;
+    const double numnotch50biq2_3 = 0.999999977219980;
+    const double dennotch50biq2_2 = 0.0434777721916751;
+    const double dennotch50biq2_3 = 0.956543692050417;
+ 
+    // highpass filter 20 Hz coefficienten
+    const double numhigh20_1 = 0.638945525159022;
+    const double numhigh20_2 = -1.27789105031804;
+    const double numhigh20_3 = 0.638945525159022;
+    const double denhigh20_2 = -1.14298050253990;
+    const double denhigh20_3 = 0.412801598096189;
+ 
+    // lowpass 5 Hz coefficienten
+    const double numlow5_1 =0.000241359049041961;
+    const double numlow5_2 =0.000482718098083923;
+    const double numlow5_3 =0.000241359049041961;
+    const double denlow5_2 =-1.95557824031504;
+    const double denlow5_3 =0.956543676511203;      
+     
+////////////////////////////////////////////////////////////////
+/////////////////// START OF SIDE FUNCTIONS ////////////////////
+//////////////////////////////////////////////////////////////
+// these functions are tailored to perform 1 specific function
+
+// this funtion flips leds on and off accordin to input with 0 being on
+void LED(int red,int green,int blue)
+{
+    ledred.write(red);
+    ledgreen.write(green);
+    ledblue.write(blue);
+}    
+    
+// counts 2 radians
+// this function takes counts from the encoder and converts it to the amount of radians from the zero position. 
+// It has been set up for standard 2X DECODING!!!
+double get_radians(double counts)       
+{
+    double radians = (counts/4200)*2*pi;        // 2X DECODING!!!!! ((32 counts/rotation, last warning)
+    return radians;
+}
+
+
+// This functions takes a 0->1 input, uses passing by reference (&c_reference)
+// to create a reference that moves with a variable speed. It is meant for -1->1 values
+double reference_f(double input, double &c_reference, double limlow, double limhigh, double Vmax)
+{
+    double reference = c_reference + input * controlstep * Vmax ;
+            //  two if statements check if the reference exceeds the limits placed upon the arms
+            if(reference < limlow){reference = limlow;}
+            if(reference > limhigh){reference = limhigh;}
+           c_reference = reference; // change the global variable to the latest location.
+    return reference;
+}
+
+
+// This function takes the controller outputvalue and ensures it is between -1 and 1
+// this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
+double outputlimiter (double output, double limit)
+{
+    if(output> limit)                                
+    {
+        output = 1;
+    }
+    else if(output < limit && output > 0)
+    {
+        output = output;
+    }
+    else if(output > -limit && output < 0)
+    {
+        output = output;
+    }
+    else if(output < -limit)
+    {
+        (output = -1);
+    }
+    return output;
+}
+
+
+// BIQUADFILTER CODE GIVEN IN SHEETS (input format: den, den, nom, nom, nom)
+double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
+{
+    double v = u - a1*v1 - a2*v2;
+    double y = b0*v + b1*v1 + b2*v2;
+    v2 = v1;
+    v1 = v;
+    return y;
+    }
+
+// PID Controller given in sheets
+// adapted to use the same differential filter, and to split the different terms
+double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
+{
+// Proportional
+double P = Kp * e;
+// Integral
+e_int = e_int + Ts * e;
+double I = e_int * Ki;
+// Derivative   
+double e_derr = (e - e_prev)/Ts;
+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);
+// 
+e_prev = e;
+double D = Kd* e_derr;
+// PID
+double output = P + I + D;
+return output;
+}
+
+
+// function that limits the angles that can be used in the motor reference signal
+ double angle_limits(double phi, double limlow, double limhigh)
+{
+    if(phi < limlow)
+    {
+        phi = limlow;
+    }
+    if(phi > limhigh)
+    {
+        phi = limhigh;
+    }
+    return phi;
+}
+
+// this function adapts the filtered emg signal for use in the reference generation
+// adds threshold value and normalizes between 0 and 1
+double adapt_signal(double input)
+{
+    // add threshold value for outputs
+    if(input < input_threshold){input = 0;}
+        
+    // return the input to a value between 0 and 1 (otherwise you will get jumps in input)
+    input = (input-input_threshold) * (1/(1-input_threshold));
+     
+    // if below 0 = 0       (otherwise values like -input_threshold start popping up)
+    if(input < 0){input = 0;}
+      
+    // limit signal maximum to 1
+    if(input > 1){input = 1;}
+    return input;
+}
+    
+// send stuff to putty to check things
+void mod_send()
+{
+    pc.printf("xx = %f, yy = %f, phi1 = %f, phi2 = %f \n",xx,yy,phi_one,phi_two);
+}
+/////////////////////////////////////////////////////////////////////
+////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
+///////////////////////////////////////////////////////////////////
+// these functions are called by go-flags and are used to update main variables and send signals to motor
+
+// function that updates the values of the filtered emg-signal
+void EMG_filter()
+{
+// filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
+    double u1 = EMG_in.read();
+    double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
+    double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
+    double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
+    double y4 = abs(y3);
+    double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
+    // update global variables
+    output1 = y5;
+    output1_amp = y5*emg_gain1;   // update global variable
+
+// filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
+    double u1t = EMG_int.read();
+    double y1t = biquadfilter( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
+    double y2t = biquadfilter( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
+    double y3t = biquadfilter( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
+    double y4t = abs(y3t);
+    double y5t = biquadfilter( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
+    // update global variables
+    output2 = y5t;
+    output2_amp = y5t*emg_gain2; 
+ }
+ 
+ 
+ // function that updates the required motor angles from the current filtered emg
+ void det_angles()
+{
+    // convert global to local variable
+    double xy_input1 = output1_amp;
+    double xy_input2 = output2_amp;
+
+    // use potmeter for debugging purposes (note: does not give a smooth signal due to mechanical breakdown)
+    xy_input1 = potright.read();
+    xy_input2 = potleft.read();
+    
+    xy_input1 = adapt_signal(xy_input1);
+    xy_input2 = adapt_signal(xy_input2);
+
+    double xy_main_input = xy_input1 - xy_input2 ;    // subtract inputs to create a signal that can go from -1 to 1
+ 
+    //scope.set(0,xy_main_input);
+    // limit the output between -1 and 1 (signal is not supposed to be able to go above but last check)
+    if(xy_main_input>1) {xy_main_input = 1;}
+    if(xy_main_input<-1) {xy_main_input = -1;}
+ 
+    if(switch_xy == false)          // use the signal to change the x-reference
+    {
+        xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x);        // change the global x-reference
+        // calculate the y limits belonging to that particular x coordinate and update global variables
+        y_min = - sqrt(4900 - pow(xx,2));
+        if(y_min<y_min_max){y_min = y_min_max;} // make sure the arm cannot hit the table (may later be removed)
+        y_max = sqrt(4900 - pow(xx,2));
+       
+    }
+    if(switch_xy == true)           // use the signal to change the y-reference
+    {
+        yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y);        // change the y-reference
+        
+    }
+    // check the y-reference (otherwise if x is controlled after y has been controlled, the circle is not followed).
+    if(yy < y_min){yy = y_min;}
+    if(yy > y_max){yy = y_max;}
+    
+    // let the arm make a circle (testing)
+    // xx = 60 + 5*cos(t);
+    // yy = 5*sin(t);
+    // t = t + 0.01;
+   
+    // x-y to arm-angles math
+    double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
+    double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
+    double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
+    double theta_one = (atan2(yy,xx)+beta); 
+    double theta_two = (-pi + alfa); 
+    
+    // convert arm-angles to motor angles( (x transmission) and offset (+ offset) to account for reset position)
+    double phi1 = 4*(theta_one) + phi_one_offset;
+    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.
+    phi2 = -phi2;   // reverse angle because of transmission.
+
+    // check the angles and apply the limits
+    phi1 = angle_limits(phi1,limlow1,limhigh1);
+    phi2 = angle_limits(phi2,limlow2,limhigh2);
+    
+    // smooth the input signal (lowpass 1Hz). (to reduce the freq content after reaching limits and to make the signal less jittery)   
+    phi1 = biquadfilter(phi1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
+    phi2 = biquadfilter(phi2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
+    
+    // write into global variables
+    phi_one = phi1;
+    phi_two = phi2;
+
+    // if the reset button has been pressed, continiously write the start position into the global variables to reset the arm  
+    if(reset == true)
+    {
+        phi_one = reset_phi_one;
+        phi_two = reset_phi_two;
+    }
+}
+
+
+// MOTOR 1
+void motor1_control()
+{
+     // change global into local variable  
+    double reference1 = phi_one;  
+    
+    // add smooth start up
+    // for a certain amount of function iterations slowly add the delta phi between positions
+    // (used to gently move to start position or move to reset position)
+    if(rc1 < start_loops)
+    {
+        rc1++;
+        reference1 = phi_one_curr + ((double) rc1/start_loops)*(reference1-phi_one_curr);
+    }
+  
+    double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
+    double error1 = (reference1 - rads1);                       // determine the error (reference - position)
+    double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
+    // check the real angles and stop if exceeded (NaN protection) // doesnt work because start angle is lower so output stays 0
+        // if(rads1 < (limlow1 - 0.05)){m_output1 = 0;}
+        // if(rads1 > (limhigh1 + 0.05)){m_output1 = 0;}
+        m_output1 = outputlimiter(m_output1,1); // relimit the output between -1 and 1 for safety
+    if(m_output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
+        motor1_rich.write(0);
+        motor1_aan.write(m_output1);
+    } else if(m_output1 < 0) {
+        motor1_rich.write(1);
+        motor1_aan.write(abs(m_output1));
+    }
+}
+
+// MOTOR 2
+void motor2_control()
+{
+    double reference2 = phi_two;
+      
+    // add smooth start up
+    // for a certain amount of function iterations slowly add the delta phi between positions
+    // (used to gently move to start position [x,y] = [60,0] or move to the reset position [phi1,phi2] = (0,0)
+    if(rc2 < start_loops)
+    {
+        rc2++;
+        reference2 = phi_two_curr + ((double) rc2/start_loops)*(reference2-phi_two_curr);
+    }
+    
+    double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
+    double error2 = (reference2 - rads2);                    // determine the error (reference - position)   
+    double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
+       // check the real angles and stop if exceeded
+        // if(rads2 < limlow2 - 0.05){m_output2 = 0;}
+        // if(rads2 > limhigh2 + 0.05){m_output2 = 0;}
+    m_output2 = outputlimiter(m_output2,1);     // final output limit (not really needed, is for safety)
+    if(m_output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
+        motor2_rich.write(0);
+        motor2_aan.write(m_output2);
+    } else if(m_output2 < 0) {
+        motor2_rich.write(1);
+        motor2_aan.write(abs(m_output2));
+    }
+}
+
+// calibrate the emg-signal
+// works bij taking a certain amount of samples taking the max then normalize to the desired value
+// went to max-value type. must be tested.!
+void calibrate_amp()
+{
+    double max1 = 0;
+    double max2 = 0;
+    for(int i = 0; i<cal_samples; i++)
+    {
+        EMG_filter();       // run filter
+        double input1 = output1;        // take data from global variable
+        if(input1>max1){max1 = input1;}       // take max input
+        double input2 = output2;
+        if(input2>max2){max2 = input2;}       // take max input
+        wait(controlstep);      // !! has to run at same interval as filter in main loop !! otherwise a 'different' signal will be used for calibration
+    }
+    emg_gain1 = normalize_emg_value/max1;  // normalize the amplification so that the maximum signal hits the desired one
+    emg_gain2 = normalize_emg_value/max2;
+   pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2); // print the calculated gains to putty
+
+}
+//////////////////////////////////////////////////////////////////
+//////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
+////////////////////////////////////////////////////////////////
+
+
+void EMG_activate(){emg_go = true;}
+void angle_activate(){cart_go = true;}
+void motor1_activate(){motor1_go = true;}
+void motor2_activate(){motor2_go = true;}
+
+int main()
+{
+    pc.baud(115200);
+    main_filter.attach(&EMG_activate, EMG_step);
+    cartesian.attach(&angle_activate, controlstep);
+    controller1.attach(&motor1_activate, controlstep);      // call a go-flag
+    controller2.attach(&motor2_activate, controlstep); 
+    send.attach(&mod_send, 1);      // send data (only global variables) to putty
+    while(true) 
+    {
+        // button press functions
+        // flow buttons
+        if(buttonlinks.read() == 0)
+        {
+            loop_start = !loop_start;     
+            wait(buttonlinks.read() == 1);
+            wait(0.2);             
+        }
+        if(buttonrechts.read() == 0)
+        {
+            calib_start = !calib_start;     
+            wait(buttonrechts.read() == 1);
+            wait(0.2);             
+        }
+        // reverse buttons
+        if(switch_xy_button.read() == 0)
+        {
+           switch_xy = !switch_xy;     
+           wait(switch_xy_button.read() == 1);
+           led_right.write(!led_right.read());  // turn on led when switched to y control
+           wait(0.2);             
+        }
+        if(reset_button.read() == 0)
+        {
+            reset = !reset;     
+            wait(reset_button.read() == 1);
+            phi_one_curr = phi_one;
+            phi_two_curr = phi_two;
+            rc1 = 0;
+            rc2 = 0;     
+            wait(0.2);
+        
+        }
+        //////////////////////////////////////////////////
+        // Main Control stuff and options
+        if(loop_start == true && calib_start == false)        // check if start button = true then start the main control loops
+        {
+            LED(1,1,0); // turn blue led on
+            if(cart_go)   { cart_go = false; det_angles();}
+            if(emg_go)    { emg_go = false; EMG_filter();}
+            if(motor1_go) { motor1_go = false; motor1_control();}
+            if(motor2_go) { motor2_go = false; motor2_control();}
+        }
+        // shut off both motors 
+        if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);}
+        
+        // turn green led on // start calibration procedures
+        if(loop_start == false && calib_start == true) 
+        {   LED(1,0,1); 
+            motor1_aan.write(0); 
+            motor2_aan.write(0);
+            calibrate_amp();        // 10 second calibration
+            calib_start = false;    // turn calibration mode off
+        }
+        
+        // turn red led on (show both buttons have been pressed)
+        if(loop_start == true && calib_start == true) 
+        { 
+        LED(0,1,1); 
+        motor1_aan.write(0); 
+        motor2_aan.write(0);
+        }
+        
+        // turn leds off (both buttons false)
+        else { LED(1,1,1);}
+    }
+}
\ No newline at end of file