Diff: main.cpp

Revision:

5:f7e4ecadffd2

Parent:

3:5cb5f0f22c5a

Child:

6:ae4a619be005

diff -r 5cb5f0f22c5a -r f7e4ecadffd2 main.cpp
--- a/main.cpp	Mon Jun 11 07:58:24 2018 +0000
+++ b/main.cpp	Mon Jun 11 08:11:21 2018 +0000
@@ -199,8 +199,8 @@
 void mapIn()
 {
     //Measure relevant signals
-    encoderPos = encoderPos;
-    motorPos = (float)enc.getPulses() / ENCODER_CPR * TWO_PI; //motor position in rad
+    encoderPos = enc.getPulses();
+    motorPos = (float)encoderPos / ENCODER_CPR * TWO_PI; //motor position in rad
     encoderVel = (float)(encoderPos - encoderPos_prev) / LOOPDURATION; //encoder velocity in counts/s
     encoderPos_prev = encoderPos; //encoder position in counts
     motorVel = encoderVel / ENCODER_CPR * TWO_PI; //motor velocity in rad/s
@@ -269,7 +269,7 @@
         // NEED TO UPDATE CALIBRATION HERE.
         //encoderPos = -MAX_ENCODER_POS_LEFT; //When calibrated, set encoder pos to the known value
         stateTime = t.read();
-        refPos = (float)MAX_ENCODER_POS_LEFT /  ENCODER_CPR * TWO_PI; //Start at the left calibration point
+        refPos = 0;//(float)MAX_ENCODER_POS_LEFT /  ENCODER_CPR * TWO_PI; //Start at the left calibration point
     }
 }
 
@@ -277,11 +277,11 @@
 {
     //Calculate a ramp to location 0 (center) with -2 rad/s
     refVel = -2.00f;
-    if (refPos > 0.0f) {
-        refPos += refVel*LOOPDURATION;//refVel*LOOPDURATION;
+    if (refPos > -1.0f) {
+        refPos -= refVel*LOOPDURATION;//refVel*LOOPDURATION;
     }
     else {
-        refPos = 0.0f;
+        refPos = -1.0f;
         refVel = 0.0f;
     }
     
@@ -308,6 +308,10 @@
         setLed(colorGreen);
         stateTime = t.read();
         currentState = stateOPAdmittance;//stateOPImpedance;
+        
+        //Reset encoder back to 0 in center:
+        enc.reset();
+         
         /*if (doAdmittance) {
             currentState = stateOPAdmittance;
         } else {