....
Dependencies: Library_Cntrl Library_Misc_cuboid
Fork of cuboid_balance_ros by
update_loop.cpp
- Committer:
- altb2
- Date:
- 2019-11-22
- Revision:
- 1:b9dc09f13a41
- Parent:
- 0:acf871f26563
File content as of revision 1:b9dc09f13a41:
#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); printf("."); // ****************** 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; }