Diff: main.cpp

Revision:

23:3a4d5e19c20d

Parent:

22:c679f753a8bd

Child:

24:393da1cc1fa8

--- a/main.cpp	Wed Oct 26 09:03:39 2016 +0000
+++ b/main.cpp	Wed Oct 26 09:18:42 2016 +0000
@@ -16,7 +16,7 @@
 - Set angle and length boundaries!!
 - Set robot constants (lengths etc.)
 - Set EMGgain and thresholds
-- Add tower height to ReferencePosition_y and Position_y
+- Add tower height to ReferencePosition_y and Position_y AND inverse kinematics calculation!
 - Add (lower) boundaries to TotalErrors
 - MotorGain could change due to arm weight!!
 - Arms should be placed manually into reference position.
@@ -45,29 +45,31 @@
 
 //set constant or variable values
 float EMGgain = 1.0;
-double DerivativeCounts;
 float x0 = 1.0;
 float L0 = 1.0;
 float L1 = 1.0;
 float L2 = 1.0;
+float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
+float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
+float TowerHeight = 0.4; //height of motor axes above table surface!
+float t_sample = 0.01; //seconds
+const float maxStampDistance = 1.5;
+
 float dx;
 float dy;
-float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp
-float MotorGain = 8.4; // rad/s for PWM, is max motor speed (motor value of 1)
-float t_sample = 0.01; //seconds
+double DerivativeCounts;
 //float referenceVelocity = 0;
 //float bqcDerivativeCounts = 0;
 const float PI = 3.141592653589793;
 const int cw = 0;       //values for cw and ccw are inverted!! cw=0 and ccw=1
 const int ccw = 1;
-const float maxStampDistance = 1.5;
 
 //set initial conditions
 float biceps_l = 0;
 float biceps_r = 0;
 
 float ReferencePosition_x = L2;
-float ReferencePosition_y = L1;
+float ReferencePosition_y = L1 + TowerHeight;
 float ReferencePosition_xnew = 0;
 float ReferencePosition_ynew = 0;
 float Position_x = 0.0;
@@ -138,7 +140,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));  
+    Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight;  
     //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2));
       
     //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG  
@@ -198,10 +200,10 @@
         ReferencePosition_ynew = ReferencePosition_y;
         }
     
-    float ReferenceVector = sqrt(pow(ReferencePosition_x,2)+pow(ReferencePosition_y,2));
+    float PointVector = sqrt(pow(PointPosition_x,2)+pow(PointPosition_y,2));
     
     //check position boundaries
-    if (ReferenceVector > maxStampDistance){
+    if (PointVector > maxStampDistance){
         ReferencePosition_x = ReferencePosition_x;
         ReferencePosition_y = ReferencePosition_y;
         }
@@ -214,10 +216,12 @@
         }
     
     //calculate reference joint angles for the new reference position
-    float alpha = atan(ReferencePosition_y/ReferencePosition_x);
-    float beta = acos((L2*L2-L1*L1-pow(ReferenceVector,2))/(-2*L1*ReferenceVector));
+    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);
-    q2_refOutNew = PI - asin(ReferenceVector*sin(beta)/L2);
+    q2_refOutNew = PI - asin(PointVector*sin(beta)/L2);
     
     //check angle boundaries
     if (q1_refOutNew > q1_refOutMin && q1_refOutMax < q1_refOutMax){
@@ -272,8 +276,6 @@
     
 
 void FeedForwardControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){
-    //float Encoder1Position = counts1/resolution;         //position in radians, encoder axis
-    //float Position1 = Encoder1Position*inverse_gear_ratio;        //position in radians, motor axis
     
     // linear feedback control
     float q1_error = q1_ref - q1; //referencePosition1 - Position1;             // proportional angular error in radians
@@ -295,8 +297,8 @@
     //scope.set(2,Ki);
     //scope.send();
     
-    TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd;         //total controller output = motor input
-    TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd;         //total controller output = motor input
+    TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd;         //total controller output in radians = motor input
+    TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd;         //total controller output in radians = motor input
     
     if (TotalError1 < TotalErrorMin) {
         TotalError1=0;