Diff: main.cpp

Revision:

39:cc7754330c26

Parent:

38:e58bab07275e

Child:

40:9ecaab27acde

--- a/main.cpp	Thu Nov 03 10:43:06 2016 +0000
+++ b/main.cpp	Thu Nov 03 15:09:14 2016 +0000
@@ -63,7 +63,7 @@
 float L1 = 0.45;
 float L2 = 0.35;
 float TowerHeight = 0.232;    //height of motor axes above table surface!
-float StampHeight = 0.06;    // height of end effector
+float StampHeight = 0.056;    // height of end effector
 float y_stampup = 0.1;      //height stamp while not stamping: 10cm above table surface
 float y_stampdown = 0.0;    //height stamp while stamping: at table surface
 
@@ -71,18 +71,21 @@
 //set initial conditions
 float biceps_l = 0;
 float biceps_r = 0;
-float ReferencePosition_x = L2;
+float ReferencePosition_x = 0.4;        //L2;
 float ReferencePosition_y = L1 + TowerHeight - StampHeight;
-float ReferencePosition_xnew = 0;
-float ReferencePosition_ynew = 0;
+float ReferencePosition_xnew = 0.4;     //L2;
+float ReferencePosition_ynew = L1 + TowerHeight - StampHeight;
 float Position_x = 0.0;
 float Position_y = 0.0;
 float q1 = 0;
-float q2 = 0;
+float q2 = PI/2;
 float q1_ref = 0;
 float q2_ref = 0;
 float q1start = 0;
-float q2start = PI/2;
+float q12start = PI/2;
+float q1Encoder = 0;
+float q12Encoder = 0;
+float q12Out = 0;
 
 float q1_error_prev = 0;
 float q2_error_prev = 0;
@@ -105,16 +108,18 @@
 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 Motor1ExtraGain = 1.0;
 float MotorMaxSpeed = 0.5;  //define a maximum PWM speed for the motor
-float t_sample = 0.1; //seconds
-const float maxStampDistance = 2.0;
+float t_sample = 0.01; //seconds
+const float maxStampDistance = 0.65;       //0.66;
+const float minStampDistance = 0.39;
 
 float q1_refOutNew = 0;
-float q1_refOutMin = 0;             //Min angle in radians
-float q1_refOutMax = 1.47;          //Max angle in radians
+float q1_refOutMin = 0;             //Physical min angle 0.00 radians + 0.1 rad
+float q1_refOutMax = 1.37;          //Physical max angle 1.47 radians - 0.1 rad
 float q2_refOutNew = 0;
-float q2_refOutMin = 0.81;     //Min angle in radians
-float q2_refOutMax = 2.17;     //Max angle in radians
+float q2_refOutMin = 0.91;     //Physical  min angle 0.81 radians + 0.1 rad
+float q2_refOutMax = 2.07;     //Physical max angle 2.17 radians - 0.1 rad
 float TotalError1= 0;
 float TotalError2= 0;
 float TotalErrorMin= 0;
@@ -144,10 +149,15 @@
     
     //get joint positions q feedback from encoder
     float Encoder1Position = counts1/resolution;         //angular position in radians, encoder axis
