Diff: main.cpp

Revision:

7:c662c825d755

Parent:

6:ae4a619be005

--- a/main.cpp	Mon Jun 11 08:33:25 2018 +0000
+++ b/main.cpp	Mon Jun 11 09:53:29 2018 +0000
@@ -16,7 +16,7 @@
 #define TWO_PI 6.283185307179586476925286766559f //rad
 #define WORKSPACEBOUND 7.0f //workspace bound for admittane model
 #define MAXVEL 100.0f //maximal admittance control velocity in rad/s
-#define FORCESENSORGAIN 15.134f //N per measured unit
+#define FORCESENSORGAIN 10.0f //N per measured unit
 
 //Low pass filter filter coeffs, 2nd order, 200 Hz
 double b[3] = {0.003621681514929, 0.007243363029857, 0.003621681514929};
@@ -79,8 +79,8 @@
 
 //Virtual model properties
 const float virtualMass = 0.0015f;
-const float virtualStiffness = 50.0f;
-const float virtualDamping = 3.0f;
+const float virtualStiffness = 0.05f;
+const float virtualDamping = 0.005f;
 const float springPos = 0.0f;
 
 //Measured variables
@@ -206,7 +206,7 @@
     motorVel = encoderVel / ENCODER_CPR * TWO_PI; //motor velocity in rad/s
     motorVel = velocityFilter(motorVel); //filtered motor velocity in rad/s
     motorPos_prev = motorPos; //Previous motor position in rad
-    force = -FORCESENSORGAIN*2.0f*(measuredForce - 0.5f); //Measured force
+    force = FORCESENSORGAIN*(measuredForce - 0.505f); //Measured force
     doNetwork(); //Receive inputs over network (if available)
 }
 
@@ -304,14 +304,17 @@
     
     //State Guard
     if ((abs(posError) < 0.01f) && (abs(velError) < 0.002f) && (t.read()-stateTime > 1.0f)) {
+        refPos = 0.0f;
+        refVel = 0.0f;
         integratedError = 0.0f;
         setLed(colorGreen);
         stateTime = t.read();
-        //currentState = stateOPAdmittance;//stateOPImpedance;
-        currentState = stateFailed;
+        currentState = stateOPAdmittance;//stateOPImpedance;
+        //currentState = stateFailed;
         
         //Reset encoder back to 0 in center:
         enc.reset();
+        encoderPos_prev = 0;
          
         /*if (doAdmittance) {
             currentState = stateOPAdmittance;
@@ -324,7 +327,7 @@
 inline void doStateOPAdmittance()
 {
     //Virtual Model
-    refAcc = force / virtualMass - virtualStiffness * (refPos - springPos) - virtualDamping * refVel;
+    refAcc = (force - virtualStiffness * (refPos - springPos) - virtualDamping * refVel) / virtualMass;
     refVel += refAcc * LOOPDURATION;
     
     if (refVel > MAXVEL) 
@@ -450,7 +453,7 @@
         }
         wait(0.01);
         float millisecondsPassed = (float)loopCounter*(((float)LOOPDURATIONUS) / 1000.0f);
-        pc.printf("t:%f;State:%d;EncPos:%d;MotPos:%f;MotVel:%f;Force:%f;RefPos:%f;PosErr:%f;MotPWM:%f\r\n",millisecondsPassed,currentState,encoderPos,motorPos,motorVel,force,refPos,posError,u);
+        pc.printf("t:%f;State:%d;EncPos:%d;MotPos:%f;MotVel:%f;Force:%f;RefPos:%f\r\n",millisecondsPassed,currentState,encoderPos,motorPos,motorVel,force,refPos);//,posError,u); ;PosErr:%f;MotPWM:%f
     }
 }
 //EOF--------------------------------------------------------------------------------------------------------
\ No newline at end of file