File content as of revision 0:acf871f26563:

#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;
}