Diff: main.cpp

Revision:

3:339b6f68b317

Parent:

2:027d19cda97b

Child:

4:89f7d7f3a7ca

--- a/main.cpp	Mon Oct 24 19:47:03 2016 +0000
+++ b/main.cpp	Tue Oct 25 12:36:58 2016 +0000
@@ -7,17 +7,17 @@
 
 
 #define M_PI 3.141592653589793238462643383279502884L // Pi
-const double R_XTABLE_LO = -30.0;     // Robot in the middle, table width=60
-const double R_XTABLE_HI = 30.0;
-const double R_YTABLE_LO = -18.0;
-const double R_YTABLE_HI = 82.0;
-const double R_XROBOT_LO = -8.0;     // Robot limited from moving onto itself (20x20 base)
-const double R_XROBOT_HI = 8.0;
-const double R_YROBOT_LO = -18.0;
-const double R_YROBOT_HI = 2.0;
+const double R_XTABLE_LEFT = -30.0;     // Robot in the middle, table width=60
+const double R_XTABLE_RIGHT = 30.0;
+const double R_YTABLE_BOTTOM = -18.0;
+const double R_YTABLE_TOP = 82.0;
+const double R_ROBOT_LEFT = -8.0;     // Robot limited from moving onto itself (20x20 base)
+const double R_ROBOT_RIGHT = 8.0;
+const double R_ROBOT_BOTTOM = -18.0;
+const double R_ROBOT_TOP = 2.0;
 
-const double ARM1_ALIMLO = (2*M_PI/360)*125;
-const double ARM1_ALIMHI = -(2*M_PI/360)*125;
+const double ARM1_ALIMLO = (2*M_PI/360)*120;
+const double ARM1_ALIMHI = -(2*M_PI/360)*120;
 const double ARM2_ALIMLO = 0;
 const double ARM2_ALIMHI = (2*M_PI/360)*145;
 
@@ -51,70 +51,96 @@
 r_link arm2;    // Robot lower arm
 r_link end;     // Robot end effector
 
