Diff: main.cpp

Revision:

16:2cf8c2705936

Parent:

15:5d24f832bb7b

Child:

17:f8dd5b8e4b52

--- a/main.cpp	Thu Nov 02 13:05:59 2017 +0000
+++ b/main.cpp	Thu Nov 02 13:47:48 2017 +0000
@@ -46,13 +46,13 @@
 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]
 volatile double xVelocity = 0, yVelocity = 0;                                   // X and Y velocities of the end effector at the start
-double velocity = 0.01;                                                         // X and Y velocity of the end effector when desired
+const double velocity = 0.03;                                                   // X and Y velocity of the end effector when desired
  
 // MOTOR 1
 volatile double position1;                                                      // Position of motor 1 [rad]
 volatile double reference1 = 0;                                                 // Desired rotation of motor 1 [rad]
-double motor1Max = 0;                                                           // Maximum rotation of motor 1 [rad]
-double motor1Min = 2*pi*-40/360;                                                // Minimum rotation of motor 1 [rad]
+const double motor1Max = 0;                                                     // Maximum rotation of motor 1 [rad]
+const double motor1Min = 2*pi*-40/360;                                          // Minimum rotation of motor 1 [rad]
 // Controller gains
 const double motor1_KP = 13;                                                    // Position gain []
 const double motor1_KI = 7;                                                     // Integral gain []
@@ -67,11 +67,9 @@
 // MOTOR 2
 volatile double position2;                                                      // Position of motor 2 [rad]
 volatile double reference2 = 0;                                                 // Desired rotation of motor 2 [rad]
-double motor2Max = 2*pi*25/360;                                                 // Maximum rotation of motor 2 [rad]
-double motor2Min = 2*pi*-28/360;                                                // Minimum rotation of motor 2 [rad]
+const double motor2Max = 2*pi*25/360;                                           // Maximum rotation of motor 2 [rad]
+const double motor2Min = 2*pi*-28/360;                                          // Minimum rotation of motor 2 [rad]
 // Controller gains
-//const double motor2_KP = potmeter1*10;                                        // Position gain []
-//const double motor2_KI = potmeter2*20;                                        // Integral gain []
 const double motor2_KP = 13;                                                    // Position gain []
 const double motor2_KI = 5;                                                     // Integral gain []
 const double motor2_KD = 0.1;                                                   // Derivative gain []
@@ -85,7 +83,7 @@
 // EMG //////////////////////////////////////////////////////////////////
 Ticker emgLeft;
 Ticker emgRight;
-double emgTs = 0.5;
+const double emgTs = 0.5;
 // Filters
 BiQuadChain bqc;
 BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004);
@@ -113,6 +111,7 @@
 
 volatile bool xdir = true, ydir = false;
 volatile bool timer = false;
+volatile int count = 0;
  
 // FUNCTIONS ///////////////////////////////////////////////////////////////////
 // BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
@@ -358,14 +357,31 @@
         position1 = radPerPulse * Encoder1.getPulses();
         position2 = radPerPulse * Encoder2.getPulses();
         
+        if(active_l && active_r)
+            {
+                count += 1;
+                if(count == 40)
+                {
+                    active_l = false;
+                    active_r = false;
+                    xdir = !xdir;
+                    ydir = !ydir;
+                    led4 = !led4;
+                    led5 = !led5;
+                    xVelocity = 0;
+                    yVelocity = 0;
+                }
+            }
+            else count = 0;
+        
         if(xdir)
         {
-            if(active_r && reference1 > motor1Min && reference2 < motor2Max) 
+            if(active_r && count == 0 && reference1 > motor1Min && reference2 < motor2Max) 
             {
                 xVelocity = velocity;
                 pc.printf("x positive \r\n");
             }
-            else if(active_l && reference1 < motor1Max && reference2 > motor2Min)
+            else if(active_l && count == 0 && reference1 < motor1Max && reference2 > motor2Min)
             {
                 xVelocity = -velocity;
                 pc.printf("x negative \r\n");
@@ -374,12 +390,12 @@
         }
         else if(ydir)
         {
-            if(active_r && reference2 < motor2Max )//&& reference1 > motor2Min)
+            if(active_r && count == 0 && reference2 < motor2Max )//&& reference1 > motor2Min)
             {
                 yVelocity = velocity;
                 pc.printf("y positive \r\n");
             }
-            else if(active_l && reference2 > motor2Min )//&& reference1 > motor2Min)
+            else if(active_l && count == 0 && reference2 > motor2Min )//&& reference1 > motor2Min)
             {
                 yVelocity = -velocity;
                 pc.printf("y negative \r\n");
@@ -470,13 +486,6 @@
         
     }
 }
-
-void StartTimer()
-{
-    timer = true;
-    wait(3);
-    timer = false;
-}
  
 // STATES //////////////////////////////////////////////////////////////////////
 void ProcessStateMachine()
@@ -539,21 +548,7 @@
                 stateChanged = false;
             }
             
-            if(active_l && active_r)
-            {
-                if(!timer)
-                {
-                    active_l = false;
-                    active_r = false;
-                    xdir = !xdir;
-                    ydir = !ydir;
-                    led4 = !led4;
-                    led5 = !led5;
-                    if(active_l)pc.printf("links active");
-                    else pc.printf("links inactive");
-                    StartTimer();
-                }
-            }
+            
             
             // Hit command    
             /*if(!button2)