altb_pmic / Mbed OS cuboid_balance_ros

....

Dependencies:   Library_Cntrl Library_Misc_cuboid

Fork of cuboid_balance_ros by Ruprecht Altenburger

update_loop.cpp@0:acf871f26563, 2019-03-08 (annotated)

Committer:
altb2
Date:
Fri Mar 08 13:34:59 2019 +0000
Revision:
0:acf871f26563
Child:
1:b9dc09f13a41
...

Who changed what in which revision?

UserRevisionLine numberNew 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 0:acf871f26563 41 // ****************** STATE MACHINE ***************************************
altb2 0:acf871f26563 42 switch(CS) {
altb2 0:acf871f26563 43 case INIT: // at very beginning
altb2 0:acf871f26563 44 if (dt > 5.0f){
altb2 0:acf871f26563 45 CS = FLAT; // switch to FLAT state
altb2 0:acf871f26563 46 ti.reset();
altb2 0:acf871f26563 47 }
altb2 0:acf871f26563 48 out.write(1.6f);
altb2 0:acf871f26563 49 break;
altb2 0:acf871f26563 50 case FLAT: // cuboid is flat, keep motor velocity to zero
altb2 0:acf871f26563 51 torq = vel_cntrl(0.0f - vel);
altb2 0:acf871f26563 52 if (dt > 10.0f){
altb2 0:acf871f26563 53 CS = BALANCE;
altb2 0:acf871f26563 54 torq = cuboid_stab_cntrl(1);
altb2 0:acf871f26563 55 ti.reset();
altb2 0:acf871f26563 56 phi1_des = 0.0f;
altb2 0:acf871f26563 57 vel_cntrl.reset(0.0f); // reset velocity controller for the next time
altb2 0:acf871f26563 58 }
altb2 0:acf871f26563 59 break;
altb2 0:acf871f26563 60 case BALANCE: // balance the cube
altb2 0:acf871f26563 61 torq = cuboid_stab_cntrl(0);
altb2 0:acf871f26563 62 if (dt > 10.0f){
altb2 0:acf871f26563 63 CS = SWING_DOWN;
altb2 0:acf871f26563 64 phi1_des = 0.0f;
altb2 0:acf871f26563 65 ti.reset();
altb2 0:acf871f26563 66 }
altb2 0:acf871f26563 67 break;
altb2 0:acf871f26563 68 case SWING_DOWN:
altb2 0:acf871f26563 69 phi1_des = dt; // ramp the desired angle up to pi/4
altb2 0:acf871f26563 70 torq = cuboid_stab_cntrl(0);// call balance routine
altb2 0:acf871f26563 71 if (dt > 1.0f){ // good value for smooth moving down
altb2 0:acf871f26563 72 CS = FLAT; // move to flat
altb2 0:acf871f26563 73 phi1_des = 0.0f;
altb2 0:acf871f26563 74 ti.reset();
altb2 0:acf871f26563 75 }
altb2 0:acf871f26563 76 break;
altb2 0:acf871f26563 77 default: break;
altb2 0:acf871f26563 78 }
altb2 0:acf871f26563 79 out.write( i2u(torq/0.217f)); // motor const. is 0.217,
altb2 0:acf871f26563 80 }
altb2 0:acf871f26563 81 }
altb2 0:acf871f26563 82 void update_loop::sendSignal() {
altb2 0:acf871f26563 83 thread.signal_set(signal);
altb2 0:acf871f26563 84 }
altb2 0:acf871f26563 85
altb2 0:acf871f26563 86
altb2 0:acf871f26563 87 float update_loop::cuboid_stab_cntrl(int do_reset){
altb2 0:acf871f26563 88 /* | phi_des
altb2 0:acf871f26563 89 v
altb2 0:acf871f26563 90 ---
altb2 0:acf871f26563 91 | V | feed forward gain
altb2 0:acf871f26563 92 ---
altb2 0:acf871f26563 93 |
altb2 0:acf871f26563 94 ------- v --------
altb2 0:acf871f26563 95 0 -->O-->|om2zero|-->O---->| System |--o---> x = [phi1 phi1_t]
altb2 0:acf871f26563 96 ^- ------- ^- -------- |
altb2 0:acf871f26563 97 | | |
altb2 0:acf871f26563 98 vel | --- |
altb2 0:acf871f26563 99 --- -| K |<-----
altb2 0:acf871f26563 100 ---
altb2 0:acf871f26563 101 */
altb2 0:acf871f26563 102 if(do_reset == 1) // reset controller
altb2 0:acf871f26563 103 om2zero.reset(0.0f);
altb2 0:acf871f26563 104
altb2 0:acf871f26563 105 float M_des = om2zero(0.0f-vel); // outer controller to bring motor to zero
altb2 0:acf871f26563 106 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 107 // output PI V K(1) K(2)
altb2 0:acf871f26563 108 return torq;
altb2 0:acf871f26563 109 }
altb2 0:acf871f26563 110
altb2 0:acf871f26563 111 // start timer as soon as Button is pressed
altb2 0:acf871f26563 112 void update_loop::pressed()
altb2 0:acf871f26563 113 {
altb2 0:acf871f26563 114 t_but.start();
altb2 0:acf871f26563 115 key_was_pressed = false;
altb2 0:acf871f26563 116 }
altb2 0:acf871f26563 117
altb2 0:acf871f26563 118 // evaluating statemachine
altb2 0:acf871f26563 119 void update_loop::released()
altb2 0:acf871f26563 120 {
altb2 0:acf871f26563 121
altb2 0:acf871f26563 122 // readout, stop and reset timer
altb2 0:acf871f26563 123 float ButtonTime = t_but.read();
altb2 0:acf871f26563 124 t_but.stop();
altb2 0:acf871f26563 125 t_but.reset();
altb2 0:acf871f26563 126 if(ButtonTime > 0.05f)
altb2 0:acf871f26563 127 key_was_pressed = true;
altb2 0:acf871f26563 128 }
altb2 0:acf871f26563 129
altb2 0:acf871f26563 130