Diff: ControllerLoop.cpp

Revision:

5:768e10f6d372

Child:

6:9ebeffe446e4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ControllerLoop.cpp	Thu Feb 25 20:28:45 2021 +0000
@@ -0,0 +1,52 @@
+#include "ControllerLoop.h"
+using namespace std;
+
+
+
+ControllerLoop::ControllerLoop(float Ts) : thread(osPriorityNormal,4096)
+{
+    thread.start(callback(this, &ControllerLoop::loop));
+    ticker.attach(callback(this, &ControllerLoop::sendSignal), Ts);
+    diff1.reset(0.0f,0);
+    diff2.reset(0.0f,0);
+    }
+
+
+ControllerLoop::~ControllerLoop() {}
+
+
+void ControllerLoop::loop(void){
+    float Kp = 1500 * 4.89e-7/0.094;   // XX * J/km
+    float Tn = .0035;
+    float Kv=50;
+    float phi_des = 0;
+    float v_des = 0;
+    PID_Cntrl v_cntrl(Kp,Tn,0,1.0,Ts,-.95,.95);
+    Unwrapper_2pi uw2pi;
+    uint8_t k = 0;
+    while(1)
+        {
+        ThisThread::flags_wait_any(threadFlag);
+        // THE LOOP ------------------------------------------------------------
+        if(++k%64==0)
+            {
+                printf("%f %f\r\n",phi_des,v_des);
+            }
+        short c1 = counter1;            // get counts from Encoder
+        short c2 = counter2;            // get counts from Encoder
+        float phi1 = uw2pi(2*3.1415927/4000.0*(float)c1);
+        float vel1 = diff1(c1);                 // motor velocity 
+        float phi2 = uw2pi(2*3.1415927/4000.0*(float)c2);
+        float vel2 = diff2(c2);                 // motor velocity 
+        float w0=2*3.1415927 * 3;
+        //phi_des = 1.0*sinf(w0*ti.read());
+        current_path->get_x_v(glob_ti.read(),&phi_des,&v_des);
+        float i1 = v_cntrl(Kv * (phi_des-phi2)+v_des-vel2 );
+        //float i1 = v_cntrl(v_des-vel ) + exc;
+        i_des2.write(i2pwm(-i1));
+        }
+}
+
+void ControllerLoop::sendSignal() {
+    thread.flags_set(threadFlag);
+}