Diff: main.cpp

Revision:

1:dcc0ad8f6477

Parent:

0:077896c03576

Child:

2:e9e3ff715ef7

--- a/main.cpp	Tue Oct 31 12:52:29 2017 +0000
+++ b/main.cpp	Wed Nov 01 09:34:50 2017 +0000
@@ -16,7 +16,7 @@
 
 DigitalOut  motor1DirectionPin(D4);
 PwmOut      motor1MagnitudePin(D5);
-DigitalOut  motor2DirectionPin(D7); // Sequence? Lines 181-183
+DigitalOut  motor2DirectionPin(D7);
 PwmOut      motor2MagnitudePin(D6);
 
 Ticker      measureTick;
@@ -27,13 +27,13 @@
 bool         switch1 = 1; // manual switch for when to start calculations (later removed for a state machine
 bool         direction1 = 1; // direction positive, 0 is negative
 bool         direction2 = 1;
-const double RAD_PER_PULSE = 0.002991; // Value for RAD_PER_PULSE given through the slides (wrong?)
+const double RAD_PER_PULSE = (2 * M_Pi) / 8400; // Value for RAD_PER_PULSE given through the slides (wrong?)
 const double DEG_PER_RAD = 180 / M_Pi; // Basic knowledge of how many degrees are in 1 radian.
 
 double       q1 = 0;                // Angle of arm 1 (upper) in starting position is 0 degrees
 double       q2 = 179/DEG_PER_RAD;  // Angle of arm 2 (lower) in starting position is 180 degrees (but can't be 0 or 180 because of determinant = 0)
-int          L1 = 47;               // Length of arm 1 (upper) in cm
-int          L2 = 29;               // Length of arm 2 (lower) in cm
+int          L1 = 29;               // Length of arm 1 (upper) in cm
+int          L2 = 47;               // Length of arm 2 (lower) in cm
 double       xdes = L1-L2;           // Desired x coordinate, arm is located at x = L1-L2 in starting position
 double       ydes = 0;              // Disired y coordinate, arm is located at y = 0 in starting position
 double       MotorValue1 = 0;
@@ -57,22 +57,22 @@
 void Counter(double &des, double dir, double sig){
     if (sig == 0){
         if (dir == 1)
-            des = des + 0.1;
+            des = des + 0.05;
         else if (dir == 0)
-            des = des - 0.1;
+            des = des - 0.05;
     }
 }
 
 //Kinematic model
-void Kinematic_referencer( double &xdes, double &ydes, double &q1, double &q2)
+void Kinematic_referencer( double &xdes, double &ydes, double &q1, double &q2, double a1, double a2)
     {
     double qt = q1 + q2;                               // current total angle
-    double xcurrent = L1 * cos (q1) + L2 * cos (qt);   // current x position
-    double ycurrent = L1 * sin (q1) + L2 * sin (qt);   // current y position
+    double xcurrent = L1 * cos (a1) + L2 * cos (a1+a2);   // current x position
+    double ycurrent = L1 * sin (a1) + L2 * sin (a1+a2);   // current y position
     
     //Initial twist
-    double vx = (xdes - xcurrent)/1;  // Running on 100 Hz
-    double vy = (ydes - ycurrent)/1;
+    double vx = (xdes - xcurrent)/0.1;  // Running on 100 Hz
+    double vy = (ydes - ycurrent)/0.1;
     
     //Jacobians
     double J11 = -ycurrent;
@@ -81,7 +81,7 @@
     double J22 = L2 * cos (qt);
     double Determinant = J11 * J22 - J21 * J12;   // calculate determinant
     
-    pc.printf("D = %.3f \r\n", Determinant);
+    //pc.printf("D = %.3f \r\n", Determinant);
  
     //Calculate angular velocities
     double q1der = (J22 * vx - J12 * vy) / Determinant;
@@ -102,7 +102,7 @@
     // New y may not be negative, this means the arm is located in on the plate
     // New q1 may not be less than -45 degrees, less means the arm will crash into the base plate
     // New q2 may not be more than 185 degrees, more means the lower arm will crash into the upper arm
-    if (ynew > -5 && q1new > -45 / DEG_PER_RAD && q2new < 185 / DEG_PER_RAD )//&& Determinant < 0.01)
+    if (ynew > -20 && q1new > -45 / DEG_PER_RAD && q2new < 200 / DEG_PER_RAD && (pow(xnew, 2.0) + pow(ynew,2.0)) > pow(17.0,2.0) )//&& Determinant < 0.01)
     {
         // If desired, change the angles
         q1 = q1new;
@@ -150,7 +150,7 @@
     Counter(xdes, direction1, button1.read());
     Counter(ydes, direction2, button2.read()); 
     
-    Kinematic_referencer(xdes, ydes, q1, q2);
+    Kinematic_referencer(xdes, ydes, q1, q2, Angle1, Angle2);
     
     //pc.printf("%.2f %.2f \r\n", xdes, ydes); 
     
@@ -171,8 +171,8 @@
     else
         motor1DirectionPin = 0; // if not, counterclockwise
         
-    if(fabs(motor1Value) > 0.5 ) // Next, check the absolute motor value, which is the magnitude
-        motor1MagnitudePin = 0.5; // This is a safety. We never want to exceed 1
+    if(fabs(motor1Value) > 1.0 ) // Next, check the absolute motor value, which is the magnitude
+        motor1MagnitudePin = 1.0; // This is a safety. We never want to exceed 1
     else 
         motor1MagnitudePin = fabs(motor1Value); // if we fall underneath the safety, take the magnitude
 }
@@ -184,8 +184,8 @@
     else
         motor2DirectionPin = 1; // if not, counterclockwise
         
-    if(fabs(motor2Value) > 0.5 ) // Next, check the absolute motor value, which is the magnitude
-        motor2MagnitudePin = 0.5; // This is a safety. We never want to exceed 1
+    if(fabs(motor2Value) > 1.0 ) // Next, check the absolute motor value, which is the magnitude
+        motor2MagnitudePin = 1.0; // This is a safety. We never want to exceed 1
     else 
         motor2MagnitudePin = fabs(motor2Value); // if we fall underneath the safety, take the magnitude
 }