Diff: main.cpp

Revision:

38:e58bab07275e

Parent:

36:72f0913c5460

Child:

39:cc7754330c26

--- a/main.cpp	Thu Nov 03 09:41:09 2016 +0000
+++ b/main.cpp	Thu Nov 03 10:43:06 2016 +0000
@@ -105,15 +105,16 @@
 float EMGgain = 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 MotorMaxSpeed = 0.5;  //define a maximum PWM speed for the motor
 float t_sample = 0.1; //seconds
 const float maxStampDistance = 2.0;
 
 float q1_refOutNew = 0;
-float q1_refOutMin = 0;         //WRONG values
-float q1_refOutMax = 0.4722*PI;      //WRONG values
+float q1_refOutMin = 0;             //Min angle in radians
+float q1_refOutMax = 1.47;          //Max angle in radians
 float q2_refOutNew = 0;
-float q2_refOutMin = 0.2611*PI;         //WRONG values
-float q2_refOutMax = 0.6667*PI;        //WRONG values
+float q2_refOutMin = 0.81;     //Min angle in radians
+float q2_refOutMax = 2.17;     //Max angle in radians
 float TotalError1= 0;
 float TotalError2= 0;
 float TotalErrorMin= 0;
@@ -225,6 +226,10 @@
         ReferencePosition_y = y_stampup;
         pc.printf("Target too far! \r\n");
         }
+    else if {PointVectorArm2 < minStampDistance){
+        ReferencePosition_x = ReferencePosition_x + 0.1;
+        ReferencePosition_y = y_stampup;
+        pc.printf("Target too far! \r\n");
     else {
         ReferencePosition_x = ReferencePosition_xnew;        
         ReferencePosition_y = ReferencePosition_ynew;