Diff: main.cpp

Revision:

3:58378b78079d

Parent:

2:afa5a01ad84b

Child:

4:b83460036800

diff -r afa5a01ad84b -r 58378b78079d main.cpp
--- a/main.cpp	Tue Nov 01 13:05:39 2016 +0000
+++ b/main.cpp	Tue Nov 01 15:00:40 2016 +0000
@@ -18,9 +18,6 @@
 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
@@ -87,8 +84,8 @@
 const double minTheta=-38.0/180.0*pi;        //minimum angle robot
 const double maxTheta=-24.0/180.0*pi;        // maximum angle to which the spatula can stabilise
 const double diffTheta=maxTheta-minTheta;  //difference between max and min angle of theta for stabilisation
-const double min_servo_pwm=0.00217;   // corresponds to angle of theta -32 degrees
-const double max_servo_pwm=0.0023;    // corresponds to angle of theta -43 degrees
+const double min_servo_pwm=0.0020;   // corresponds to angle of theta -38 degrees
+const double max_servo_pwm=0.0022;    // corresponds to angle of theta -24 degrees
 const double res_servo=max_servo_pwm-min_servo_pwm;  //resolution of servo pwm signal between min and max angle
 const double servo_Ts=0.02;
 bool z_push=false;
@@ -138,11 +135,11 @@
 //low pass filter
 BiQuad bq331 = bq131;
 
-void sampleflag()
+void activate_sample()
 {
     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");
+        pc.printf("rate too high error in activate_sample\n\r");
     }
     //This sets the go flag for when the function sample needs to be called
     sample_go=true;
@@ -173,9 +170,17 @@
     }
     //Read the emg signals and filter it
 
+    //scope.set(0,emg1.read());
     emg11=bqc13.step(fabs(bqc11.step(emg1.read())));    //filtered signal 1
+    //scope.set(1,emg11);
+    
+    //scope.set(2,emg2.read());
     emg21=bqc23.step(fabs(bqc21.step(emg2.read())));    //filtered signal 2
+    //scope.set(3,emg21);
+
+    //scope.set(4,emg3.read());
     emg31=bqc33.step(fabs(bqc31.step(emg3.read())));    //filtered signal 3
+    //scope.set(5,emg31);
 
     //remember what the reference was
     old_ref_x=ref_x;
@@ -202,6 +207,10 @@
         ref_y=ref_y+speed;
     }
 
+    if (key == 'z') {
+        z_push=true;
+    }
+
     if (key != 'z' && z_push) {
         ref_x=0.0;
         ref_y=0.0;
@@ -219,8 +228,6 @@
         if (key != 'z') {
             ref_x=old_ref_x;
             ref_y=old_ref_y;
-        } else if (key == 'z') {
-            z_push=true;
         }
     } else if ( radius > maxRadius) {
         ref_x=old_ref_x;
@@ -230,6 +237,8 @@
     }
     theta=atan(ref_y/(ref_x+minRadius));
     radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
+
+    //scope.send();
 }
 
 double PID( double err, const double Kp, const double Ki, const double Kd,
@@ -279,13 +288,15 @@
     }
 
     //hidsopce to check what the code does exactly
+    
     scope.set(0,ref_angle1-angle1); //error1
     scope.set(1,ref_angle1);
     scope.set(2,m1_pwm);
     scope.set(3,ref_angle2-angle2);
     scope.set(4,ref_angle2);
-    scope.set(5,servo_pwm);
+    scope.set(5,servo_pwm*1000);
     scope.send();
+    
 }
 
 void servo_controller()  // this function is used to stabalize the spatula with the servo
@@ -295,10 +306,10 @@
     } else {
         servo_pwm=max_servo_pwm;
     }
-    if (!servo_button){ // flip burger, spatula to max position
+    if (!servo_button) { // flip burger, spatula to max position
         servo_pwm=0.0014;
-        }
-        
+    }
+
     servo.pulsewidth(servo_pwm);
 
 }
@@ -333,12 +344,12 @@
     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) {
+    while (servo_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
@@ -354,15 +365,16 @@
         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);
+    sample_timer.attach(&activate_sample, samplefreq);
 
     //Attach the function my_pos to the timer pos_timer.
     //This ensures that the reference position is printed every second.
-    pos_timer.attach(&my_pos, 1); 
+    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);