Diff: main.cpp

Revision:

53:75076f9705dc

Parent:

52:17b3aeacb643

Child:

54:31957e8d6a73

--- a/main.cpp	Thu Nov 01 21:17:00 2018 +0000
+++ b/main.cpp	Thu Nov 01 21:35:38 2018 +0000
@@ -129,8 +129,8 @@
 const double xbase = 450-lb;                                    //Length between the motors
 
 //General parameters
-double theta1 = PI/2.0;                                        //Angle of the left motor
-double theta4 = PI/2.0;                                        //Angle of the right motor
+double theta1 = PI*0.40;                                        //Angle of the left motor
+double theta4 = PI*0.40;                                        //Angle of the right motor
 double thetaflip = 0;                                           //Angle of the flipping motor
 double prefx;                                                   //Desired x velocity
 double prefy;                                                   //Desired y velocity                                                        "
@@ -194,6 +194,18 @@
     ledBlue =! ledBlue;                 // toggle LED
 }
 
+float   angleCurrent(float counts)      // Calculates the current angle of the motor (between -2*PI to 2*PI) based on the counts from the encoder
+{
+    float   angle = ((float)counts*2.0f*PI)/countsRad;
+    while   (angle > 2.0f*PI) {
+        angle = angle-2.0f*PI;
+    }
+    while   (angle < -2.0f*PI) {
+        angle = angle+2.0f*PI;
+    }
+    return angle;
+}
+
 //  ============================= EMG FUNCTIONS ===============================
 
 //Function to read and filter the EMG
@@ -344,9 +356,9 @@
 
     const float dq = 0.001;      //benadering van 'delta' hoek
 
-    kinematicsForward(xend1,yend1,theta1,theta4);
-    kinematicsForward(xend2,yend2,theta1+dq,theta4);
-    kinematicsForward(xend3,yend3,theta1,theta4+dq);
+    kinematicsForward(xend1,yend1,angleCurrent(countsL),angleCurrent(countsR));
+    kinematicsForward(xend2,yend2,angleCurrent(countsL)+dq,angleCurrent(countsR));
+    kinematicsForward(xend3,yend3,angleCurrent(countsL),angleCurrent(countsR)+dq);
 
     float a,b,c,d;
     a = xend2-xend1;
@@ -396,18 +408,6 @@
     return  countsF;
 }
 
-float   angleCurrent(float counts)      // Calculates the current angle of the motor (between -2*PI to 2*PI) based on the counts from the encoder
-{
-    float   angle = ((float)counts*2.0f*PI)/countsRad;
-    while   (angle > 2.0f*PI) {
-        angle = angle-2.0f*PI;
-    }
-    while   (angle < -2.0f*PI) {
-        angle = angle+2.0f*PI;
-    }
-    return angle;
-}
-
 float  errorCalc(float angleReference,float angleCurrent)     // Calculates the error of the system, based on the current angle and the reference value
 {
     float   error = angleReference - angleCurrent;