Ruprecht Altenburger
/
simple_program
Simple program for introduction of mirror actuator.
main.cpp@10:bfacffec199a, 2021-04-28 (annotated)
- Committer:
- altb2
- Date:
- Wed Apr 28 08:26:37 2021 +0000
- Revision:
- 10:bfacffec199a
- Parent:
- 8:49ac75c42da0
- Child:
- 11:d43f8b421d6d
template 1;
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 | 8:49ac75c42da0 | 19 | #include "FastPWM.h" |
altb2 | 0:a56e9c932891 | 20 | |
altb2 | 6:9ebeffe446e4 | 21 | static BufferedSerial serial_port(USBTX, USBRX); |
altb2 | 10:bfacffec199a | 22 | DigitalIn big_button(PC_3); // Enable button an backside |
altb2 | 10:bfacffec199a | 23 | bool key_was_pressed = false; // |
altb2 | 10:bfacffec199a | 24 | float Ts=.0002f; // sampling time |
altb2 | 5:768e10f6d372 | 25 | void pressed(void); |
altb2 | 5:768e10f6d372 | 26 | void released(void); |
altb2 | 5:768e10f6d372 | 27 | //------------- DEFINE FILTERS ---------------- |
altb2 | 5:768e10f6d372 | 28 | // missing |
altb2 | 6:9ebeffe446e4 | 29 | //------------- Define In/Out ----------------- |
altb2 | 7:942fd77d5e19 | 30 | AnalogOut i_des1(PA_5); |
altb2 | 6:9ebeffe446e4 | 31 | AnalogOut i_des2(PA_4); |
altb2 | 8:49ac75c42da0 | 32 | //AnalogIn i_act1(PA_3); |
altb2 | 8:49ac75c42da0 | 33 | //AnalogIn i_act2(PC_0); |
altb2 | 8:49ac75c42da0 | 34 | //FastPWM i_des1(PB_10); |
altb2 | 8:49ac75c42da0 | 35 | //FastPWM i_des2(PA_15); |
altb2 | 6:9ebeffe446e4 | 36 | DigitalOut i_enable(PC_4); |
altb2 | 6:9ebeffe446e4 | 37 | DigitalOut laser_on(PB_0); |
altb2 | 6:9ebeffe446e4 | 38 | ///------------- Encoder ----------------------- |
altb2 | 5:768e10f6d372 | 39 | EncoderCounter counter1(PA_6, PC_7); // initialize counter on PA_6 and PC_7 |
altb2 | 5:768e10f6d372 | 40 | InterruptIn indexpulse1(PA_8); |
altb2 | 5:768e10f6d372 | 41 | EncoderCounterIndex index1(counter1,indexpulse1); |
altb2 | 6:9ebeffe446e4 | 42 | // ------------------------------------ |
altb2 | 5:768e10f6d372 | 43 | EncoderCounter counter2(PB_6, PB_7); // initialize counter on PB_6 and PB_7 |
altb2 | 5:768e10f6d372 | 44 | InterruptIn indexpulse2(PB_4); |
altb2 | 5:768e10f6d372 | 45 | EncoderCounterIndex index2(counter2,indexpulse2); // initialize counter on PA_6 and PC_7 |
altb2 | 6:9ebeffe446e4 | 46 | // ------------------------------------ |
altb2 | 6:9ebeffe446e4 | 47 | DiffCounter diff1(0.0005f,Ts,4000); // discrete differentiate, based on encoder data |
altb2 | 6:9ebeffe446e4 | 48 | DiffCounter diff2(0.0005f,Ts,4000); // discrete differentiate, based on encoder data |
altb2 | 6:9ebeffe446e4 | 49 | //LinearCharacteristics i2pwm(-1.0,1.0,0.02,0.98,.02,.98); |
altb2 | 10:bfacffec199a | 50 | LinearCharacteristics i2u(-.80,.80,0.0f,1.0f); |
altb2 | 8:49ac75c42da0 | 51 | LinearCharacteristics u2i(0.0,1.0,-1.0,1.0); |
altb2 | 6:9ebeffe446e4 | 52 | |
altb2 | 6:9ebeffe446e4 | 53 | Unwrapper_2pi uw2pi1; |
altb2 | 6:9ebeffe446e4 | 54 | Unwrapper_2pi uw2pi2; |
altb2 | 0:a56e9c932891 | 55 | //------------------------------------------ |
altb2 | 0:a56e9c932891 | 56 | // ----- User defined functions ----------- |
altb2 | 10:bfacffec199a | 57 | ControllerLoop loop(Ts); // this is forthe main controller loop |
altb2 | 10:bfacffec199a | 58 | uart_comm_thread uart_com(&serial_port,.05f); // this is the communication thread |
altb2 | 10:bfacffec199a | 59 | Timer glob_ti; // the global timer |
altb2 | 10:bfacffec199a | 60 | path_1d p1; // pathplanner (under constr.) |
altb2 | 10:bfacffec199a | 61 | path_1d p2; // pathplanner (under constr.) |
altb2 | 5:768e10f6d372 | 62 | path_1d *current_path; |
altb2 | 10:bfacffec199a | 63 | // --------- GPA ----------------------------- |
altb2 | 10:bfacffec199a | 64 | //init values: f0, f1, nbPts, A0, A1, Ts |
altb2 | 10:bfacffec199a | 65 | GPA myGPA(5 , 2400, 40, 25, 25, Ts); |
altb2 | 10:bfacffec199a | 66 | float exc=0.0; // excitation GPA |
altb2 | 10:bfacffec199a | 67 | //------------------------------------------------------------------------------ |
altb2 | 10:bfacffec199a | 68 | // --------- Mirror kinematik, define values, trafos etc there |
altb2 | 7:942fd77d5e19 | 69 | Mirror_Kinematic mk; |
altb2 | 10:bfacffec199a | 70 | //------------------------------------------------------------------------------ |
altb2 | 10:bfacffec199a | 71 | // --------- data: overall data structure for x-change |
altb2 | 6:9ebeffe446e4 | 72 | DATA_Xchange data; |
altb2 | 6:9ebeffe446e4 | 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 | 10:bfacffec199a | 86 | i_enable = 0; // disable current first |
altb2 | 0:a56e9c932891 | 87 | counter1.reset(); // encoder reset |
altb2 | 5:768e10f6d372 | 88 | counter2.reset(); // encoder reset |
altb2 | 10:bfacffec199a | 89 | mk.set_offsets(1401,1510); // individal set values for global position |
altb2 | 5:768e10f6d372 | 90 | glob_ti.start(); |
altb2 | 5:768e10f6d372 | 91 | glob_ti.reset(); |
altb2 | 6:9ebeffe446e4 | 92 | loop.init_controllers(); |
altb2 | 10:bfacffec199a | 93 | uart_com.start_uart(); |
altb2 | 10:bfacffec199a | 94 | loop.start_loop(); |
altb2 | 7:942fd77d5e19 | 95 | i_des1.write(i2u(0)); |
altb2 | 6:9ebeffe446e4 | 96 | i_des2.write(i2u(0)); |
altb2 | 8:49ac75c42da0 | 97 | ThisThread::sleep_for(200); |
altb2 | 8:49ac75c42da0 | 98 | uart_com.send_text((char *)"Start Mirroractuator 1.0"); |
altb2 | 6:9ebeffe446e4 | 99 | /* p1.initialize(300,10,A,0,0,0); |
altb2 | 10:bfacffec199a | 100 | p2.initialize(300,10,-A,0,0,A);*/ |
altb2 | 6:9ebeffe446e4 | 101 | laser_on = 0; |
altb2 | 6:9ebeffe446e4 | 102 | //for(int wk =0;wk<5;wk++) |
altb2 | 6:9ebeffe446e4 | 103 | while(0) |
altb2 | 5:768e10f6d372 | 104 | { |
altb2 | 6:9ebeffe446e4 | 105 | short c1 = counter1; // get counts from Encoder |
altb2 | 6:9ebeffe446e4 | 106 | short c2 = counter2; // get counts from Encoder |
altb2 | 10:bfacffec199a | 107 | current_path = &p1; |
altb2 | 5:768e10f6d372 | 108 | current_path->start(glob_ti.read()); |
altb2 | 5:768e10f6d372 | 109 | while(!current_path->finished) |
altb2 | 5:768e10f6d372 | 110 | wait(.1); |
altb2 | 5:768e10f6d372 | 111 | current_path = &p2; |
altb2 | 5:768e10f6d372 | 112 | current_path->start(glob_ti.read()); |
altb2 | 5:768e10f6d372 | 113 | while(!current_path->finished) |
altb2 | 5:768e10f6d372 | 114 | wait(.1); |
altb2 | 5:768e10f6d372 | 115 | ThisThread::sleep_for(100); |
altb2 | 6:9ebeffe446e4 | 116 | wait(.25); |
altb2 | 10:bfacffec199a | 117 | laser_on = !laser_on; |
altb2 | 10:bfacffec199a | 118 | } // end of while(..) |
altb2 | 10:bfacffec199a | 119 | i_enable = 0; |
altb2 | 10:bfacffec199a | 120 | while(1) |
altb2 | 10:bfacffec199a | 121 | { |
altb2 | 10:bfacffec199a | 122 | ThisThread::sleep_for(200); |
altb2 | 5:768e10f6d372 | 123 | } |
altb2 | 0:a56e9c932891 | 124 | } // END OF main |