Diff: main.cpp

Revision:

36:72f0913c5460

Parent:

34:96bcbddc7d2d

Child:

37:1360978f49ba

Child:

38:e58bab07275e

--- a/main.cpp	Tue Nov 01 13:45:31 2016 +0000
+++ b/main.cpp	Thu Nov 03 09:41:09 2016 +0000
@@ -58,12 +58,12 @@
 const int ccw = 1;
 
 //set lengths (VALUES HAVE TO BE CHANGED)
-float x0 = 1.0;
-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 x0 = 1.0;
+float L0 = 0.232;
+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 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
 
@@ -109,11 +109,11 @@
 const float maxStampDistance = 2.0;
 
 float q1_refOutNew = 0;
-float q1_refOutMin = -100;         //WRONG values
-float q1_refOutMax = 100;        //WRONG values
+float q1_refOutMin = 0;         //WRONG values
+float q1_refOutMax = 0.4722*PI;      //WRONG values
 float q2_refOutNew = 0;
-float q2_refOutMin = -100;         //WRONG values
-float q2_refOutMax = 100;        //WRONG values
+float q2_refOutMin = 0.2611*PI;         //WRONG values
+float q2_refOutMax = 0.6667*PI;        //WRONG values
 float TotalError1= 0;
 float TotalError2= 0;
 float TotalErrorMin= 0;
@@ -211,7 +211,7 @@
         //led1 = 0;
         //led2 = 0;
         ReferencePosition_xnew = ReferencePosition_x;
-        ReferencePosition_ynew = ReferencePosition_y;   //y_stampup; 
+        ReferencePosition_ynew = y_stampup;     //ReferencePosition_y;   //
         }
       //  }
     
@@ -255,6 +255,8 @@
     //q2Out = q2Out + q2_dotOut;
     
     pc.baud(115200);
+    pc.printf("posX: %f ",Position_x);
+    pc.printf("posY: %f ",Position_y);
     pc.printf("refX: %f ",ReferencePosition_xnew);
     pc.printf("refY: %f ",ReferencePosition_ynew);
     pc.printf("q1: %f   ", q1Out);
@@ -286,7 +288,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 = 3; //potMeter2.read();
+    float Kp = 6; //potMeter2.read();
 
     float q1IntError = q1IntError + q1_error*t_sample;             // integrated error in radians
     float q2IntError = q2IntError + q2_error*t_sample;             // integrated error in radians
@@ -440,6 +442,9 @@
  //int led2val = led2.read();
  pc.baud(115200);
  pc.printf("Test putty IK");
+            counts1 = Encoder1.getPulses();           // gives position of encoder 
+            counts2 = Encoder2.getPulses();           // gives position of encoder 
+ wait(10.0);
  MeasureTicker.attach(&MeasureTicker_act, 0.1f); 
  bqc.add(&bq1).add(&bq2);