Diff: main.cpp

Revision:

24:393da1cc1fa8

Parent:

23:3a4d5e19c20d

Child:

25:596255732b65

--- a/main.cpp	Wed Oct 26 09:18:42 2016 +0000
+++ b/main.cpp	Wed Oct 26 09:24:38 2016 +0000
@@ -200,6 +200,8 @@
         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));
     
     //check position boundaries
@@ -216,8 +218,6 @@
         }
     
     //calculate reference joint angles for the new reference position
-    float PointPosition_x = ReferencePosition_x;
-    float PointPosition_y = ReferencePosition_y - TowerHeight;
     float alpha = atan(PointPosition_y/PointPosition_x);
     float beta = acos((L2*L2-L1*L1-pow(PointVector,2))/(-2*L1*PointVector));
     q1_refOutNew = PI/2 - (alpha+beta);
@@ -275,7 +275,7 @@
     }
     
 
-void FeedForwardControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){
+void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){
     
     // linear feedback control
     float q1_error = q1_ref - q1; //referencePosition1 - Position1;             // proportional angular error in radians
@@ -383,10 +383,10 @@
 void MeasureAndControl()
 {
     // This function measures the EMG of both arms, calculates via IK what
-    // the joint speeds should be, and controls the motor with 
-    // a Feedforward controller. This is called from a Ticker.
+    // the joint positions should be, and controls the motor with 
+    // a Feedback controller. This is called from a Ticker.
     GetReferenceKinematics1(q1, q2, q1_ref, q2_ref);
-    FeedForwardControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
+    FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2);
     SetMotor1(motorValue1, motorValue2);
 }