main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "BiQuad.h"
00004 #include "MODSERIAL.h"
00005 
00006 
00007 //Define objects
00008 //Define the button interrupt for the calibration
00009 InterruptIn button_calibrate(PTA4);
00010 
00011 //Define the EMG inputs
00012 AnalogIn    emg1( A0 );
00013 AnalogIn    emg2( A1 );
00014 AnalogIn    emg3( A2 );
00015 
00016 //Define the Tickers
00017 Ticker      pos_timer;
00018 Ticker      sample_timer;
00019 
00020 HIDScope    scope( 6 );
00021 MODSERIAL pc(USBTX, USBRX);
00022 
00023 //Initialize all variables
00024 volatile bool sampletimer = false;
00025 volatile bool buttonflag = false;
00026 volatile bool newcase = false;
00027 
00028 double threshold = 0.04;
00029 double samplefreq=0.002;
00030 double emg02;
00031 double emg12;
00032 double emg22;
00033 double ref_x=0.0000;
00034 double ref_y=0.0000;
00035 double old_ref_x;
00036 double old_ref_y;
00037 double speed_emg=0.00002;
00038 double speed_key=0.000326;
00039 double speed=0.00002;
00040 double theta=0.0;
00041 double radius=0.0;
00042 
00043 const double minRadius=0.3;                // minimum radius of arm
00044 const double maxRadius=0.6;                // maximum radius of arm
00045 const double minAngle=-1.25;               // minimum angle for limiting controller
00046 
00047 char key;
00048 
00049 
00050 // create a variable called 'mystate', define it
00051 typedef enum { STATE_CALIBRATION, STATE_PAUZE, STATE_X, STATE_X_NEG, STATE_Y, STATE_Y_NEG, STATE_XY, STATE_XY_NEG } states;
00052 states mystate = STATE_PAUZE;
00053 
00054 //Define the needed Biquad chains
00055 BiQuadChain bqc11;
00056 BiQuadChain bqc13;
00057 BiQuadChain bqc21;
00058 BiQuadChain bqc23;
00059 BiQuadChain bqc31;
00060 BiQuadChain bqc33;
00061 
00062 //Define the BiQuads for the filter of the first emg signal
00063 //Notch filter
00064 BiQuad bq111(0.9795,   -1.5849,    0.9795,    1.0000,   -1.5849,    0.9589);
00065 BiQuad bq112(0.9833,   -1.5912,    0.9833,    1.0000,   -1.5793,    0.9787);
00066 BiQuad bq113(0.9957,   -1.6111,    0.9957,    1.0000,   -1.6224,    0.9798);
00067 //High pass filter
00068 //BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
00069 BiQuad bq121( 0.8956,   -1.7911,    0.8956,    1.0000,   -1.7814,    0.7941);
00070 BiQuad bq122( 0.9192,   -1.8385,    0.9192,    1.0000,   -1.8319,    0.8450);
00071 BiQuad bq123( 0.9649,   -1.9298,    0.9649,    1.0000,   -1.9266,    0.9403);
00072 //Low pass filter
00073 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
00074 
00075 //Define the BiQuads for the filter of the second emg signal
00076 //Notch filter
00077 BiQuad bq211 = bq111;
00078 BiQuad bq212 = bq112;
00079 BiQuad bq213 = bq113;
00080 /*  High pass filter*/
00081 BiQuad bq221 = bq121;
00082 BiQuad bq222 = bq122;
00083 BiQuad bq223 = bq123;
00084 /*  Low pass filter*/
00085 BiQuad bq231 = bq131;
00086 
00087 //Define the BiQuads for the filter of the third emg signal
00088 //notch filter
00089 BiQuad bq311 = bq111;
00090 BiQuad bq312 = bq112;
00091 BiQuad bq313 = bq113;
00092 //High pass filter
00093 BiQuad bq321 = bq121;
00094 BiQuad bq323 = bq122;
00095 BiQuad bq322 = bq123;
00096 //low pass filter
00097 BiQuad bq331 = bq131;
00098 
00099 void sampleflag()
00100 {
00101      if (sampletimer==true) {
00102         pc.printf("rate too high error in setgoflag\n\r");
00103     }
00104     //This sets the go flag for when the function sample needs to be called
00105     sampletimer=true;
00106 }
00107 
00108 void calibrate()
00109 {
00110     //This resets the reference signals so that the robot can be calibrated
00111     ref_x=0.0000;
00112     ref_y=0.0000;
00113 }
00114 
00115 void sample(states &mystate)
00116 {
00117     states myoldstate=mystate;
00118 
00119     //This checks if a key is pressed and adjusts the speed to the needed speed
00120     if (pc.readable()==1) {
00121         
00122         key=pc.getc();
00123         //printf("%c\n\r",key);
00124     } 
00125 
00126 
00127     //Read the emg signals and filter it
00128 
00129     scope.set(0, emg1.read());                          //original signal 0
00130     emg02=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 0
00131     scope.set(1, emg02);
00132     scope.set(2, emg2.read());                          //original signal 1
00133     emg12=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 1
00134     scope.set(3, emg12);
00135     scope.set(4, emg3.read());                          //original signal 2
00136     emg22=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 2
00137     scope.set(5, emg22);
00138 
00139 emg02=1;
00140     //Ensure that enough channels are available at the top (HIDScope scope( 6 ))
00141     //Finally, send all channels to the PC at once
00142     scope.send();
00143 
00144     old_ref_x=ref_x;
00145     old_ref_y=ref_y;
00146     //look if the emg signals go over the threshold and change the state to the cooresponding state. Also change the reference.
00147     if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
00148         mystate = STATE_XY_NEG;
00149         ref_x=ref_x-speed;
00150         ref_y=ref_y-speed;
00151 
00152     } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
00153         mystate = STATE_X_NEG;
00154         ref_x=ref_x-speed;
00155 
00156     } else if (emg02>threshold&&emg22>threshold || key == 's') {
00157         mystate = STATE_Y_NEG;
00158         ref_y=ref_y-speed;
00159 
00160     } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
00161         mystate = STATE_XY;
00162         ref_x=ref_x+speed;
00163         ref_y=ref_y+speed;
00164 
00165     } else if (emg12>threshold || key == 'q' ) {
00166         mystate = STATE_X;
00167         ref_x=ref_x+speed;
00168 
00169     } else if (emg22>threshold || key == 'w') {
00170         mystate = STATE_Y;
00171         ref_y=ref_y+speed;
00172     } else {
00173         mystate = STATE_PAUZE;
00174     }
00175 
00176     // convert ref to gearbox angle
00177     theta=atan((ref_y+sin(theta)*minRadius)/(ref_x+cos(theta)*minRadius));
00178     radius=sqrt(pow(ref_x+cos(theta)*minRadius,2)+pow(ref_y+sin(theta)*minRadius,2));
00179     if (theta != theta) {
00180         theta=0;
00181     }
00182     if (theta <= minAngle) {
00183         ref_x=old_ref_x;
00184         ref_y=old_ref_y;
00185         pc.printf("fout 1 ");
00186     } else if (radius < minRadius) {
00187         ref_x=old_ref_x;
00188         ref_y=old_ref_y;
00189         pc.printf("fout 2 ");
00190     } /*else if (theta >= 0 ) {
00191         ref_x=old_ref_x;
00192         ref_y=old_ref_y;
00193         pc.printf("fout 3 ");
00194     }*/ else if ( radius > maxRadius) {
00195         ref_x=old_ref_x;
00196         ref_y=old_ref_y;
00197         pc.printf("fout 4 ");
00198     }
00199 
00200     //change newcase so that the state will only be printed once
00201     if (myoldstate==mystate) {
00202         newcase=false;
00203 
00204     } else {
00205         newcase=true;
00206     }
00207 }
00208 
00209 void my_pos()
00210 {
00211     //This function is attached to a ticker so that the reference position is printed every second.
00212     pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta);
00213 
00214 }
00215 
00216 void print_state()
00217 {
00218     //This code looks in which state the robot is in and prints it to the screen
00219     if (newcase==true) {
00220         switch (mystate) {
00221             case STATE_CALIBRATION : { // calibration
00222                 pc.printf("calibration\n\r");
00223                 break;
00224             }
00225             case STATE_X : // X direction
00226                 pc.printf("X\n\r");
00227                 break;
00228             case STATE_X_NEG : // negative X direction
00229                 pc.printf("Xneg\n\r");
00230                 break;
00231             case STATE_Y : // Y direction
00232                 pc.printf("Y\n\r");
00233                 break;
00234             case STATE_Y_NEG : // negative Y direction
00235                 pc.printf("Yneg\n\r");
00236                 break;
00237             case STATE_XY : // X&Y direction
00238                 pc.printf("XY\n\r");
00239                 break;
00240             case STATE_XY_NEG : // negative X&Y direction
00241                 pc.printf("XYneg\n\r");
00242                 break;
00243             case STATE_PAUZE : // Pauze: do nothing
00244                 pc.printf("PAUZE\n\r");
00245                 break;
00246         }
00247     }
00248 }
00249 
00250 int main()
00251 {
00252     pc.printf("RESET\n\r");
00253     pc.baud(115200);
00254 
00255     //Initialize the Biquad chains
00256     bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
00257     bqc13.add( &bq131);
00258     bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
00259     bqc23.add( &bq231);
00260     bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
00261     bqc33.add( &bq331);
00262 
00263     //Attach the 'sample' function to the timer 'sample_timer'.
00264     //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
00265     sample_timer.attach(&sampleflag, samplefreq);
00266     //Attach the function calibrate to the button interrupt
00267     button_calibrate.fall(&calibrate);
00268     //Attach the function my_pos to the timer pos_timer.
00269     //This ensures that the position is printed every second.
00270     pos_timer.attach(&my_pos, 1);
00271 
00272     while(1) {
00273         //Only take samples when the go flag is true.
00274         if (sampletimer==true) {
00275             sample(mystate);
00276             print_state();
00277             sampletimer = false;
00278         }
00279     }
00280 }