-// robot_applylinmits
-// Input: x,y
-// Output: x,y limited to limits set by constants
-void robot_applylimits (float& x, float& y){
+/*
+** =============================================================================
+** robot_applySetpointLimits
+** =============================================================================
+** Function:
+** Limits the cartesian coordinates to table, length of robot and prevents
+** the robot arm crossing it's own base. 
+** Input:
+** float x, float y: position in cartesian coordinates
+** Output:
+** float x, float y: limited position in cartesian coordinates
+** =============================================================================
+*/
+void robot_applySetpointLimits (float& x, float& y){
     float z;
     float angle;
     
     // Limit x,y to table top size
-    if (x<R_XTABLE_LO) { x=R_XTABLE_LO; vx=0; vy=0;}
-    else if (x>R_XTABLE_HI) { x=R_XTABLE_HI; vx=0; vy=0;}
-    if (y<R_YTABLE_LO) { y=R_YTABLE_LO; vx=0; vy=0;}
-    else if (y>R_YTABLE_HI) { y=R_YTABLE_HI; vx=0; vy=0; }
-
-    // Limit x,y from moving into the robot base
-/*
-    if (x>R_XROBOT_LO && x<R_XROBOT_HI && y>R_YROBOT_LO && y<R_YROBOT_HI) 
-    { 
-        if (x<=0){
-            x=R_XROBOT_LO;
-        }
-        else {
-            x=R_XROBOT_HI;
-        }
-        if (y>R_YROBOT_LO && y<R_YROBOT_HI){
-            y=R_YROBOT_HI;
-        }
-    }
-*/
-    // Limit x,y to maximum size of robot arm
+    if (x<R_XTABLE_LEFT) { x=R_XTABLE_LEFT; vx=0;}
+    else if (x>R_XTABLE_RIGHT) { x=R_XTABLE_RIGHT; vx=0;}
+    if (y<R_YTABLE_BOTTOM) { y=R_YTABLE_BOTTOM; vy=0;}
+    else if (y>R_YTABLE_TOP) { y=R_YTABLE_TOP; vy=0; }
+    
+    // Limit x,y to maximum reach of robot arm
     z = sqrt(pow(x,2)+pow(y,2));
     angle = atan2(y,x);
-//    angle = angle + M_PI;
     if (z>(arm1.length+arm2.length)){
         z = arm1.length+arm2.length;
         x = z*cos(angle);
         y = z*sin(angle);
     }
+    
+    // Prevent x,y from entering robot body
+    if (end.x > R_ROBOT_LEFT && end.x < R_ROBOT_RIGHT && end.y >= R_ROBOT_TOP && y < R_ROBOT_TOP) {
+        y = R_ROBOT_TOP;
+    }
+    if (end.x <= R_ROBOT_LEFT && x > R_ROBOT_LEFT && y < R_ROBOT_TOP) {
+        x = R_ROBOT_LEFT;
+    }
+    if (end.x >= R_ROBOT_RIGHT && x < R_ROBOT_RIGHT && y < R_ROBOT_TOP) {
+        x = R_ROBOT_RIGHT;
+    }
 }
 
-// robot_setposition:
-// Input: x,y cartesian coordinates
-// Function: calculate arm angles based on cartesian coordinates
-void robot_setposition (float x,float y){
-    float z;
-    float z1;
-    float z2;
-
-    float angle;
-    float b1,b2;
-    
-    float old;
+/*
+** =============================================================================
+** robot_setposition
+** =============================================================================
+** Function:
+** Transforms cartesian coordinates into rotadions of the joints of the robot's 
+** arms.
+** Input:
+** float x, float y: position in cartesian coordinates
+** =============================================================================
+*/
+void robot_setSetpoint (float x,float y){
+/*
+       (x,y)
+        |\
+        | \
+        |b2\
+     z2 |   \arm2.length
+        |    \
+  z     |_ _ _\
+        |     /
+        |    /
+     z1 |   /arm1.length
+        |b1/
+        | /
+        |/
         
-    robot_applylimits(x,y);
-//    end.x = x;
-//    end.y = y;
+*/
+    float z;        // length z of vector (x,y)
+    float z1;       // part z1
+    float z2;       // part z2
+
+    float angle;    // angle of vector (x,y), !pointing up (x==0) the angle is defined as 0!
+    float b1,b2;    // angle b1 and b2 (see drawing)
     
-//    printf("xy: %f, %f\n\r",x,y);
-    z = sqrt(pow(x,2)+pow(y,2));
-    angle = atan2(y,x);
+    float old;      // 'old' helper variable used for calculating speeds
+        
+    
+    robot_applySetpointLimits(x,y); // Apply limits
     
-//    printf("angle voor gekloot: %f\n\r", angle);
+    z = sqrt(pow(x,2)+pow(y,2));    // Calculate (x,y) length
+    
+    // Calculate (x,y) angle
+    angle = atan2(y,x);                 // Calculate (x,y) angle using atan 2
+    if (angle<0.5*M_PI && angle>0){     // Rotate result of atan2 90 degrees counter clockwise, so up pointing vector (x==0, y>0) has a zero degree rotation instead of 90 degrees.
 
-// Rotate result of atan2 90 degrees counter clockwise, so up pointing vector has a zero degree rotation instead of 90 degrees.
-    if (angle<0.5*M_PI && angle>0){
         angle = angle - 0.5*M_PI;
     }
     else if (angle<M_PI && angle>0.5*M_PI){
@@ -126,53 +152,59 @@
     else{
         angle = angle + 1.5*M_PI;
     }  
-    if (y==0 && x<0) { angle = 0.5*M_PI; }  // Special case y=0, x < 0 Normally this returns PI, now it  must return PI/2
+    if (y==0 && x<0) { angle = 0.5*M_PI; }  // Special case correction: y=0, x < 0 Normally this returns PI, now it  must return PI/2
+    if (y==0 && x>0) { angle = -0.5*M_PI; }  // Special case correction: y=0, x > 0 Normally this returns -PI, now it  must return -PI/2
 
-    z1 = (pow(arm1.length,2)-pow(arm2.length,2)+pow(z,2))/(2*z);
-    z2 = z-z1;
-    printf("z1 and z2: %f, %f\n\r", z1, z2);
+    z1 = (pow(arm1.length,2)-pow(arm2.length,2)+pow(z,2))/(2*z);    // Calculate z1
+    z2 = z-z1;                                                      // Calculate z2
+    // Roundoff errors sometimes make z1 and z2 slightly higher than the arm lengths, this causes erratic behaviour in the acos function,
+    // so it is countered here by an extra check and fix.
     if (z1>arm1.length) {z1 = arm1.length;}
     else if (z1<-arm1.length) { z1 = -arm1.length;}
     if (z2>arm2.length) {z2 = arm2.length;}
     else if (z2<-arm2.length) { z2 = -arm2.length;}
     
-    b1 = acos(z1/arm1.length);
-    b2 = acos(z2/arm2.length);
+    b1 = acos(z1/arm1.length);      // Calculate angle b1
+    b2 = acos(z2/arm2.length);      // Calculate angle b2
     
-    old = arm1.theta;
-    arm1.theta = angle - b1;
+    // Arm 1 angle and rotational speed
+    old = arm1.theta;               // Preserve old angle arm 1
+    arm1.theta = angle - b1;        // Calculate new angle arm 1
     
-  if (arm1.theta>ARM1_ALIMLO){
-            arm1.theta = ARM1_ALIMLO;
+    if (arm1.theta>ARM1_ALIMLO){    // Limit angle arm1
+        arm1.theta = ARM1_ALIMLO;
     }
     else if (arm1.theta<ARM1_ALIMHI){
-            arm1.theta = ARM1_ALIMHI;
+        arm1.theta = ARM1_ALIMHI;
     }
   
-    arm1.omega = arm1.theta - old;
-    old = arm2.theta;
-    arm2.theta = b1 + b2;
-    if (arm2.theta>ARM2_ALIMHI){
+    arm1.omega = arm1.theta - old;  // Calculate rotational speed arm 1
+    
+    // Arm 2 angle and rotational speed
+    old = arm2.theta;               // Preserve old angle arm 2
+    arm2.theta = b1 + b2;           // Calculate new angle arm 2
+    if (arm2.theta>ARM2_ALIMHI){    // Limit angle arm 2
         arm2.theta = ARM2_ALIMHI;
     } else if (arm2.theta < 0) {
         arm2.theta = 0;
     }
 
-
-    arm2.omega = arm2.theta - old;
-    old = arm2.x;
-    arm2.x = arm1.length*-sin(arm1.theta);
-    arm2.vx = arm2.x - old;
-    old = arm2.y;
-    arm2.y = arm1.length*cos(arm1.theta);
-    arm2.vy = arm2.y - old;
+    arm2.omega = arm2.theta - old;  // Calculate rotational speed arm 2
     
-    old = end.x;
-    end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x;
-    end.vx = end.x - old;
-    old = end.y;
-    end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y;
-    end.vy = old - end.y;
+    // Arm 2 position
+    old = arm2.x;                           // Preserve old x position arm 2
+    arm2.x = arm1.length*-sin(arm1.theta);  // Calculate new x position arm 2
+    arm2.vx = arm2.x - old;                 // Calculate new x speed arm 2
+    old = arm2.y;                           // Preserve old y position arm 2
+    arm2.y = arm1.length*cos(arm1.theta);   // Calculate new y position arm 2
+    arm2.vy = arm2.y - old;                 // Calculate new y speed arm 2
+    
+    old = end.x;                                                // Preserve old x position end effector
+    end.x = arm2.length*(-sin(arm1.theta+arm2.theta)) + arm2.x; // Calculate new x position end effector
+    end.vx = end.x - old;                                       // Calculate new x speed end effector
+    old = end.y;                                                // Preserve old y position end effector
+    end.y = arm2.length*(cos(arm1.theta+arm2.theta)) + arm2.y;  // Calculate new y position end effector
+    end.vy = old - end.y;                                       // Calculate new y speed end effector
 
 }
 
@@ -304,7 +336,7 @@
     robot_init();
     while(true){
         inputswitches();
-        robot_setposition(end.x+vx,end.y+vy);
+        robot_setSetpoint(end.x+vx,end.y+vy);
         printf("Angle1\n\r");
         printf("%f\n\r", arm1.theta);
         printf("Angle2\n\r");