Diff: main.cpp

Revision:

28:6d8c6fe79394

Parent:

27:456e8a47f815

Child:

29:caf3dd699617

--- a/main.cpp	Wed Oct 26 20:24:58 2016 +0000
+++ b/main.cpp	Thu Oct 27 10:35:53 2016 +0000
@@ -59,12 +59,13 @@
 float L1 = 1.0;
 float L2 = 1.0;
 float TowerHeight = 0.4; //height of motor axes above table surface!
+float StampHeight = 0.1; // height of end effector
 
 //set initial conditions
 float biceps_l = 0;
 float biceps_r = 0;
 float ReferencePosition_x = L2;
-float ReferencePosition_y = L1 + TowerHeight;
+float ReferencePosition_y = L1 + TowerHeight - StampHeight;
 float ReferencePosition_xnew = 0;
 float ReferencePosition_ynew = 0;
 float Position_x = 0.0;
@@ -145,7 +146,7 @@
     */
     //get end effector position feedback with trigonometry
     Position_x = (L1*sin(q1) + L2*sin(q1+q2));
-    Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight;  
+    Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight;  
     //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2));
       
     //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
@@ -205,12 +206,12 @@
         ReferencePosition_ynew = ReferencePosition_y;
         }
     
-    float PointPosition_x = ReferencePosition_x;
-    float PointPosition_y = ReferencePosition_y - TowerHeight;
-    float PointVector = sqrt(pow(PointPosition_x,2)+pow(PointPosition_y,2));
+    float PointPositionArm2_x = ReferencePosition_x;
+    float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight;
+    float PointVectorArm2 = sqrt(pow(PointPositionArm2_x,2)+pow(PointPositionArm2_y,2));
     
     //check position boundaries
-    if (PointVector > maxStampDistance){
+    if (PointVectorArm2 > maxStampDistance){
         ReferencePosition_x = ReferencePosition_x;
         ReferencePosition_y = ReferencePosition_y;
         }
@@ -223,10 +224,10 @@
         }
     
     //calculate reference joint angles for the new reference position
-    float alpha = atan(PointPosition_y/PointPosition_x);
-    float beta = acos((L2*L2-L1*L1-pow(PointVector,2))/(-2*L1*PointVector));
+    float alpha = atan(PointPositionArm2_y/PointPositionArm2_x);
+    float beta = acos((L2*L2-L1*L1-pow(PointVectorArm2,2))/(-2*L1*PointVectorArm2));
     q1_refOutNew = PI/2 - (alpha+beta);
-    q2_refOutNew = PI - asin(PointVector*sin(beta)/L2);
+    q2_refOutNew = PI - asin(PointVectorArm2*sin(beta)/L2);
     
     //check angle boundaries
     if (q1_refOutNew > q1_refOutMin && q1_refOutNew < q1_refOutMax){