-    float Encoder2Position = counts2/resolution;
-
-    q1Out = q1start + Encoder1Position*inverse_gear_ratio;        //angular position in radians, motor axis
-    q2Out = q2start + Encoder2Position*inverse_gear_ratio;
+    float Encoder2Position = -1*counts2/resolution;     //NEGATIVE!
+    
+    q1Encoder = Encoder1Position*inverse_gear_ratio;
+    q12Encoder = Encoder2Position*inverse_gear_ratio;
+    q1Out = q1start + q1Encoder;        //angular position in radians, motor axis
+    q12Out = q12start + q12Encoder;     //encoder 2 gives sum of both angles!
+    q2Out = q12Out - q1Out;             
+    float q1deg = q1Out*360/2/PI;
+    float q2deg = q2Out*360/2/PI;
     
     /*
     //get end effector position feedback with Brockett
@@ -181,7 +191,7 @@
         //arm 1 activated, move left
         //led1 = 1;
         //led2 = 0;
-        ReferencePosition_xnew = ReferencePosition_x - 0.2; //biceps_l;
+        ReferencePosition_xnew = ReferencePosition_x - 0.02; //biceps_l;
         ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
         /*
         PositionError_x = ReferencePosition_x - Position_x;        //Position error in dx,dy
@@ -197,7 +207,7 @@
         //arm 1 activated, move right
         //led1 = 0;
         //led2 = 1;
-        ReferencePosition_xnew = ReferencePosition_x + 0.2; //biceps_r;
+        ReferencePosition_xnew = ReferencePosition_x + 0.02; //biceps_r;
         ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
         /*PositionError_x = ReferencePosition_x - Position_x;        //Position error in dx,dy
         PositionError_y = ReferencePosition_y - Position_y;        //Position error in dx,dy
@@ -211,30 +221,31 @@
     else{
         //led1 = 0;
         //led2 = 0;
-        ReferencePosition_xnew = ReferencePosition_x;
+        //ReferencePosition_xnew = ReferencePosition_x;
         ReferencePosition_ynew = y_stampup;     //ReferencePosition_y;   //
         }
       //  }
     
+    //check position boundaries
+    if (ReferencePosition_xnew > maxStampDistance){
+        ReferencePosition_x = maxStampDistance; // - 0.1;
+        ReferencePosition_y = y_stampup;
+        pc.printf("Target too far! \r\n");
+        }
+    else if (ReferencePosition_xnew < minStampDistance){
+        ReferencePosition_x = minStampDistance; // + 0.1;
+        ReferencePosition_y = y_stampup;
+        pc.printf("Target too close! \r\n");
+        }
+    else {
+        ReferencePosition_x = ReferencePosition_xnew;        
+        ReferencePosition_y = ReferencePosition_ynew;
+        }
+    
     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 (PointVectorArm2 > maxStampDistance){
-        ReferencePosition_x = ReferencePosition_x - 0.1;
-        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;
-        }
-    
     //calculate reference joint angles for the new reference position
     float alpha = atan(PointPositionArm2_y/PointPositionArm2_x);
     float beta = acos((L2*L2-L1*L1-pow(PointVectorArm2,2))/(-2*L1*PointVectorArm2));
@@ -242,19 +253,29 @@
     q2_refOutNew = PI - asin(PointVectorArm2*sin(beta)/L2);
     
     //check angle boundaries
-    if (q1_refOutNew > q1_refOutMin && q1_refOutNew < q1_refOutMax){
-        q1_refOut = q1_refOutNew;
+    if (q1_refOutNew < q1_refOutMin){
+        q1_refOut = q1_refOutMin;
+        pc.printf("\r\n Under q1 angle boundaries\r\n");
+        }
+    else if (q1_refOutNew > q1_refOutMax){
+        q1_refOut = q1_refOutMax;
+        pc.printf("\r\n Above q1 angle boundaries\r\n");
         }
     else {
-        q1_refOut = q1_refOut;
+        q1_refOut = q1_refOutNew;
         }
-    if (q2_refOutNew > q2_refOutMin && q2_refOutNew < q2_refOutMax){
-        q2_refOut = q2_refOutNew;
+        
+    if (q2_refOutNew < q2_refOutMin){
+        q2_refOut = q2_refOutMin;
+        pc.printf("\r\n Under q2 angle boundaries");
+        }
+    else if (q2_refOutNew > q2_refOutMax){
+        q2_refOut = q2_refOutMax;
+        pc.printf("\r\n Above q2 angle boundaries");
         }
     else {
-        q2_refOut = q2_refOut;
-        }
-        
+        q2_refOut = q2_refOutNew;
+        }        
     //update joint angles
     //q1Out = q1Out + q1_dotOut;  //in radians
     //q2Out = q2Out + q2_dotOut;
@@ -268,7 +289,9 @@
     pc.printf("q1ref: %f    ", q1_refOut);
     pc.printf("q2: %f   ", q2Out);
     pc.printf("q2ref: %f    ", q2_refOut);
-    
+    pc.printf("q1deg: %f   ", q1deg);
+    pc.printf("q2deg: %f   \r\n", q2deg);
+        
     /*
     pc.printf("dx:          %f \r\n", dx);
     pc.printf("dy:          %f \r\n", dy);
@@ -293,7 +316,7 @@
     // linear feedback control
     float q1_error = q1_ref - q1; //referencePosition1 - Position1;             // proportional angular error in radians
     float q2_error = q2_ref - q2; //referencePosition1 - Position1;             // proportional angular error in radians
-    float Kp = 6; //potMeter2.read();
+    float Kp = 3; //potMeter2.read();
 
     float q1IntError = q1IntError + q1_error*t_sample;             // integrated error in radians
     float q2IntError = q2IntError + q2_error*t_sample;             // integrated error in radians
@@ -334,7 +357,7 @@
     motorValue2Out = DerTotalError2/MotorGain;       
     */
     
