Diff: main.cpp

Revision:

11:5c06c97c3673

Parent:

10:a9e344e440b8

Child:

12:12b72bd60fd1

--- a/main.cpp	Fri Oct 27 08:43:34 2017 +0000
+++ b/main.cpp	Mon Oct 30 12:11:42 2017 +0000
@@ -36,17 +36,18 @@
 const double controller_Ts = 0.01;                                              // Time step for controllerTicker [s]; Should be between 5kHz and 10kHz
 const double motorRatio = 131.25;                                               // Ratio of the gearbox in the motors []
 const double radPerPulse = 2*pi/(32*motorRatio);                                // Amount of radians the motor rotates per encoder pulse [rad/pulse]
-double xVelocity = 0, yVelocity = 0;                                            // X and Y velocities of the end effector
+double xVelocity = 0, yVelocity = 0;                                            // X and Y velocities of the end effector at the start
+double velocity = 0.3;                                                          // X and Y velocity of the end effector when desired
 
 // MOTOR 1
 double position1;                                                               // Position of motor 1 [rad]
-volatile double reference1 = 0;                                                          // Desired rotation of motor 1 [rad]
+double reference1 = 0;                                                          // Desired rotation of motor 1 [rad]
 double motor1Max = 2*pi*45/360;                                                 // Maximum rotation of motor 1 [rad]
 double motor1Min = 0;                                                           // Minimum rotation of motor 1 [rad]
 // Controller gains
 const double motor1_KP = 5;                                                     // Position gain []
-const double motor1_KI = 5;                                                  // Integral gain []
-const double motor1_KD = 0.0;                                                     // Derivative gain []
+const double motor1_KI = 5;                                                     // Integral gain []
+const double motor1_KD = 0.0;                                                   // Derivative gain []
 double motor1_err_int = 0, motor1_prev_err = 0;
 // Derivative filter coefficients
 const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0;                              // Derivative filter coefficients []
@@ -61,8 +62,8 @@
 double motor2Min = 2*pi*-45/360;                                                // Minimum rotation of motor 2 [rad]
 // Controller gains
 const double motor2_KP = 3;                                                     // Position gain []
-const double motor2_KI = 3;                                                  // Integral gain []
-const double motor2_KD = 0.0;                                                     // Derivative gain []
+const double motor2_KI = 3;                                                     // Integral gain []
+const double motor2_KD = 0.0;                                                   // Derivative gain []
 double motor2_err_int = 0, motor2_prev_err = 0;
 // Derivative filter coefficients
 const double motor2_f_a1 = 1.0, motor2_f_a2 = 2.0;                              // Derivative filter coefficients []
@@ -89,12 +90,6 @@
     return y;
 }
 
-// P-CONTROLLER ////////////////////////////////////////////////////////////////
-double P_Controller(double error, const double Kp)
-{
-    return Kp * error;
-}
-
 // PID-CONTROLLER WITH FILTER //////////////////////////////////////////////////
 double PID_Controller(double e, const double Kp, const double Ki,
  const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
@@ -103,14 +98,14 @@
 {
     // Derivative
     double e_der = (e - e_prev)/Ts;                                             // Calculate the derivative of error
-    //pc.printf("der error %f \r\n", e_der);
+    //pc.printf("derivative error %f \r\n", e_der);
     //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);            // Filter the derivative of error
     e_prev = e;
     // Integral
     e_int = e_int + Ts*e;                                                       // Calculate the integral of error
     // PID
-    //pc.printf("NAAA der error %f \r\n", e_der);
-    return -(Kp*e + Ki*e_int + Kd*e_der);
+    //pc.printf("Na derivative error %f \r\n", e_der);
+    return Kp*e + Ki*e_int + Kd*e_der;
 }
 
 // MOTOR 1 /////////////////////////////////////////////////////////////////////
@@ -141,15 +136,6 @@
     else motor2MagnitudePin = 0;
 }
 
-// MOTOR 1 P-CONTROLLER ////////////////////////////////////////////////////////
-void Motor1PController()
-{
-    position1 = radPerPulse * Encoder1.getPulses();                             // Calculate the rotation of motor 1
-    //pc.printf("%f", position1);
-    double motor1Value = P_Controller(reference1 - position1, motor1_KP);
-    RotateMotor1(motor1Value);
-}
-
 // MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
 void Motor1Controller()
 {
@@ -189,13 +175,12 @@
     position1 = radPerPulse * Encoder1.getPulses();
     position2 = radPerPulse * Encoder2.getPulses();
     
-    
-    if(!button1 ) xVelocity = 0.3;//&& position1 > motor1Min && position2 > motor2Min
-    else if(!button2 ) xVelocity = -0.3;//&& position1 < motor1Max && position2 < motor2Max
+    if(!button1) xVelocity = velocity;//&& position1 > motor1Min && position2 > motor2Min
+    else if(!button2) xVelocity = -velocity;//&& position1 < motor1Max && position2 < motor2Max
     else xVelocity = 0;
 
-    if(!button3 ) yVelocity = 0.3;//&& position1 < motor1Max && position2 < motor2Max
-    else if(!button4 ) yVelocity = -0.3;//&& position1 < motor1Max && position2 > motor2Min
+    if(!button3) yVelocity = velocity;//&& position1 < motor1Max && position2 < motor2Max
+    else if(!button4) yVelocity = -velocity;//&& position1 < motor1Max && position2 > motor2Min
     else yVelocity = 0;
     
     //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity);
@@ -284,8 +269,9 @@
         //double emgFiltered = bqc.step(emg.read());                              // Filter the EMG signal
         /*
             If EMG signal 1 --> xVelocity / yVelocity = velocity
-            Else if EMG signal 2 --> xVelocity / yVelocity = velocity
+            Else if EMG signal 2 --> xVelocity / yVelocity = -velocity
             Else xVelocity / yVelocity = 0
+            If both signal 1 and 2 --> Switch
         */
         jointVelocities();
     }
@@ -371,8 +357,7 @@
     
     bqc.add(&bq1).add(&bq2);                                                    // Make BiQuad Chain
     
-    //controllerTicker.attach(&Motor1PController, controller_Ts);               // Ticker to control motor 1 (P)
-    //controllerTicker.attach(&Motor1Controller, controller_Ts);                  // Ticker to control motor 1 (PID)
+    //controllerTicker.attach(&Motor1Controller, controller_Ts);                // Ticker to control motor 1 (PID)
     emgSampleTicker.attach(&EmgSample, emg_Ts);                                 // Ticker to sample EMG
     
     while(true)