Diff: main.cpp

Revision:

26:bb7e14f59ddd

Parent:

25:596255732b65

Child:

27:456e8a47f815

--- a/main.cpp	Wed Oct 26 09:38:42 2016 +0000
+++ b/main.cpp	Wed Oct 26 09:53:35 2016 +0000
@@ -43,18 +43,7 @@
 Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
 //HIDScope    scope(4);
 
-//set constant or variable values
-float EMGgain = 1.0;
-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;
-
+//initial values
 float dx;
 float dy;
 double DerivativeCounts;
@@ -64,10 +53,16 @@
 const int cw = 0;       //values for cw and ccw are inverted!! cw=0 and ccw=1
 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!
+
 //set initial conditions
 float biceps_l = 0;
 float biceps_r = 0;
-
 float ReferencePosition_x = L2;
 float ReferencePosition_y = L1 + TowerHeight;
 float ReferencePosition_xnew = 0;
@@ -81,22 +76,12 @@
 float q1start = 0;
 float q2start = PI/2;
 
-float q1_refOutNew = 0;
-float q1_refOutMin = 0;         //WRONG values
-float q1_refOutMax = PI;        //WRONG values
-float q2_refOutNew = 0;
-float q2_refOutMin = 0;         //WRONG values
-float q2_refOutMax = PI;        //WRONG values
-
 float q1_error_prev = 0;
 float q2_error_prev = 0;
 float DerTotalError1 = 0;
 float DerTotalError2 = 0;
 float q1IntError = 0;
 float q2IntError = 0;
-float TotalError1= 0;
-float TotalError2= 0;
-float TotalErrorMin= 0;
 
 float motorValue1 = 0.0;
 float motorValue2 = 0.0;
@@ -104,6 +89,24 @@
 int counts2 = 0;
 int counts1Prev = 0;
 int counts2Prev = 0;
+
+
+//set constant or variable values (VALUES HAVE TO BE EDITED)
+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 q1_refOutNew = 0;
+float q1_refOutMin = 0;         //WRONG values
+float q1_refOutMax = PI;        //WRONG values
+float q2_refOutNew = 0;
+float q2_refOutMin = 0;         //WRONG values
+float q2_refOutMax = PI;        //WRONG values
+float TotalError1= 0;
+float TotalError2= 0;
+float TotalErrorMin= 0;
  
 //set BiQuad
 BiQuadChain bqc;