....
Dependencies: Library_Cntrl Library_Misc_cuboid
Fork of cuboid_balance_ros by
update_loop.cpp@1:b9dc09f13a41, 2019-11-22 (annotated)
- Committer:
- altb2
- Date:
- Fri Nov 22 16:44:48 2019 +0000
- Revision:
- 1:b9dc09f13a41
- Parent:
- 0:acf871f26563
updated ros
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:acf871f26563 | 1 | #include "update_loop.h" |
altb2 | 0:acf871f26563 | 2 | #include "EncoderCounter.h" |
altb2 | 0:acf871f26563 | 3 | #include "IIR_filter.h" |
altb2 | 0:acf871f26563 | 4 | #include "mbed.h" |
altb2 | 0:acf871f26563 | 5 | |
altb2 | 0:acf871f26563 | 6 | |
altb2 | 0:acf871f26563 | 7 | using namespace std; |
altb2 | 0:acf871f26563 | 8 | |
altb2 | 0:acf871f26563 | 9 | update_loop::update_loop(float Ts, PinName P1) : thread(osPriorityNormal,4096), |
altb2 | 0:acf871f26563 | 10 | AE(Ts),button(P1),counter1(PB_6, PB_7),diff(0.01f,Ts),vel_cntrl(0.5f,.05f,Ts,0.5f), |
altb2 | 0:acf871f26563 | 11 | om2zero(-0.02f,4.0f,Ts,0.9f),out(PA_5),pc(USBTX, USBRX,115200) // Serial conn. via USB |
altb2 | 0:acf871f26563 | 12 | { |
altb2 | 0:acf871f26563 | 13 | thread.start(callback(this, &update_loop::loop)); |
altb2 | 0:acf871f26563 | 14 | ticker.attach(callback(this, &update_loop::sendSignal), Ts); |
altb2 | 0:acf871f26563 | 15 | key_was_pressed = false; |
altb2 | 0:acf871f26563 | 16 | button.fall(callback(this, &update_loop::pressed)); // attach key pressed function |
altb2 | 0:acf871f26563 | 17 | button.rise(callback(this, &update_loop::released)); // attach key pressed function |
altb2 | 0:acf871f26563 | 18 | ti.reset(); |
altb2 | 0:acf871f26563 | 19 | ti.start(); |
altb2 | 0:acf871f26563 | 20 | counter1.reset(); // encoder reset |
altb2 | 0:acf871f26563 | 21 | diff.reset(0.0f,0); |
altb2 | 0:acf871f26563 | 22 | CS = INIT; |
altb2 | 0:acf871f26563 | 23 | float phi1_des = 0.0f; |
altb2 | 0:acf871f26563 | 24 | i2u.setup(-15.0f,15.0f,0.0f,3.2f / 3.3f); // output is normalized output |
altb2 | 0:acf871f26563 | 25 | AE.calc_angle_phi1(1); |
altb2 | 0:acf871f26563 | 26 | kk=0; |
altb2 | 0:acf871f26563 | 27 | } |
altb2 | 0:acf871f26563 | 28 | |
altb2 | 0:acf871f26563 | 29 | |
altb2 | 0:acf871f26563 | 30 | update_loop::~update_loop() {} |
altb2 | 0:acf871f26563 | 31 | |
altb2 | 0:acf871f26563 | 32 | |
altb2 | 0:acf871f26563 | 33 | void update_loop::loop(void){ |
altb2 | 0:acf871f26563 | 34 | while(1){ |
altb2 | 0:acf871f26563 | 35 | thread.signal_wait(signal); |
altb2 | 0:acf871f26563 | 36 | short counts = counter1; // get counts from Encoder |
altb2 | 0:acf871f26563 | 37 | vel = diff(counts); // motor velocity |
altb2 | 0:acf871f26563 | 38 | float torq = 0.0f; // set motor torque to zero (will be overwritten) |
altb2 | 0:acf871f26563 | 39 | float dt = ti.read(); // time elapsed in current state |
altb2 | 0:acf871f26563 | 40 | AE.calc_angle_phi1(0); |
altb2 | 1:b9dc09f13a41 | 41 | printf("."); |
altb2 | 0:acf871f26563 | 42 | // ****************** STATE MACHINE *************************************** |
altb2 | 0:acf871f26563 | 43 | switch(CS) { |
altb2 | 0:acf871f26563 | 44 | case INIT: // at very beginning |
altb2 | 0:acf871f26563 | 45 | if (dt > 5.0f){ |
altb2 | 0:acf871f26563 | 46 | CS = FLAT; // switch to FLAT state |
altb2 | 0:acf871f26563 | 47 | ti.reset(); |
altb2 | 0:acf871f26563 | 48 | } |
altb2 | 0:acf871f26563 | 49 | out.write(1.6f); |
altb2 | 0:acf871f26563 | 50 | break; |
altb2 | 0:acf871f26563 | 51 | case FLAT: // cuboid is flat, keep motor velocity to zero |
altb2 | 0:acf871f26563 | 52 | torq = vel_cntrl(0.0f - vel); |
altb2 | 0:acf871f26563 | 53 | if (dt > 10.0f){ |
altb2 | 0:acf871f26563 | 54 | CS = BALANCE; |
altb2 | 0:acf871f26563 | 55 | torq = cuboid_stab_cntrl(1); |
altb2 | 0:acf871f26563 | 56 | ti.reset(); |
altb2 | 0:acf871f26563 | 57 | phi1_des = 0.0f; |
altb2 | 0:acf871f26563 | 58 | vel_cntrl.reset(0.0f); // reset velocity controller for the next time |
altb2 | 0:acf871f26563 | 59 | } |
altb2 | 0:acf871f26563 | 60 | break; |
altb2 | 0:acf871f26563 | 61 | case BALANCE: // balance the cube |
altb2 | 0:acf871f26563 | 62 | torq = cuboid_stab_cntrl(0); |
altb2 | 0:acf871f26563 | 63 | if (dt > 10.0f){ |
altb2 | 0:acf871f26563 | 64 | CS = SWING_DOWN; |
altb2 | 0:acf871f26563 | 65 | phi1_des = 0.0f; |
altb2 | 0:acf871f26563 | 66 | ti.reset(); |
altb2 | 0:acf871f26563 | 67 | } |
altb2 | 0:acf871f26563 | 68 | break; |
altb2 | 0:acf871f26563 | 69 | case SWING_DOWN: |
altb2 | 0:acf871f26563 | 70 | phi1_des = dt; // ramp the desired angle up to pi/4 |
altb2 | 0:acf871f26563 | 71 | torq = cuboid_stab_cntrl(0);// call balance routine |
altb2 | 0:acf871f26563 | 72 | if (dt > 1.0f){ // good value for smooth moving down |
altb2 | 0:acf871f26563 | 73 | CS = FLAT; // move to flat |
altb2 | 0:acf871f26563 | 74 | phi1_des = 0.0f; |
altb2 | 0:acf871f26563 | 75 | ti.reset(); |
altb2 | 0:acf871f26563 | 76 | } |
altb2 | 0:acf871f26563 | 77 | break; |
altb2 | 0:acf871f26563 | 78 | default: break; |
altb2 | 0:acf871f26563 | 79 | } |
altb2 | 0:acf871f26563 | 80 | out.write( i2u(torq/0.217f)); // motor const. is 0.217, |
altb2 | 0:acf871f26563 | 81 | } |
altb2 | 0:acf871f26563 | 82 | } |
altb2 | 0:acf871f26563 | 83 | void update_loop::sendSignal() { |
altb2 | 0:acf871f26563 | 84 | thread.signal_set(signal); |
altb2 | 0:acf871f26563 | 85 | } |
altb2 | 0:acf871f26563 | 86 | |
altb2 | 0:acf871f26563 | 87 | |
altb2 | 0:acf871f26563 | 88 | float update_loop::cuboid_stab_cntrl(int do_reset){ |
altb2 | 0:acf871f26563 | 89 | /* | phi_des |
altb2 | 0:acf871f26563 | 90 | v |
altb2 | 0:acf871f26563 | 91 | --- |
altb2 | 0:acf871f26563 | 92 | | V | feed forward gain |
altb2 | 0:acf871f26563 | 93 | --- |
altb2 | 0:acf871f26563 | 94 | | |
altb2 | 0:acf871f26563 | 95 | ------- v -------- |
altb2 | 0:acf871f26563 | 96 | 0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t] |
altb2 | 0:acf871f26563 | 97 | ^- ------- ^- -------- | |
altb2 | 0:acf871f26563 | 98 | | | | |
altb2 | 0:acf871f26563 | 99 | vel | --- | |
altb2 | 0:acf871f26563 | 100 | --- -| K |<----- |
altb2 | 0:acf871f26563 | 101 | --- |
altb2 | 0:acf871f26563 | 102 | */ |
altb2 | 0:acf871f26563 | 103 | if(do_reset == 1) // reset controller |
altb2 | 0:acf871f26563 | 104 | om2zero.reset(0.0f); |
altb2 | 0:acf871f26563 | 105 | |
altb2 | 0:acf871f26563 | 106 | float M_des = om2zero(0.0f-vel); // outer controller to bring motor to zero |
altb2 | 0:acf871f26563 | 107 | 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!!! |
altb2 | 0:acf871f26563 | 108 | // output PI V K(1) K(2) |
altb2 | 0:acf871f26563 | 109 | return torq; |
altb2 | 0:acf871f26563 | 110 | } |
altb2 | 0:acf871f26563 | 111 | |
altb2 | 0:acf871f26563 | 112 | // start timer as soon as Button is pressed |
altb2 | 0:acf871f26563 | 113 | void update_loop::pressed() |
altb2 | 0:acf871f26563 | 114 | { |
altb2 | 0:acf871f26563 | 115 | t_but.start(); |
altb2 | 0:acf871f26563 | 116 | key_was_pressed = false; |
altb2 | 0:acf871f26563 | 117 | } |
altb2 | 0:acf871f26563 | 118 | |
altb2 | 0:acf871f26563 | 119 | // evaluating statemachine |
altb2 | 0:acf871f26563 | 120 | void update_loop::released() |
altb2 | 0:acf871f26563 | 121 | { |
altb2 | 0:acf871f26563 | 122 | |
altb2 | 0:acf871f26563 | 123 | // readout, stop and reset timer |
altb2 | 0:acf871f26563 | 124 | float ButtonTime = t_but.read(); |
altb2 | 0:acf871f26563 | 125 | t_but.stop(); |
altb2 | 0:acf871f26563 | 126 | t_but.reset(); |
altb2 | 0:acf871f26563 | 127 | if(ButtonTime > 0.05f) |
altb2 | 0:acf871f26563 | 128 | key_was_pressed = true; |
altb2 | 0:acf871f26563 | 129 | } |
altb2 | 0:acf871f26563 | 130 | |
altb2 | 0:acf871f26563 | 131 |