Revision 1:ba63033da653, committed 2016-10-31

Comitter:

RiP

Date:

Mon Oct 31 12:21:16 2016 +0000

Parent:

0:3c99f1705565

Commit message:

calibration voor emg thresholds toegevoegd en wat variabelen betere namen gegeven

Changed in this revision

--- a/main.cpp	Mon Oct 31 11:25:32 2016 +0000
+++ b/main.cpp	Mon Oct 31 12:21:16 2016 +0000
@@ -18,6 +18,9 @@
 AnalogIn    emg2( A1 );
 AnalogIn    emg3( A2 );
 
+//Button used to calibrate the threshold values
+DigitalIn button(PTC6);
+
 //Define motor outputs
 DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
 FastPWM motor1(D6);     // speed of motor 1
@@ -35,15 +38,21 @@
 Ticker      servo_control;                          // Ticker for calling servo_control
 
 //Initialize all variables
-volatile bool sampletimer = false;          // go flag
-volatile bool controller_go=false;
-volatile bool servo_go=false;
+volatile bool sample_go = false;          // go flag sample()
+volatile bool controller_go=false;          // go flag controller()
+volatile bool servo_go=false;               // go flag servo_controller()
+
 
-double threshold = 0.04;                    // the threshold which the emg signals need to surpass to do something
+double highest_emg1;                        // the highest emg signal of emg input 1
+double highest_emg2;                        // the highest emg signal of emg input 2
+double highest_emg3;                        // the highest emg signal of emg input 3
+double threshold1;                          // the threshold which the first emg signal needs to surpass to do something
+double threshold2;                          // the threshold which the second emg signal needs to surpass to do something
+double threshold3;                          // the threshold which the third emg signal needs to surpass to do something
 double samplefreq=0.002;                    // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
-double emg02;                               // the first emg signal
-double emg12;                               // the second emg signal
-double emg22;                               // the third emg signal
+double emg11;                               // the first emg signal
+double emg21;                               // the second emg signal
+double emg31;                               // the third emg signal
 double ref_x=0.0000;                        // the x reference position
 double ref_y=0.0000;                        // the y reference position
 double old_ref_x;                           // the old x reference
@@ -130,12 +139,12 @@
 
 void sampleflag()
 {
-    if (sampletimer==true) {
+    if (sample_go==true) {
         // this if statement is used to see if the code takes too long before it is called again
         pc.printf("rate too high error in sampleflag\n\r");
     }
     //This sets the go flag for when the function sample needs to be called
-    sampletimer=true;
+    sample_go=true;
 }
 
 void activate_controller()
@@ -163,32 +172,32 @@
     }
     //Read the emg signals and filter it
 
-    emg02=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 0
-    emg12=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 1
-    emg22=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 2
+    emg11=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 1
+    emg21=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 2
+    emg31=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 3
 
     //remember what the reference was
     old_ref_x=ref_x;
     old_ref_y=ref_y;
     //look if the emg signals go over the threshold and change the reference accordingly
-    if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
+    if (emg11>threshold1&&emg21>threshold2&&emg31>threshold3 || key=='d') {
         ref_x=ref_x-speed;
         ref_y=ref_y-speed;
 
-    } else if (emg02>threshold&&emg12>threshold || key == 'a' || key == 'z') {
+    } else if (emg11>threshold1&&emg21>threshold2 || key == 'a' || key == 'z') {
         ref_x=ref_x-speed;
 
-    } else if (emg02>threshold&&emg22>threshold || key == 's') {
+    } else if (emg11>threshold1&&emg31>threshold3 || key == 's') {
         ref_y=ref_y-speed;
 
-    } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
+    } else if (emg21>threshold2&&emg31>threshold3 || key == 'e' ) {
         ref_x=ref_x+speed;
         ref_y=ref_y+speed;
 
-    } else if (emg12>threshold || key == 'q' ) {
+    } else if (emg21>threshold2 || key == 'q' ) {
         ref_x=ref_x+speed;
 
-    } else if (emg22>threshold || key == 'w') {
+    } else if (emg31>threshold3 || key == 'w') {
         ref_y=ref_y+speed;
     }
 
@@ -290,12 +299,17 @@
 
 }
 
+void my_emg()
+{
+    //This function is attached to a ticker so that the highest emg values are printed every second.
+    pc.printf("highest_emg1=%.4f\thighest_emg2=%.4f\thighest_emg3=%.4f\n\r", highest_emg1, highest_emg2, highest_emg3);
+}
+
 
 void my_pos()
 {
-    //This function is attached to a ticker so that the reference position is printed every second.
+    //This function is attached to the same ticker as my_emg so that the reference position is printed every second instead of the highest emg values.
     pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta);
-
 }
 
 int main()
@@ -311,26 +325,48 @@
     bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
     bqc33.add( &bq331);
 
-    motor1.period(0.02f); //period of pwm signal for motor 1
-    motor2.period(0.02f); // period of pwm signal for motor 2
-    motor1dir=0; // setting direction to ccw
-    motor2dir=0; // setting direction to ccw
+    motor1.period(0.02f);           // period of pwm signal for motor 1
+    motor2.period(0.02f);           // period of pwm signal for motor 2
+    motor1dir=0;                    // setting direction to ccw
+    motor2dir=0;                    // setting direction to ccw
+    
+    pos_timer.attach(&my_emg, 1);   // ticker used to print the maximum emg values every second
+    
+    //this while loop is used to determine what the highest possible value of the emg signals are and the threshold values are 1/5 of that.
+    //this is done so that every user can use his own threshold value.
+    while (button==1) {
+        emg11=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 1
+        emg21=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 2
+        emg31=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 3
+        if(emg11>highest_emg1) {
+            highest_emg1=emg11;
+        }
+        if(emg21>highest_emg2) {
+            highest_emg2=emg21;
+        }
+        if(emg31>highest_emg3) {
+            highest_emg3=emg31;
+        }
+        threshold1=0.2*highest_emg1;
+        threshold2=0.2*highest_emg2;
+        threshold3=0.2*highest_emg3;
+    }
 
     //Attach the 'sample' function to the timer 'sample_timer'.
     //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
     sample_timer.attach(&sampleflag, samplefreq);
 
     //Attach the function my_pos to the timer pos_timer.
-    //This ensures that the position is printed every second.
-    pos_timer.attach(&my_pos, 1);
+    //This ensures that the reference position is printed every second.
+    pos_timer.attach(&my_pos, 1); 
     control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
     servo_control.attach(&activate_servo_control,servo_Ts);
 
     while(1) {
         //Only take a sample when the go flag is true.
-        if (sampletimer==true) {
+        if (sample_go==true) {
             sample();
-            sampletimer = false;            //change sampletimer to false if sample() is finished
+            sample_go = false;            //change sample_go to false if sample() is finished
         }
         if(controller_go) { // go flag
             controller();