Diff: main.cpp

Revision:

8:ef8e5f23abf7

Parent:

7:c662c825d755

--- a/main.cpp	Mon Jun 11 09:53:29 2018 +0000
+++ b/main.cpp	Thu Jun 21 07:52:17 2018 +0000
@@ -3,7 +3,7 @@
 #include "MODSERIAL.h" //for more efficient serial comm
 #include "FastPWM.h" //To fix a bug in the standard mbed lib concerning pwm
 #include "QEI.h"
-
+//#include "HIDScope.h"
 //#efines--------------------------------------------------------------------------------------------------------
 #define PWMCYCLETIMEUS 50 //20 kHz PWM cycle time
 #define MAX_ENCODER_POS_RIGHT -1453 //Encoder counts at right end-stop
@@ -25,10 +25,14 @@
 //Pins and interrupts--------------------------------------------------------------------------------------------------------
 // Remember that pins D4,D5,D6,D7 are internally connected to the drivers! Do not use D6 and D7.
 Ticker mainLoop; //Main loop runs at fixed fequency
-
+//HIDScope    scope(2);
+//Ticker      scopeTimer;
 //Inputs
 AnalogIn    measuredForce(A5);  //Pin that measures analog force
 
+
+//HIDScope
+
 QEI enc(PTC12,PTC4,NC,1024,QEI::X2_ENCODING);
 
 //Outputs
@@ -68,10 +72,14 @@
 float refAcc = 0.0f; //System reference acceleration
 float u = 0.0f; //Control signal (PWM duty cycle)
 
+
+float refForce = 0.0f;
+float forceError= 0.0f;
 //Controller gains
-const float K_p = 4.5f; //Proportional position gain
-const float K_d = 0.03f; //Differential position gain
-const float K_i = 0.000f; //Integral position gain
+const float K_p = 4.0;
+//Proportional position gain
+const float K_d = 0.00f; //Differential position gain
+const float K_i = 0.5f; //Integral position gain
 const float I_ff = 0.0000; //Feed-forward mass
 float posError = 0.0f;
 float velError = 0.0f;
@@ -79,7 +87,7 @@
 
 //Virtual model properties
 const float virtualMass = 0.0015f;
-const float virtualStiffness = 0.05f;
+const float virtualStiffness = 0.20f;
 const float virtualDamping = 0.005f;
 const float springPos = 0.0f;
 
@@ -289,7 +297,7 @@
     posError = refPos - motorPos;
     integratedError += posError*LOOPDURATION;
     velError = refVel - motorVel;
-    u = K_p * posError + K_d * velError + 0*K_i*integratedError;
+    u = 4.5 * posError + 0.03 * velError + 0*K_i*integratedError;
 
     //Handle LED
     ledCnt++;
@@ -309,7 +317,7 @@
         integratedError = 0.0f;
         setLed(colorGreen);
         stateTime = t.read();
-        currentState = stateOPAdmittance;//stateOPImpedance;
+        currentState = stateOPImpedance;
         //currentState = stateFailed;
         
         //Reset encoder back to 0 in center:
@@ -365,10 +373,14 @@
 
 inline void doStateOPImpedance()
 {
-    //Controller is the virtual model in OPEN LOOP impedance
-    u = - virtualStiffness * (motorPos - springPos) - virtualDamping * motorVel;
-    //StateGuard
-    //if...
+    //Virtual model: Damper + Spring. Calculates the force it acutally wants to have
+    refForce = virtualStiffness * motorPos + virtualDamping * motorVel;
+    //Input error for  PID
+    forceError = -refForce + force;
+    integratedError += forceError*LOOPDURATION;
+    //Calculation of the duty cycle
+    u = (K_p * forceError + K_i * integratedError);
+    //scope.set(0,u);
 }
 
 inline void doStateFailed()
@@ -443,7 +455,7 @@
 
     motorPWM.period_us(PWMCYCLETIMEUS); //50 us = 20 kHz
     mainLoop.attach_us(&loopFunction,LOOPDURATIONUS); //200 us = 5 kHz
-    
+    //scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); 
     //Infinite while loop
     while (true) {
         //Print relevant stuff to terminal here, do not print in the in the loopFunction ticker!!
@@ -451,9 +463,13 @@
             pc.printf(">>>>>State Changed to State: %d\r\n",currentState);
             statePrev = currentState; //Update the prev state var
         }
-        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\r\n",millisecondsPassed,currentState,encoderPos,motorPos,motorVel,force,refPos);//,posError,u); ;PosErr:%f;MotPWM:%f
+        wait(0.1);
+        //float millisecondsPassed = (float)loopCounter*(((float)LOOPDURATIONUS) / 1000.0f);
+        //pc.printf("Proportional: %f \t Integrator %f \t ForceError: %f \t Voltage:%f\r\n",(K_p * forceError)/12 ,(K_i * integratedError)/12,forceError,u);
+        //pc.printf("ForceMeasured: %f \t Force Reference: %f \r\n",force, refForce);
+        pc.printf("Voltage: %f \r\n",u);
+        pc.printf("Error: %f \r\n",forceError);
+        //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