Diff: main.cpp

Revision:

2:afa5a01ad84b

Parent:

1:ba63033da653

Child:

3:58378b78079d

--- a/main.cpp	Mon Oct 31 12:21:16 2016 +0000
+++ b/main.cpp	Tue Nov 01 13:05:39 2016 +0000
@@ -27,6 +27,7 @@
 FastPWM motor2(D5);     //speed of motor 2
 DigitalOut motor2dir(D4);   //direction of motor 2, attach at m2, set to 0: ccw
 FastPWM servo(D9);   //servo pwm
+DigitalIn servo_button(PTC12); // servo flip
 
 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
 QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
@@ -73,7 +74,7 @@
 double m1_v1 = 0, m1_v2 = 0; // Memory variables
 const double m1_Ts = 0.01; // Controller sample time
 
-const double m2_Kp = 9.974, m2_Ki = 16.49, m2_Kd = 1.341, m2_N = 100; // controller constants motor 2
+const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100; // controller constants motor 2
 double m2_v1 = 0, m2_v2 = 0; // Memory variables
 const double m2_Ts = 0.01; // Controller sample time
 
@@ -83,8 +84,8 @@
 const double pulleyRadius=0.0398/2.0;        // pulley radius
 
 double servo_pwm=0.0023; // duty cycle 1.5 ms 7.5%, min 0.5 ms 2.5%, max 2.5 ms 12.5%
-const double minTheta=-43.0/180.0*pi;        //minimum angle robot
-const double maxTheta=-32.0/180.0*pi;        // maximum angle to which the spatula can stabilise
+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
@@ -278,7 +279,7 @@
     }
 
     //hidsopce to check what the code does exactly
-    scope.set(0,ref_angle1-angle1); //error
+    scope.set(0,ref_angle1-angle1); //error1
     scope.set(1,ref_angle1);
     scope.set(2,m1_pwm);
     scope.set(3,ref_angle2-angle2);
@@ -287,14 +288,17 @@
     scope.send();
 }
 
-void servo_controller()
+void servo_controller()  // this function is used to stabalize the spatula with the servo
 {
-    if (theta < maxTheta ) {
+    if (theta < maxTheta ) { //servo can stabilize until a maximum theta
         servo_pwm=min_servo_pwm+(theta-minTheta)/diffTheta*res_servo;
     } else {
         servo_pwm=max_servo_pwm;
     }
-
+    if (!servo_button){ // flip burger, spatula to max position
+        servo_pwm=0.0014;
+        }
+        
     servo.pulsewidth(servo_pwm);
 
 }
@@ -309,7 +313,7 @@
 void my_pos()
 {
     //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);
+    pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta/pi*180.0);
 }
 
 int main()