Diff: update_loop.cpp

Revision:

0:acf871f26563

Child:

1:b9dc09f13a41

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/update_loop.cpp	Fri Mar 08 13:34:59 2019 +0000
@@ -0,0 +1,130 @@
+#include "update_loop.h"
+#include "EncoderCounter.h"
+#include "IIR_filter.h"
+#include "mbed.h"
+
+
+using namespace std;
+
+update_loop::update_loop(float Ts, PinName P1) : thread(osPriorityNormal,4096),
+AE(Ts),button(P1),counter1(PB_6, PB_7),diff(0.01f,Ts),vel_cntrl(0.5f,.05f,Ts,0.5f),
+om2zero(-0.02f,4.0f,Ts,0.9f),out(PA_5),pc(USBTX, USBRX,115200) // Serial conn. via USB
+{
+    thread.start(callback(this, &update_loop::loop));
+    ticker.attach(callback(this, &update_loop::sendSignal), Ts);
+    key_was_pressed = false;
+    button.fall(callback(this, &update_loop::pressed));          // attach key pressed function
+    button.rise(callback(this, &update_loop::released));         // attach key pressed function
+    ti.reset();
+    ti.start();
+    counter1.reset();   // encoder reset
+    diff.reset(0.0f,0);
+    CS = INIT;
+    float phi1_des = 0.0f;
+    i2u.setup(-15.0f,15.0f,0.0f,3.2f / 3.3f);       // output is normalized output
+    AE.calc_angle_phi1(1);
+    kk=0;
+    }
+
+
+update_loop::~update_loop() {}
+
+
+void update_loop::loop(void){
+    while(1){
+        thread.signal_wait(signal);
+        short counts = counter1;            // get counts from Encoder
+        vel = diff(counts);                 // motor velocity 
+        float torq = 0.0f;                  // set motor torque to zero (will be overwritten)
+        float dt  = ti.read();              // time elapsed in current state
+        AE.calc_angle_phi1(0);
+        // ****************** STATE  MACHINE ***************************************
+        switch(CS)  {
+            case INIT:                      // at very beginning
+                if (dt > 5.0f){
+                    CS = FLAT;              // switch to FLAT state
+                    ti.reset(); 
+                    }
+                    out.write(1.6f);
+                break;
+            case FLAT:                      // cuboid is flat, keep motor velocity to zero
+                torq = vel_cntrl(0.0f - vel); 
+                if (dt > 10.0f){
+                    CS = BALANCE;
+                    torq = cuboid_stab_cntrl(1);
+                    ti.reset(); 
+                    phi1_des = 0.0f;
+                    vel_cntrl.reset(0.0f);  // reset velocity controller for the next time
+                    }
+                break;
+            case BALANCE:                   // balance the cube
+                torq = cuboid_stab_cntrl(0);
+                if (dt > 10.0f){
+                    CS = SWING_DOWN;
+                    phi1_des = 0.0f; 
+                    ti.reset(); 
+                    }
+                break;  
+            case SWING_DOWN: 
+                phi1_des = dt;              // ramp the desired angle up to pi/4
+                torq = cuboid_stab_cntrl(0);// call balance routine
+                if (dt > 1.0f){             // good value for smooth moving down
+                    CS = FLAT;              // move to flat
+                    phi1_des = 0.0f; 
+                    ti.reset(); 
+                    }
+                break;  
+        default: break;
+        }
+        out.write( i2u(torq/0.217f)); // motor const. is 0.217, 
+    }
+}
+void update_loop::sendSignal() {
+        thread.signal_set(signal);
+}
+
+
+float update_loop::cuboid_stab_cntrl(int do_reset){
+/*                   | phi_des
+                     v 
+                    ---
+                   | V |  feed forward gain
+                    ---
+                     |
+          -------    v      --------
+0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t]
+     ^-   -------    ^-     --------   |
+     |               |                 |
+    vel              |       ---       |
+                      ---  -| K |<-----
+                             ---
+*/
+    if(do_reset == 1)       // reset controller
+        om2zero.reset(0.0f);
+        
+    float M_des = om2zero(0.0f-vel);    // outer controller to bring motor to zero
+    float torq = M_des    +  (-7.1*phi1_des) -(-9.6910f * AE.get_phi1() -0.7119f * AE.get_gyro());  // calculationof gains are based on the MATLAB script at the end of this file!!!
+    //          output PI       V               K(1)              K(2)
+    return torq;
+}
+ 
+// start timer as soon as Button is pressed
+void update_loop::pressed()
+{
+    t_but.start();
+    key_was_pressed = false;
+}
+ 
+// evaluating statemachine
+void update_loop::released()
+{
+ 
+    // readout, stop and reset timer
+    float ButtonTime = t_but.read();
+    t_but.stop();
+    t_but.reset();
+    if(ButtonTime > 0.05f) 
+        key_was_pressed = true;
+}
+ 
+