![](/media/cache/profiles/altb_JGUSAOG.jpg.50x50_q85.jpg)
Mirror actuator for RT2 lab
main.cpp@7:942fd77d5e19, 2021-04-15 (annotated)
- Committer:
- altb2
- Date:
- Thu Apr 15 05:36:55 2021 +0000
- Revision:
- 7:942fd77d5e19
- Parent:
- 6:9ebeffe446e4
- Child:
- 8:49ac75c42da0
intermediate
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 | 6:9ebeffe446e4 | 16 | #include "Mirror_Kinematic.h" |
altb2 | 6:9ebeffe446e4 | 17 | #include "data_structs.h" |
altb2 | 6:9ebeffe446e4 | 18 | #include "uart_comm_thread.h" |
altb2 | 0:a56e9c932891 | 19 | |
altb2 | 6:9ebeffe446e4 | 20 | static BufferedSerial serial_port(USBTX, USBRX); |
altb2 | 5:768e10f6d372 | 21 | InterruptIn button(USER_BUTTON); // User Button, short and long presses! |
altb2 | 0:a56e9c932891 | 22 | bool key_was_pressed = false; |
altb2 | 7:942fd77d5e19 | 23 | float Ts=.0005f; // sampling time |
altb2 | 5:768e10f6d372 | 24 | void pressed(void); |
altb2 | 5:768e10f6d372 | 25 | void released(void); |
altb2 | 7:942fd77d5e19 | 26 | //DigitalOut led1(LED1); |
altb2 | 7:942fd77d5e19 | 27 | //DigitalOut led2(LED2); |
altb2 | 5:768e10f6d372 | 28 | //------------- DEFINE FILTERS ---------------- |
altb2 | 5:768e10f6d372 | 29 | // missing |
altb2 | 6:9ebeffe446e4 | 30 | //------------- Define In/Out ----------------- |
altb2 | 7:942fd77d5e19 | 31 | AnalogOut i_des1(PA_5); |
altb2 | 6:9ebeffe446e4 | 32 | AnalogOut i_des2(PA_4); |
altb2 | 6:9ebeffe446e4 | 33 | DigitalOut i_enable(PC_4); |
altb2 | 6:9ebeffe446e4 | 34 | DigitalOut laser_on(PB_0); |
altb2 | 6:9ebeffe446e4 | 35 | ///------------- Encoder ----------------------- |
altb2 | 5:768e10f6d372 | 36 | EncoderCounter counter1(PA_6, PC_7); // initialize counter on PA_6 and PC_7 |
altb2 | 5:768e10f6d372 | 37 | InterruptIn indexpulse1(PA_8); |
altb2 | 5:768e10f6d372 | 38 | EncoderCounterIndex index1(counter1,indexpulse1); |
altb2 | 6:9ebeffe446e4 | 39 | // ------------------------------------ |
altb2 | 5:768e10f6d372 | 40 | EncoderCounter counter2(PB_6, PB_7); // initialize counter on PB_6 and PB_7 |
altb2 | 5:768e10f6d372 | 41 | InterruptIn indexpulse2(PB_4); |
altb2 | 5:768e10f6d372 | 42 | EncoderCounterIndex index2(counter2,indexpulse2); // initialize counter on PA_6 and PC_7 |
altb2 | 6:9ebeffe446e4 | 43 | // ------------------------------------ |
altb2 | 6:9ebeffe446e4 | 44 | DiffCounter diff1(0.0005f,Ts,4000); // discrete differentiate, based on encoder data |
altb2 | 6:9ebeffe446e4 | 45 | DiffCounter diff2(0.0005f,Ts,4000); // discrete differentiate, based on encoder data |
altb2 | 6:9ebeffe446e4 | 46 | //LinearCharacteristics i2pwm(-1.0,1.0,0.02,0.98,.02,.98); |
altb2 | 6:9ebeffe446e4 | 47 | LinearCharacteristics i2u(-1.0,1.0,0.0,1.0,0.0,1.0); |
altb2 | 6:9ebeffe446e4 | 48 | |
altb2 | 6:9ebeffe446e4 | 49 | Unwrapper_2pi uw2pi1; |
altb2 | 6:9ebeffe446e4 | 50 | Unwrapper_2pi uw2pi2; |
altb2 | 0:a56e9c932891 | 51 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 52 | // ----- User defined functions ----------- |
altb2 | 5:768e10f6d372 | 53 | ControllerLoop loop(Ts); |
altb2 | 7:942fd77d5e19 | 54 | uart_comm_thread uart_com(&serial_port,.1f); |
altb2 | 6:9ebeffe446e4 | 55 | Ticker do_referencing; |
altb2 | 6:9ebeffe446e4 | 56 | Timer glob_ti; |
altb2 | 5:768e10f6d372 | 57 | path_1d p1; |
altb2 | 5:768e10f6d372 | 58 | path_1d p2; |
altb2 | 5:768e10f6d372 | 59 | path_1d *current_path; |
altb2 | 5:768e10f6d372 | 60 | float A = 2.7; |
altb2 | 5:768e10f6d372 | 61 | float dc=0.0; |
altb2 | 7:942fd77d5e19 | 62 | Mirror_Kinematic mk; |
altb2 | 6:9ebeffe446e4 | 63 | DATA_Xchange data; |
altb2 | 6:9ebeffe446e4 | 64 | |
altb2 | 5:768e10f6d372 | 65 | //GPA myGPA(1, 2500, 100, 30, 20, Ts); |
altb2 | 5:768e10f6d372 | 66 | //GPA myGPA(5, 2500, 80, 0.3, 0.3, Ts); |
altb2 | 5:768e10f6d372 | 67 | float exc=0.0; // excitation GPA |
altb2 | 5:768e10f6d372 | 68 | // f1 f2 N A1 A2 Ts |
altb2 | 5:768e10f6d372 | 69 | |
altb2 | 5:768e10f6d372 | 70 | // ***************************************************************************** |
altb2 | 6:9ebeffe446e4 | 71 | |
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 | 6:9ebeffe446e4 | 79 | serial_port.set_baud(115200); |
altb2 | 6:9ebeffe446e4 | 80 | serial_port.set_format( |
altb2 | 6:9ebeffe446e4 | 81 | /* bits */ 8, |
altb2 | 6:9ebeffe446e4 | 82 | /* parity */ BufferedSerial::None, |
altb2 | 6:9ebeffe446e4 | 83 | /* stop bit */ 1 |
altb2 | 6:9ebeffe446e4 | 84 | ); |
altb2 | 6:9ebeffe446e4 | 85 | serial_port.set_blocking(false); // force to send whenever possible and data is there |
altb2 | 5:768e10f6d372 | 86 | i_enable = 0; // disable current first |
altb2 | 0:a56e9c932891 | 87 | counter1.reset(); // encoder reset |
altb2 | 5:768e10f6d372 | 88 | counter2.reset(); // encoder reset |
altb2 | 6:9ebeffe446e4 | 89 | i_enable = 0; |
altb2 | 7:942fd77d5e19 | 90 | mk.set_offsets(0,0); |
altb2 | 5:768e10f6d372 | 91 | glob_ti.start(); |
altb2 | 5:768e10f6d372 | 92 | glob_ti.reset(); |
altb2 | 6:9ebeffe446e4 | 93 | printf("Start Mirroractuator 1.0\r\n"); |
altb2 | 6:9ebeffe446e4 | 94 | loop.init_controllers(); |
altb2 | 6:9ebeffe446e4 | 95 | uart_com.start_uart(); |
altb2 | 6:9ebeffe446e4 | 96 | loop.start_loop(); |
altb2 | 7:942fd77d5e19 | 97 | i_des1.write(i2u(0)); |
altb2 | 6:9ebeffe446e4 | 98 | i_des2.write(i2u(0)); |
altb2 | 6:9ebeffe446e4 | 99 | /* p1.initialize(300,10,A,0,0,0); |
altb2 | 5:768e10f6d372 | 100 | p2.initialize(300,10,-A,0,0,A); |
altb2 | 6:9ebeffe446e4 | 101 | laser_on = 0; |
altb2 | 6:9ebeffe446e4 | 102 | |
altb2 | 6:9ebeffe446e4 | 103 | |
altb2 | 6:9ebeffe446e4 | 104 | //for(int wk =0;wk<5;wk++) |
altb2 | 6:9ebeffe446e4 | 105 | while(0) |
altb2 | 5:768e10f6d372 | 106 | { |
altb2 | 6:9ebeffe446e4 | 107 | short c1 = counter1; // get counts from Encoder |
altb2 | 6:9ebeffe446e4 | 108 | short c2 = counter2; // get counts from Encoder |
altb2 | 6:9ebeffe446e4 | 109 | /*current_path = &p1; |
altb2 | 5:768e10f6d372 | 110 | current_path->start(glob_ti.read()); |
altb2 | 5:768e10f6d372 | 111 | while(!current_path->finished) |
altb2 | 5:768e10f6d372 | 112 | wait(.1); |
altb2 | 5:768e10f6d372 | 113 | current_path = &p2; |
altb2 | 5:768e10f6d372 | 114 | current_path->start(glob_ti.read()); |
altb2 | 5:768e10f6d372 | 115 | while(!current_path->finished) |
altb2 | 5:768e10f6d372 | 116 | wait(.1); |
altb2 | 5:768e10f6d372 | 117 | ThisThread::sleep_for(100); |
altb2 | 6:9ebeffe446e4 | 118 | |
altb2 | 6:9ebeffe446e4 | 119 | // printf("c1,c2: %d %d \r\n",c1,c2); |
altb2 | 6:9ebeffe446e4 | 120 | |
altb2 | 6:9ebeffe446e4 | 121 | wait(.25); |
altb2 | 6:9ebeffe446e4 | 122 | laser_on = !laser_on; |
altb2 | 5:768e10f6d372 | 123 | } |
altb2 | 6:9ebeffe446e4 | 124 | i_enable = 0;*/ |
altb2 | 5:768e10f6d372 | 125 | while(1) |
altb2 | 6:9ebeffe446e4 | 126 | ThisThread::sleep_for(2000); |
altb2 | 0:a56e9c932891 | 127 | |
altb2 | 0:a56e9c932891 | 128 | } // END OF main |