Diff: main.cpp

Revision:

6:ae4a619be005

Parent:

5:f7e4ecadffd2

Child:

7:c662c825d755

--- a/main.cpp	Mon Jun 11 08:11:21 2018 +0000
+++ b/main.cpp	Mon Jun 11 08:33:25 2018 +0000
@@ -11,7 +11,7 @@
 #define LOOPDURATIONUS 200 //microsec
 #define LOOPDURATION ((float)LOOPDURATIONUS*0.000001f) //sec
 #define LEDPERIODS 1000 //How many loops before changing the LED state
-#define CALIBPWM 0.085f //12.5% of max PWM is the calibration PWM
+#define CALIBPWM -0.1f //12.5% of max PWM is the calibration PWM
 #define ENCODER_CPR 2048.0f //Counts per revolution for the encode (1x quad, to avoid missing counts)
 #define TWO_PI 6.283185307179586476925286766559f //rad
 #define WORKSPACEBOUND 7.0f //workspace bound for admittane model
@@ -179,10 +179,10 @@
     //Set proper PWM
     if (u >= 0.0f) {
         motorPWM = u;
-        motorDir = false;
+        motorDir = true;
     } else {
         motorPWM = -u;
-        motorDir = true;
+        motorDir = false;
     }
 }
 
@@ -276,12 +276,12 @@
 inline void doStateHoming()
 {
     //Calculate a ramp to location 0 (center) with -2 rad/s
-    refVel = -2.00f;
-    if (refPos > -1.0f) {
-        refPos -= refVel*LOOPDURATION;//refVel*LOOPDURATION;
+    refVel = 2.00f;
+    if (refPos < 9.296f) {
+        refPos += refVel*LOOPDURATION;//refVel*LOOPDURATION;
     }
     else {
-        refPos = -1.0f;
+        refPos = 9.296f;
         refVel = 0.0f;
     }
     
@@ -303,11 +303,12 @@
     }
     
     //State Guard
-    if ((abs(encoderPos) < 6) && (abs(velError) < 0.002f) && (t.read()-stateTime > 1.0f)) {
+    if ((abs(posError) < 0.01f) && (abs(velError) < 0.002f) && (t.read()-stateTime > 1.0f)) {
         integratedError = 0.0f;
         setLed(colorGreen);
         stateTime = t.read();
-        currentState = stateOPAdmittance;//stateOPImpedance;
+        //currentState = stateOPAdmittance;//stateOPImpedance;
+        currentState = stateFailed;
         
         //Reset encoder back to 0 in center:
         enc.reset();