Diff: main.cpp

Revision:

34:96bcbddc7d2d

Parent:

33:b09608fa69e9

Child:

35:a21c1cab8086

--- a/main.cpp	Tue Nov 01 12:35:16 2016 +0000
+++ b/main.cpp	Tue Nov 01 13:45:31 2016 +0000
@@ -62,8 +62,11 @@
 float L0 = 1.0;
 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
+float TowerHeight = 0.4;    //height of motor axes above table surface!
+float StampHeight = 0.1;    // 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
+
 
 //set initial conditions
 float biceps_l = 0;
@@ -102,15 +105,15 @@
 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 t_sample = 0.01; //seconds
-const float maxStampDistance = 1.5;
+float t_sample = 0.1; //seconds
+const float maxStampDistance = 2.0;
 
 float q1_refOutNew = 0;
-float q1_refOutMin = 0;         //WRONG values
-float q1_refOutMax = PI;        //WRONG values
+float q1_refOutMin = -100;         //WRONG values
+float q1_refOutMax = 100;        //WRONG values
 float q2_refOutNew = 0;
-float q2_refOutMin = 0;         //WRONG values
-float q2_refOutMax = PI;        //WRONG values
+float q2_refOutMin = -100;         //WRONG values
+float q2_refOutMax = 100;        //WRONG values
 float TotalError1= 0;
 float TotalError2= 0;
 float TotalErrorMin= 0;
@@ -170,7 +173,7 @@
         //led1 = 1;
         //led2 = 1;
         ReferencePosition_xnew = ReferencePosition_x;
-        ReferencePosition_ynew = ReferencePosition_y - dy_stampdown;   //into stamping vertical position ~the stamp down action
+        ReferencePosition_ynew = y_stampdown; //ReferencePosition_y - dy_stampdown;   //into stamping vertical position ~the stamp down action
         
         }
     else if (biceps_l > 0 && biceps_r <= 0){
@@ -178,7 +181,7 @@
         //led1 = 1;
         //led2 = 0;
         ReferencePosition_xnew = ReferencePosition_x - 0.2; //biceps_l;
-        ReferencePosition_ynew = ReferencePosition_y;
+        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
@@ -194,7 +197,7 @@
         //led1 = 0;
         //led2 = 1;
         ReferencePosition_xnew = ReferencePosition_x + 0.2; //biceps_r;
-        ReferencePosition_ynew = ReferencePosition_y;
+        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
         
@@ -208,7 +211,7 @@
         //led1 = 0;
         //led2 = 0;
         ReferencePosition_xnew = ReferencePosition_x;
-        ReferencePosition_ynew = ReferencePosition_y;
+        ReferencePosition_ynew = ReferencePosition_y;   //y_stampup; 
         }
       //  }
     
@@ -218,11 +221,9 @@
     
     //check position boundaries
     if (PointVectorArm2 > maxStampDistance){
-        ReferencePosition_x = ReferencePosition_x;
-        ReferencePosition_y = ReferencePosition_y;
-        }
-    else if (ReferencePosition_ynew < 0){
-        ReferencePosition_y = 0;            //could also be little negative value to get more pressure on table
+        ReferencePosition_x = ReferencePosition_x - 0.1;
+        ReferencePosition_y = y_stampup;
+        pc.printf("Target too far! \r\n");
         }
     else {
         ReferencePosition_x = ReferencePosition_xnew;        
@@ -249,16 +250,17 @@
         q2_refOut = q2_refOut;
         }
         
-    
     //update joint angles
     //q1Out = q1Out + q1_dotOut;  //in radians
     //q2Out = q2Out + q2_dotOut;
     
-    //pc.baud(115200);
-    //pc.printf("q1: %f   ", q1Out);
-    //pc.printf("q1ref: %f    ", q1_refOut);
-    //pc.printf("q2: %f   ", q2Out);
-    //pc.printf("q2ref: %f    ", q2_refOut);
+    pc.baud(115200);
+    pc.printf("refX: %f ",ReferencePosition_xnew);
+    pc.printf("refY: %f ",ReferencePosition_ynew);
+    pc.printf("q1: %f   ", q1Out);
+    pc.printf("q1ref: %f    ", q1_refOut);
+    pc.printf("q2: %f   ", q2Out);
+    pc.printf("q2ref: %f    ", q2_refOut);
     
     /*
     pc.printf("dx:          %f \r\n", dx);
@@ -289,7 +291,7 @@
     float q1IntError = q1IntError + q1_error*t_sample;             // integrated error in radians
     float q2IntError = q2IntError + q2_error*t_sample;             // integrated error in radians
     //float maxKi = 0.2;
-    float Ki = 0.04;  //0.01*Kp; //potMeter2.read();
+    float Ki = 0.04*Kp;  //0.01*Kp; //potMeter2.read();
     
     float q1DerivativeError = (q1_error - q1_error_prev)/t_sample;  // derivative of error in radians
     float q2DerivativeError = (q2_error_prev + q2_error)/t_sample;  // derivative of error in radians
@@ -344,12 +346,12 @@
     pc.printf("E2: %f  ", q2_error);
     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("M2: %f   \r\n", motorValue2Out);
-    */  
+      
     q1_error_prev = q1_error;
     q2_error_prev = q2_error;
     TotalError1_prev = TotalError1;
@@ -436,9 +438,9 @@
  //Initialize
  //int led1val = led1.read();
  //int led2val = led2.read();
- //pc.baud(115200);
- //pc.printf("Test putty IK");
- MeasureTicker.attach(&MeasureTicker_act, 0.01f); 
+ pc.baud(115200);
+ pc.printf("Test putty IK");
+ MeasureTicker.attach(&MeasureTicker_act, 0.1f); 
  bqc.add(&bq1).add(&bq2);
  
  while(1)