altb_pmic / Mbed OS cuboid_balance_ros

....

Dependencies:   Library_Cntrl Library_Misc_cuboid

Fork of cuboid_balance_ros by Ruprecht Altenburger

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?

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 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