....
Dependencies: Library_Cntrl Library_Misc_cuboid
Fork of cuboid_balance_ros by
Diff: update_loop.cpp
- Revision:
- 0:acf871f26563
- Child:
- 1:b9dc09f13a41
diff -r 000000000000 -r acf871f26563 update_loop.cpp --- /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; +} + +