....
Dependencies: Library_Cntrl Library_Misc_cuboid
Fork of cuboid_balance_ros by
update_loop.h@0:acf871f26563, 2019-03-08 (annotated)
- Committer:
- altb2
- Date:
- Fri Mar 08 13:34:59 2019 +0000
- Revision:
- 0:acf871f26563
...
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:acf871f26563 | 1 | #include "PI_Cntrl.h" |
altb2 | 0:acf871f26563 | 2 | #include "mbed.h" |
altb2 | 0:acf871f26563 | 3 | #include "Signal.h" |
altb2 | 0:acf871f26563 | 4 | #include "EncoderCounter.h" |
altb2 | 0:acf871f26563 | 5 | #include "DiffCounter.h" |
altb2 | 0:acf871f26563 | 6 | #include "angle_estimator.h" |
altb2 | 0:acf871f26563 | 7 | |
altb2 | 0:acf871f26563 | 8 | |
altb2 | 0:acf871f26563 | 9 | // define STATES: |
altb2 | 0:acf871f26563 | 10 | #define INIT 0 // at very beginning |
altb2 | 0:acf871f26563 | 11 | #define FLAT 10 // cuboid is flat, motor is controlled to zero |
altb2 | 0:acf871f26563 | 12 | #define BALANCE 20 // balancing |
altb2 | 0:acf871f26563 | 13 | #define SWING_DOWN 30 // move cuboid down |
altb2 | 0:acf871f26563 | 14 | |
altb2 | 0:acf871f26563 | 15 | |
altb2 | 0:acf871f26563 | 16 | |
altb2 | 0:acf871f26563 | 17 | |
altb2 | 0:acf871f26563 | 18 | class update_loop |
altb2 | 0:acf871f26563 | 19 | { |
altb2 | 0:acf871f26563 | 20 | public: |
altb2 | 0:acf871f26563 | 21 | |
altb2 | 0:acf871f26563 | 22 | update_loop(float Ts, PinName); |
altb2 | 0:acf871f26563 | 23 | virtual ~update_loop(); |
altb2 | 0:acf871f26563 | 24 | InterruptIn button; |
altb2 | 0:acf871f26563 | 25 | |
altb2 | 0:acf871f26563 | 26 | private: |
altb2 | 0:acf871f26563 | 27 | void loop(void); |
altb2 | 0:acf871f26563 | 28 | Signal signal; |
altb2 | 0:acf871f26563 | 29 | Thread thread; |
altb2 | 0:acf871f26563 | 30 | Ticker ticker; |
altb2 | 0:acf871f26563 | 31 | Mutex mutex; // mutex to lock critical sections |
altb2 | 0:acf871f26563 | 32 | double Ts; |
altb2 | 0:acf871f26563 | 33 | bool key_was_pressed; |
altb2 | 0:acf871f26563 | 34 | Timer ti; |
altb2 | 0:acf871f26563 | 35 | Timer t_but; // define button time // |
altb2 | 0:acf871f26563 | 36 | PI_Cntrl vel_cntrl; // velocity controller for motor |
altb2 | 0:acf871f26563 | 37 | PI_Cntrl om2zero; // slow vel. contrl. to bring motor to zero |
altb2 | 0:acf871f26563 | 38 | Serial pc; |
altb2 | 0:acf871f26563 | 39 | void sendSignal(); |
altb2 | 0:acf871f26563 | 40 | void pressed(void); // user Button pressed |
altb2 | 0:acf871f26563 | 41 | void released(void); // user Button released |
altb2 | 0:acf871f26563 | 42 | float cuboid_stab_cntrl(int); |
altb2 | 0:acf871f26563 | 43 | float vel; |
altb2 | 0:acf871f26563 | 44 | int CS; |
altb2 | 0:acf871f26563 | 45 | int kk; |
altb2 | 0:acf871f26563 | 46 | EncoderCounter counter1; // |
altb2 | 0:acf871f26563 | 47 | DiffCounter diff; // discrete differentiate, based on encoder data |
altb2 | 0:acf871f26563 | 48 | float phi1_des; |
altb2 | 0:acf871f26563 | 49 | angle_estimator AE; |
altb2 | 0:acf871f26563 | 50 | AnalogOut out; |
altb2 | 0:acf871f26563 | 51 | LinearCharacteristics i2u; // output is normalized output |
altb2 | 0:acf871f26563 | 52 | }; |