-    motorValue1Out = TotalError1/MotorGain;       
+    motorValue1Out = Motor1ExtraGain*(TotalError1/MotorGain);       
     motorValue2Out = TotalError2/MotorGain;       
     
     /*
@@ -354,9 +377,9 @@
     pc.printf("IE2: %f  ", q2IntError);
     pc.printf("DE2: %f  ", q2DerivativeError);
     */
-    pc.printf("TE1: %f  ", TotalError1);
-    pc.printf("TE2: %f  ", TotalError2);
-    pc.printf("M1: %f   ", motorValue1Out);
+    //pc.printf("TE1: %f  ", TotalError1);
+    //pc.printf("TE2: %f  ", TotalError2);
+    pc.printf("M1: %f   \r\n", motorValue1Out);
     pc.printf("M2: %f   \r\n", motorValue2Out);
       
     q1_error_prev = q1_error;
@@ -382,8 +405,8 @@
         //led1=0;
         //led2=1;
         }
-    if (fabs(motorValue1)>1){
-        motor1MagnitudePin = 1;
+    if (fabs(motorValue1)>MotorMaxSpeed){
+        motor1MagnitudePin = MotorMaxSpeed;
         }
     else{
         motor1MagnitudePin = fabs(motorValue1);  //fabs(motorValue1);
@@ -391,25 +414,25 @@
     
     //control motor 2
     if (motorValue2 >=0)  //clockwise rotation
-        {motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted)
+        {motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted)
         //led1=1;
         //led2=0;
         }
     else    //counterclockwise rotation 
-        {motor2DirectionPin=cw;    //action is ccw, due to faulty motor2DirectionPin (inverted)
+        {motor2DirectionPin=ccw;    //action is cw, due to faulty motor2DirectionPin (inverted)
         //led1=0;
         //led2=1;
         }
-    if (fabs(motorValue2)>1){
-        motor2MagnitudePin = 1;
+    if (fabs(motorValue2)>MotorMaxSpeed){
+        motor2MagnitudePin = MotorMaxSpeed;
         }
     else{
         motor2MagnitudePin = fabs(motorValue2);
         }
-        float ReadMagn1 = motor1MagnitudePin.read();
-        float ReadMagn2 = motor2MagnitudePin.read();
-        //pc.printf("motor1Magnitude: %f \r\n", ReadMagn1);
-        //pc.printf("motor2Magnitude: %f \r\n", ReadMagn2);
+        float ReadDir1 = motor1DirectionPin.read();
+        float ReadDir2 = motor2DirectionPin.read();
+        pc.printf("M1 dir: %f \r\n", ReadDir1);
+        pc.printf("M2 dir: %f \r\n", ReadDir2);
 }
 
 void MeasureAndControl()