main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 #include "encoder.h"
00004 #include "HIDScope.h"
00005 #include "math.h"
00006 
00007 Serial pc(USBTX,USBRX);     // MODSERIAL output
00008 HIDScope scope(4);          // definieerd het aantal kanalen van de scope
00009 
00010 // Define Tickers and control frequencies
00011 Ticker          controller1, controller2, main_filter, cartesian, send;
00012     // Go flag variables belonging to Tickers
00013     volatile bool motor1_go = false, motor2_go = false, emg_go = false, cart_go = false;
00014 
00015 // Frequency control
00016     double controlfreq = 50 ;    // controlloops frequentie (Hz)
00017     double controlstep = 1/controlfreq; // timestep derived from controlfreq
00018 
00019 
00020 //////////////////////// IN-OUTPUT /////////////////////////////////////////////
00021 //MOTOR OUTPUTPINS
00022     PwmOut motor1_aan(D6), motor2_aan(D5);      // PWM signaal motor 2 (uit sheets)
00023     DigitalOut motor1_rich(D7), motor2_rich(D4); // digitaal signaal voor richting
00024 
00025 // ENCODER INPUTPINS
00026     Encoder motor1_enc(D12,D11), motor2_enc(D10,D9);        // encoder outputpins
00027 
00028 // EXTRA INPUTS AND REQUIRED VARIABLES
00029 //POTMETERS (used for debuggin purposes, not reliable anymore)
00030     AnalogIn potright(A4);      // define the potmeter outputpins
00031     AnalogIn potleft(A5);
00032     
00033 // Analoge input signalen defineren
00034     AnalogIn    EMG_in(A0);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A2 uit te lezen
00035     AnalogIn    EMG_int(A2);                // deze leest A3 uit
00036 
00037 // BUTTONS 
00038     // control flow             
00039     DigitalIn   buttonlinks(PTA4);       
00040     DigitalIn   buttonrechts(PTC6);
00041     DigitalIn   reset_button(D1);
00042         // init values
00043         bool loop_start = false;
00044         bool calib_start = false;
00045         bool reset = false;
00046         
00047     // axis control
00048     DigitalIn   switch_xy_button(D0);
00049         // init values
00050         bool switch_xy = false;
00051 // LED outputs on bioshield
00052     DigitalOut led_right(D2);
00053 // LED outputs on dev-board
00054     DigitalOut ledred(LED1);
00055     DigitalOut ledgreen(LED2);
00056     DigitalOut ledblue(LED3);
00057     
00058 
00059 //////////////////////////////////////////////////////////////////////////////////////////////
00060 
00061 double t = 0;
00062 // physical constants
00063     const double L = 36;           // lenght of arms
00064     const double pi = 3.1415926535897;
00065 
00066     // angle limitations (in radians)
00067     // motor1
00068     const double limlow1 = 1;             // min height in rad
00069     const double limhigh1 = 6.3;            // max height in rad
00070     // motor 2
00071     const double limlow2 = -4.0;            // maximum height, motor has been inverted due to transmission
00072     const double limhigh2 = 2.5;              // minimum height in rad
00073 
00074 ///////////////////////////
00075 // Main control loop transfer variables
00076 // here all variables that transfer bewtween the primary control functions
00077 
00078 // filter to calibration
00079     double output1;
00080     double output2;
00081 // filter to x-y
00082     double output1_amp;
00083     double output2_amp;
00084 // x-y to motor control
00085     double phi_one;
00086     double phi_two;
00087 
00088 // define start up variables (is to create a delayed start up instead of going to the ref value inmediately)
00089     double start_up_time = 3;
00090     double start_loops = start_up_time*controlfreq;
00091     int rc1 = 0;            // counters in function to enable slow start up
00092     int rc2 = 0;
00093     
00094 // define return to start variables
00095 double reset_phi_one = 0;
00096 double reset_phi_two = 0;
00097 
00098 double phi_one_curr = 0;
00099 double phi_two_curr = 0;
00100 
00101 // REFERENCE SETTINGS
00102     double input_threshold = 0.25;   // the minimum value the signal must have to change the reference.
00103  // Define storage variables for reference values (also start position)
00104     double c_reference_x = 60, c_reference_y = 0;
00105 // x-settings   (no y-settings because these are calculated from the current x-position)
00106     double x_min = 47, x_max = 70;
00107     double xx,yy,y_min,y_max;
00108     // Define the maximum rate of change for the reference (velocity)
00109     double Vmax_x = 10, Vmax_y = 10;  // [cm/s]          
00110 
00111 
00112 // CONTROLLER SETTINGS
00113     // motor 1
00114     const double m1_Kp = 5;         // Proportional constant     
00115     const double m1_Ki = 0.5;         // integration constant
00116     const double m1_Kd = 0.4;         // differentiation constant
00117     // motor 2
00118     const double m2_Kp = 3;
00119     const double m2_Ki = 0.5;
00120     const double m2_Kd = 0.3;
00121 // storage variables.   these variables are used to store data between controller iterations
00122     // motor 1
00123     double m1_err_int = 0; 
00124     double m1_prev_err = 0;
00125     // motor 2
00126     double m2_err_int = 0;
00127     double m2_prev_err = 0;
00128 
00129 // EMG calibration variables
00130 double emg_gain1 = 7;       
00131 double emg_gain2 = 7;
00132 
00133 double cal_time = 2.5;       // amount of time calibration should take
00134 double cal_samples = controlfreq*cal_time;
00135 double normalize_emg_value = 1;      // set te desired value to calibrate the signal to
00136 
00137 // FILTER VARIABLES
00138     // storage variables
00139         // differential action filter, same is used for both controllers
00140             double m_f_v1 = 0, m_f_v2 = 0;
00141         // input filter to smooth signal
00142             double r1_f_v1 = 0, r1_f_v2 = 0, r2_f_v1 = 0, r2_f_v2 = 0;
00143         // Define the storage variables and filter coeficients for eight filters
00144         // EMG filter 1
00145             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;
00146         // EMG filter2
00147             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;
00148         
00149     // Filter coefficients
00150         // differential action filter (lowpass 5Hz at 50Hz)
00151             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;
00152         // input filter (lowpass 1Hz at 50samples) (used to make the x-y angle signal as smooth as possible
00153             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;    
00154     // EMG-Filter (calculated for 500Hz)
00155         // tweede orde notch filter 50 Hz
00156         // biquad 1 coefficienten
00157             const double numnotch50biq1_1 = 1, numnotch50biq1_2 = -1.61816178466632, numnotch50biq1_3 = 1.00000006127058, dennotch50biq1_2 = -1.59325742941798, dennotch50biq1_3 = 0.982171881701431;
00158         // biquad 2 coefficienten
00159             const double numnotch50biq2_1 = 1, numnotch50biq2_2 = -1.61816171933244, numnotch50biq2_3 = 0.999999938729428, dennotch50biq2_2 = -1.61431180968071, dennotch50biq2_3 = 0.982599066293075;
00160         // highpass filter 20 Hz coefficienten
00161             const double numhigh20_1 = 0.837089190566345, numhigh20_2 = -1.67417838113269, numhigh20_3 = 0.837089190566345, denhigh20_2 = -1.64745998107698, denhigh20_3 = 0.700896781188403;
00162         // lowpass 5 Hz coefficienten
00163             const double numlow5_1 =0.000944691843840162, numlow5_2 =0.00188938368768032, numlow5_3 =0.000944691843840162, denlow5_2 =-1.91119706742607, denlow5_3 =0.914975834801434;
00164 
00165 ////////////////////////////////////////////////////////////////
00166 /////////////////// START OF SIDE FUNCTIONS ////////////////////
00167 //////////////////////////////////////////////////////////////
00168 // these functions are tailored to perform 1 specific function
00169 
00170 // this funtion flips leds on and off accordin to input with 0 being on
00171 void LED(int red,int green,int blue)
00172 {
00173     ledred.write(red);
00174     ledgreen.write(green);
00175     ledblue.write(blue);
00176 }    
00177     
00178 // counts 2 radians
00179 // this function takes counts from the encoder and converts it to the amount of radians from the zero position. 
00180 // It has been set up for standard 2X DECODING!!!
00181 double get_radians(double counts)       
00182 {
00183     double radians = (counts/4200)*2*pi;        // 2X DECODING!!!!! ((32 counts/rotation, last warning)
00184     return radians;
00185 }
00186 
00187 
00188 // This functions takes a 0->1 input, uses passing by reference (&c_reference)
00189 // to create a reference that moves with a variable speed. It is meant for -1->1 values
00190 double reference_f(double input, double &c_reference, double limlow, double limhigh, double Vmax)
00191 {
00192     double reference = c_reference + input * controlstep * Vmax ;
00193             // two if statements check if the reference exceeds the limits placed upon the arms
00194             if(reference < limlow){reference = limlow;}
00195             if(reference > limhigh){reference = limhigh;}
00196            c_reference = reference; // change the global variable to the latest location.
00197     return reference;
00198 }
00199 
00200 
00201 // This function takes the controller outputvalue and ensures it is between -1 and 1
00202 // this is done to limit the motor input to possible values (the motor takes 0 to 1 and the sign changes the direction).
00203 double outputlimiter (double output, double limit)
00204 {
00205     if(output> limit)                                
00206     {
00207         output = 1;
00208     }
00209     else if(output < limit && output > 0)
00210     {
00211         output = output;
00212     }
00213     else if(output > -limit && output < 0)
00214     {
00215         output = output;
00216     }
00217     else if(output < -limit)
00218     {
00219         (output = -1);
00220     }
00221     return output;
00222 }
00223 
00224 
00225 // BIQUADFILTER CODE GIVEN IN SHEETS (input format: den, den, nom, nom, nom)
00226 double biquadfilter(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
00227 {
00228     double v = u - a1*v1 - a2*v2;
00229     double y = b0*v + b1*v1 + b2*v2;
00230     v2 = v1;
00231     v1 = v;
00232     return y;
00233     }
00234 
00235 // PID Controller given in sheets
00236 // aangepast om zelfde filter te gebruiken en om de termen te splitsen
00237 double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,double &e_int, double &e_prev)
00238 {
00239 // Proportional
00240 double P = Kp * e;
00241 // Integral
00242 e_int = e_int + Ts * e;
00243 double I = e_int * Ki;
00244 // Derivative   
00245 double e_derr = (e - e_prev)/Ts;
00246 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);
00247 // 
00248 e_prev = e;
00249 double D = Kd* e_derr;
00250 // PID
00251 double output = P + I + D;
00252 return output;
00253 }
00254 
00255 
00256 // function that limits the angles that can be used in the motor reference signal
00257  double angle_limits(double phi, double limlow, double limhigh)
00258 {
00259     if(phi < limlow)
00260     {
00261         phi = limlow;
00262     }
00263     if(phi > limhigh)
00264     {
00265         phi = limhigh;
00266     }
00267     return phi;
00268 }
00269 
00270 void mod_send()
00271 {
00272     pc.printf("xx = %f, yy = %f, phi1 = %f, phi2 = %f \n",xx,yy,phi_one,phi_two);
00273 }
00274 /////////////////////////////////////////////////////////////////////
00275 ////////////////// PRIMARY CONTROL FUNCTIONS ///////////////////////
00276 ///////////////////////////////////////////////////////////////////
00277 // these functions are called by go-flags and are used to update main variables and send signals to motor
00278 
00279 // function that updates the inputs
00280 void EMG_filter()
00281 {
00282 // filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
00283     double u1 = EMG_in.read();
00284     double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
00285     double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
00286     double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
00287     double y4 = abs(y3);
00288     double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
00289     // update global variables
00290     output1 = y5;
00291     output1_amp = y5*emg_gain1;   // update global variable
00292 
00293 // filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
00294     double u1t = EMG_int.read();
00295     double y1t = biquadfilter( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
00296     double y2t = biquadfilter( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
00297     double y3t = biquadfilter( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
00298     double y4t = abs(y3t);
00299     double y5t = biquadfilter( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
00300     // update global variables
00301     output2 = y5t;
00302     output2_amp = y5t*emg_gain2; 
00303     
00304     scope.set(0,output1);
00305     scope.set(1,output2);
00306     scope.set(0,output1_amp);
00307     scope.set(1,output2_amp);
00308     scope.send();
00309  }
00310  
00311  
00312  // function that updates the required motor angles
00313  void det_angles()
00314 {
00315     // convert global to local variable
00316     double xy_input1 = output1_amp;
00317     double xy_input2 = output2_amp;
00318 
00319     // use potmeter for debugging purposes
00320     xy_input1 = potright.read();
00321     xy_input2 = potleft.read();
00322     
00323     // add threshold value for outputs
00324     if(xy_input1 < input_threshold){xy_input1 = 0;}
00325     if(xy_input2 < input_threshold){xy_input2 = 0;}
00326     
00327     // return the input to a value between 0 and 1 (otherwise you will get jumps in input)
00328     xy_input1 = (xy_input1-input_threshold) * (1/(1-input_threshold)); 
00329     xy_input2 = (xy_input2-input_threshold) * (1/(1-input_threshold)) ;
00330     // if below 0 = 0
00331     if(xy_input1 < 0){xy_input1 = 0;}
00332     if(xy_input2 < 0){xy_input2 = 0;}
00333     
00334     
00335     // limit signal to 1
00336     if(xy_input1 > 1){xy_input1 = 1;}
00337     if(xy_input2 > 1){xy_input2 = 1;}
00338        
00339     double xy_main_input = xy_input1 - xy_input2 ;    // subtract inputs to create a signal that can go from -1 to 1
00340  
00341     //scope.set(0,xy_main_input);
00342     
00343     // limit the output between -1 and 1
00344     if(xy_main_input>1) {xy_main_input = 1;}
00345     if(xy_main_input<-1) {xy_main_input = -1;}
00346  
00347     if(switch_xy == false)          // x-movement
00348     {
00349         xx = reference_f(xy_main_input,c_reference_x,x_min,x_max,Vmax_x);        // change the global x-reference
00350         // calculate the y limits belonging to that particular x coordinate
00351         y_min = - sqrt(4900 - pow(xx,2));
00352         if(y_min<-35){y_min = -35;} // make sure the arm cannot hit the table (may later be removed)
00353         y_max = sqrt(4900 - pow(xx,2));
00354        
00355     }
00356     if(switch_xy == true)
00357     {
00358         yy = reference_f(xy_main_input,c_reference_y,y_min,y_max,Vmax_y);        // change the y-reference
00359         
00360     }
00361     // last check for the depedent y-reference (otherwise if x is controlled after y, the circle is not followed).
00362     if(yy < y_min){yy = y_min;}
00363     if(yy > y_max){yy = y_max;}
00364     
00365     // let the arm make a circle
00366     // xx = 60 + 5*cos(t);
00367     // yy = 5*sin(t);
00368     // t = t + 0.01;
00369    
00370     // x-y to arm-angles math
00371     double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
00372     double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
00373     double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
00374     double theta_one = (atan2(yy,xx)+beta); 
00375     double theta_two = (-pi + alfa); 
00376     
00377     // convert arm-angles to motor angles (x transmission) and offset (+ offset) to account for reset position
00378     double phi1 = 4*(theta_one) + 2.8;
00379     double phi2 = 4*(theta_one+theta_two) + 1.85;       // math assumes angle relative to first arm. motor does not change relative orientation, so angle wrt reference position is needed.
00380     phi2 = -phi2;   // reverse angle because of double timing belt.
00381 
00382     // check the input angles and apply the limits
00383     phi1 = angle_limits(phi1,limlow1,limhigh1);
00384     phi2 = angle_limits(phi2,limlow2,limhigh2);
00385     
00386     // smooth the input signal (lowpass 1Hz). (to reduce the freq content after reaching limit)   
00387     phi1 = biquadfilter(phi1, r1_f_v1, r1_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
00388     phi2 = biquadfilter(phi2, r2_f_v1, r2_f_v2, r1_f_a1, r1_f_a2, r1_f_b0, r1_f_b1, r1_f_b2);
00389     
00390     // write into global variables
00391     phi_one = phi1;
00392     phi_two = phi2;
00393     
00394     if(reset == true)
00395     {
00396         phi_one = reset_phi_one;
00397         phi_two = reset_phi_two;
00398     }
00399     
00400     // modserial
00401     // pc.printf("x = %f, y = %f, phi_one = %f, phi_two = %f \n",xx,yy,phi_one,phi_two);
00402 }
00403 // MOTOR 1
00404 void motor1_control()
00405 {
00406      // change global into local variable  
00407     double reference1 = phi_one;  
00408     
00409     // add smooth start up
00410     if(rc1 < start_loops)
00411     {
00412         rc1++;
00413         reference1 = phi_one_curr + ((double) rc1/start_loops)*(reference1-phi_one_curr);
00414     }
00415   
00416     double rads1 = get_radians(motor1_enc.getPosition());    // determine the position of the motor
00417     double error1 = (reference1 - rads1);                       // determine the error (reference - position)
00418     double m_output1 = PID(error1, m1_Kp, m1_Ki, m1_Kd, controlstep, m1_err_int, m1_prev_err);
00419     // check the real angles and stop if exceeded (NaN protection)
00420         // if(rads1 < (limlow1 - 0.05)){m_output1 = 0;}
00421         // if(rads1 > (limhigh1 + 0.05)){m_output1 = 0;}
00422         m_output1 = outputlimiter(m_output1,1); // relimit the output between -1 and 1 for safety
00423     if(m_output1 > 0) {                    // uses the calculated output to determine the direction  of the motor
00424         motor1_rich.write(0);
00425         motor1_aan.write(m_output1);
00426     } else if(m_output1 < 0) {
00427         motor1_rich.write(1);
00428         motor1_aan.write(abs(m_output1));
00429     }
00430 }
00431 
00432 // MOTOR 2
00433 void motor2_control()
00434 {
00435     double reference2 = phi_two;  
00436     
00437     if(rc2 < start_loops)
00438     {
00439         rc2++;
00440         reference2 = phi_two_curr + ((double) rc2/start_loops)*(reference2-phi_two_curr);
00441     }
00442     double rads2 = get_radians(motor2_enc.getPosition());    // determine the position of the motor
00443     double error2 = (reference2 - rads2);                    // determine the error (reference - position)
00444     
00445     
00446     double m_output2 = PID(error2, m2_Kp, m2_Ki, m2_Kd, controlstep, m2_err_int, m2_prev_err);
00447        // check the real angles and stop if exceeded
00448         // if(rads2 < limlow2 - 0.05){m_output2 = 0;}
00449         // if(rads2 > limhigh2 + 0.05){m_output2 = 0;}
00450     m_output2 = outputlimiter(m_output2,1);     // final output limit (not really needed, is for safety)
00451     if(m_output2 > 0) {                    // uses the calculated output to determine the direction  of the motor
00452         motor2_rich.write(0);
00453         motor2_aan.write(m_output2);
00454     } else if(m_output2 < 0) {
00455         motor2_rich.write(1);
00456         motor2_aan.write(abs(m_output2));
00457     }
00458 }
00459 
00460 // calibrate the emg-signal
00461 // works bij taking a certain amount of samples adding them and then normalize to the desired value
00462 // STILL BUGGED!!!      // went to max-value type. must be tested.!
00463 void calibrate_amp()
00464 {
00465     double max1 = 0;
00466     double max2 = 0;
00467     for(int i = 0; i<cal_samples; i++)
00468     {
00469         EMG_filter();       // run filter
00470         double input1 = output1;        // take data from global variable
00471         if(input1>max1){max1 = input1;}       // take max input
00472         double input2 = output2;
00473         if(input2>max2){max2 = input2;}       // take max input
00474         wait(controlstep);
00475     }
00476     emg_gain1 = normalize_emg_value/max1;  // normalize the amplification so that the maximum signal hits the desired one
00477     emg_gain2 = normalize_emg_value/max2;
00478    pc.printf("gain1 = %f, gain2 = %f",emg_gain1,emg_gain2);
00479 
00480 }
00481 //////////////////////////////////////////////////////////////////
00482 //////////// DEFINE GO-FLAG FUNCTIONS ///////////////////////////
00483 ////////////////////////////////////////////////////////////////
00484 
00485 
00486 void EMG_activate(){emg_go = true;}
00487 void angle_activate(){cart_go = true;}
00488 void motor1_activate(){motor1_go = true;}
00489 void motor2_activate(){motor2_go = true;}
00490 
00491 int main()
00492 {
00493     pc.baud(115200);
00494     main_filter.attach(&EMG_activate, controlstep);
00495     cartesian.attach(&angle_activate, controlstep);
00496     controller1.attach(&motor1_activate, controlstep);      // call a go-flag
00497     controller2.attach(&motor2_activate, controlstep); 
00498     send.attach(&mod_send, 1);  
00499     while(true) 
00500     {
00501         // button press functions
00502         // flow buttons
00503         if(buttonlinks.read() == 0)
00504         {
00505             loop_start = !loop_start;     
00506             wait(buttonlinks.read() == 1);
00507             wait(0.3);             
00508         }
00509         if(buttonrechts.read() == 0)
00510         {
00511             calib_start = !calib_start;     
00512             wait(buttonrechts.read() == 1);
00513             wait(0.3);             
00514         }
00515         // reverse buttons
00516         if(switch_xy_button.read() == 0)
00517         {
00518            switch_xy = !switch_xy;     
00519            wait(switch_xy_button.read() == 1);
00520            led_right.write(!led_right.read());  // turn on led when switched to y control
00521            wait(0.2);             
00522         }
00523         if(reset_button.read() == 0)
00524         {
00525             reset = !reset;     
00526             wait(reset_button.read() == 1);
00527             phi_one_curr = phi_one;
00528             phi_two_curr = phi_two;
00529             rc1 = 0;
00530             rc2 = 0;     
00531             wait(0.3);
00532         
00533         }
00534         //////////////////////////////////////////////////
00535         // Main Control stuff and options
00536         if(loop_start == true && calib_start == false)        // check if start button = true then start the main control loops
00537         {
00538             LED(1,1,0); // turn blue led on
00539             if(cart_go)   { cart_go = false; det_angles();}
00540             if(emg_go)    { emg_go = false; EMG_filter();}
00541             if(motor1_go) { motor1_go = false; motor1_control();}
00542             if(motor2_go) { motor2_go = false; motor2_control();}
00543         }
00544         // shut off both motors 
00545         if(loop_start == false) {motor1_aan.write(0); motor2_aan.write(0);} // c_reference_x = 60; c_reference_y = 0;}
00546         
00547         // turn green led on // start calibration procedures
00548         if(loop_start == false && calib_start == true) 
00549         {   LED(1,0,1); 
00550             motor1_aan.write(0); 
00551             motor2_aan.write(0);
00552             calibrate_amp();        // 10 second calibration
00553             calib_start = false;    // turn fork off
00554         }
00555         
00556         // turn red led on
00557         if(loop_start == true && calib_start == true) 
00558         { 
00559         LED(0,1,1); 
00560         motor1_aan.write(0); 
00561         motor2_aan.write(0);
00562         }
00563         
00564         // turn leds off (both buttons false)
00565         else { LED(1,1,1);}
00566     }
00567 }