Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
main.cpp@5:768e10f6d372, 2021-02-25 (annotated)
- Committer:
- altb2
- Date:
- Thu Feb 25 20:28:45 2021 +0000
- Revision:
- 5:768e10f6d372
- Parent:
- 4:3d8dd3d17564
- Child:
- 6:9ebeffe446e4
first commit of Mirror actuator
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:a56e9c932891 | 1 | #include "mbed.h" |
altb2 | 0:a56e9c932891 | 2 | #include "math.h" |
altb2 | 0:a56e9c932891 | 3 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 4 | #define PI 3.1415927f |
altb2 | 0:a56e9c932891 | 5 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 6 | #include "EncoderCounter.h" |
altb2 | 5:768e10f6d372 | 7 | #include "EncoderCounterIndex.h" |
altb2 | 0:a56e9c932891 | 8 | #include "DiffCounter.h" |
altb2 | 0:a56e9c932891 | 9 | #include "IIR_filter.h" |
altb2 | 0:a56e9c932891 | 10 | #include "LinearCharacteristics.h" |
altb2 | 5:768e10f6d372 | 11 | #include "PID_Cntrl.h" |
altb2 | 5:768e10f6d372 | 12 | #include "Unwrapper_2pi.h" |
altb2 | 5:768e10f6d372 | 13 | #include "path_1d.h" |
altb2 | 5:768e10f6d372 | 14 | #include "GPA.h" |
altb2 | 5:768e10f6d372 | 15 | #include "ControllerLoop.h" |
altb2 | 5:768e10f6d372 | 16 | |
altb2 | 5:768e10f6d372 | 17 | |
altb2 | 0:a56e9c932891 | 18 | |
altb2 | 5:768e10f6d372 | 19 | Serial pc(USBTX, USBRX,115200); |
altb2 | 5:768e10f6d372 | 20 | InterruptIn button(USER_BUTTON); // User Button, short and long presses! |
altb2 | 0:a56e9c932891 | 21 | bool key_was_pressed = false; |
altb2 | 5:768e10f6d372 | 22 | float Ts=.001; // sampling time |
altb2 | 5:768e10f6d372 | 23 | void pressed(void); |
altb2 | 5:768e10f6d372 | 24 | void released(void); |
altb2 | 5:768e10f6d372 | 25 | float vel; |
altb2 | 5:768e10f6d372 | 26 | //------------- DEFINE FILTERS ---------------- |
altb2 | 5:768e10f6d372 | 27 | // missing |
altb2 | 5:768e10f6d372 | 28 | //----------------------------------- |
altb2 | 5:768e10f6d372 | 29 | Timer glob_ti; |
altb2 | 5:768e10f6d372 | 30 | EncoderCounter counter1(PA_6, PC_7); // initialize counter on PA_6 and PC_7 |
altb2 | 5:768e10f6d372 | 31 | InterruptIn indexpulse1(PA_8); |
altb2 | 5:768e10f6d372 | 32 | EncoderCounterIndex index1(counter1,indexpulse1); |
altb2 | 5:768e10f6d372 | 33 | // |
altb2 | 5:768e10f6d372 | 34 | EncoderCounter counter2(PB_6, PB_7); // initialize counter on PB_6 and PB_7 |
altb2 | 5:768e10f6d372 | 35 | InterruptIn indexpulse2(PB_4); |
altb2 | 5:768e10f6d372 | 36 | EncoderCounterIndex index2(counter2,indexpulse2); // initialize counter on PA_6 and PC_7 |
altb2 | 5:768e10f6d372 | 37 | // |
altb2 | 5:768e10f6d372 | 38 | DiffCounter diff1(0.0008,Ts); // discrete differentiate, based on encoder1 data |
altb2 | 5:768e10f6d372 | 39 | DiffCounter diff2(0.0008,Ts); // discrete differentiate, based on encoder2 data |
altb2 | 5:768e10f6d372 | 40 | LinearCharacteristics i2pwm(-1.0,1.0,0.01,0.99,.01,.99); |
altb2 | 5:768e10f6d372 | 41 | // |
altb2 | 5:768e10f6d372 | 42 | PwmOut i_des1(PB_10); |
altb2 | 5:768e10f6d372 | 43 | PwmOut i_des2(PA_15); |
altb2 | 5:768e10f6d372 | 44 | DigitalOut i_enable(PC_4); |
altb2 | 5:768e10f6d372 | 45 | float data[2000][4]; |
altb2 | 5:768e10f6d372 | 46 | Unwrapper_2pi uw2pi; |
altb2 | 0:a56e9c932891 | 47 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 48 | // ----- User defined functions ----------- |
altb2 | 5:768e10f6d372 | 49 | ControllerLoop loop(Ts); |
altb2 | 5:768e10f6d372 | 50 | path_1d p1; |
altb2 | 5:768e10f6d372 | 51 | path_1d p2; |
altb2 | 5:768e10f6d372 | 52 | path_1d *current_path; |
altb2 | 5:768e10f6d372 | 53 | float A = 2.7; |
altb2 | 5:768e10f6d372 | 54 | float dc=0.0; |
altb2 | 5:768e10f6d372 | 55 | //GPA myGPA(1, 2500, 100, 30, 20, Ts); |
altb2 | 5:768e10f6d372 | 56 | //GPA myGPA(5, 2500, 80, 0.3, 0.3, Ts); |
altb2 | 5:768e10f6d372 | 57 | float exc=0.0; // excitation GPA |
altb2 | 5:768e10f6d372 | 58 | // f1 f2 N A1 A2 Ts |
altb2 | 5:768e10f6d372 | 59 | |
altb2 | 5:768e10f6d372 | 60 | // ***************************************************************************** |
altb2 | 5:768e10f6d372 | 61 | void rise_edge(void) |
altb2 | 5:768e10f6d372 | 62 | { |
altb2 | 5:768e10f6d372 | 63 | //glob_ti.reset(); |
altb2 | 5:768e10f6d372 | 64 | } |
altb2 | 5:768e10f6d372 | 65 | // ***************************************************************************** |
altb2 | 5:768e10f6d372 | 66 | void fall_edge(void) |
altb2 | 5:768e10f6d372 | 67 | { |
altb2 | 5:768e10f6d372 | 68 | uint32_t time_us = glob_ti.read_us(); |
altb2 | 5:768e10f6d372 | 69 | dc = (float)time_us/200.0f; |
altb2 | 5:768e10f6d372 | 70 | } |
altb2 | 5:768e10f6d372 | 71 | //InterruptIn i_des2(PA_6); |
altb2 | 5:768e10f6d372 | 72 | |
altb2 | 5:768e10f6d372 | 73 | |
altb2 | 0:a56e9c932891 | 74 | //****************************************************************************** |
altb2 | 0:a56e9c932891 | 75 | //---------- main loop ------------- |
altb2 | 0:a56e9c932891 | 76 | //****************************************************************************** |
altb2 | 0:a56e9c932891 | 77 | int main() |
altb2 | 0:a56e9c932891 | 78 | { |
altb2 | 5:768e10f6d372 | 79 | i_des1.period_us(200); |
altb2 | 5:768e10f6d372 | 80 | i_des2.period_us(200); // PWM Frequency of i_desired |
altb2 | 5:768e10f6d372 | 81 | i_enable = 0; // disable current first |
altb2 | 0:a56e9c932891 | 82 | counter1.reset(); // encoder reset |
altb2 | 5:768e10f6d372 | 83 | counter2.reset(); // encoder reset |
altb2 | 5:768e10f6d372 | 84 | ThisThread::sleep_for(100); |
altb2 | 5:768e10f6d372 | 85 | i_enable = 1; |
altb2 | 5:768e10f6d372 | 86 | |
altb2 | 5:768e10f6d372 | 87 | glob_ti.start(); |
altb2 | 5:768e10f6d372 | 88 | glob_ti.reset(); |
altb2 | 5:768e10f6d372 | 89 | printf("Start Mirror\r\n"); |
altb2 | 5:768e10f6d372 | 90 | p1.initialize(300,10,A,0,0,0); |
altb2 | 5:768e10f6d372 | 91 | p2.initialize(300,10,-A,0,0,A); |
altb2 | 5:768e10f6d372 | 92 | for(int wk =0;wk<10;wk++) |
altb2 | 5:768e10f6d372 | 93 | { |
altb2 | 5:768e10f6d372 | 94 | current_path = &p1; |
altb2 | 5:768e10f6d372 | 95 | current_path->start(glob_ti.read()); |
altb2 | 5:768e10f6d372 | 96 | while(!current_path->finished) |
altb2 | 5:768e10f6d372 | 97 | wait(.1); |
altb2 | 5:768e10f6d372 | 98 | current_path = &p2; |
altb2 | 5:768e10f6d372 | 99 | current_path->start(glob_ti.read()); |
altb2 | 5:768e10f6d372 | 100 | while(!current_path->finished) |
altb2 | 5:768e10f6d372 | 101 | wait(.1); |
altb2 | 5:768e10f6d372 | 102 | ThisThread::sleep_for(100); |
altb2 | 5:768e10f6d372 | 103 | } |
altb2 | 5:768e10f6d372 | 104 | i_enable = 0; |
altb2 | 5:768e10f6d372 | 105 | while(1) |
altb2 | 5:768e10f6d372 | 106 | wait(1); |
altb2 | 5:768e10f6d372 | 107 | |
altb2 | 5:768e10f6d372 | 108 | |
altb2 | 0:a56e9c932891 | 109 | |
altb2 | 0:a56e9c932891 | 110 | } // END OF main |