![](/media/cache/profiles/altb_JGUSAOG.jpg.50x50_q85.jpg)
corrections altb2
main.cpp@0:d2e117716219, 2021-05-02 (annotated)
- Committer:
- altb2
- Date:
- Sun May 02 19:32:30 2021 +0000
- Revision:
- 0:d2e117716219
- Child:
- 1:25a2b47ca291
1st commit;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:d2e117716219 | 1 | #include "mbed.h" |
altb2 | 0:d2e117716219 | 2 | #include "math.h" |
altb2 | 0:d2e117716219 | 3 | //------------------------------------------ |
altb2 | 0:d2e117716219 | 4 | #define PI 3.1415927f |
altb2 | 0:d2e117716219 | 5 | //------------------------------------------ |
altb2 | 0:d2e117716219 | 6 | #include "EncoderCounter.h" |
altb2 | 0:d2e117716219 | 7 | #include "EncoderCounterIndex.h" |
altb2 | 0:d2e117716219 | 8 | #include "DiffCounter.h" |
altb2 | 0:d2e117716219 | 9 | #include "IIR_filter.h" |
altb2 | 0:d2e117716219 | 10 | #include "LinearCharacteristics.h" |
altb2 | 0:d2e117716219 | 11 | #include "PID_Cntrl.h" |
altb2 | 0:d2e117716219 | 12 | #include "Unwrapper_2pi.h" |
altb2 | 0:d2e117716219 | 13 | #include "path_1d.h" |
altb2 | 0:d2e117716219 | 14 | #include "GPA.h" |
altb2 | 0:d2e117716219 | 15 | #include "ControllerLoop.h" |
altb2 | 0:d2e117716219 | 16 | #include "Mirror_Kinematic.h" |
altb2 | 0:d2e117716219 | 17 | #include "data_structs.h" |
altb2 | 0:d2e117716219 | 18 | #include "uart_comm_thread.h" |
altb2 | 0:d2e117716219 | 19 | #include "FastPWM.h" |
altb2 | 0:d2e117716219 | 20 | |
altb2 | 0:d2e117716219 | 21 | static BufferedSerial serial_port(USBTX, USBRX); |
altb2 | 0:d2e117716219 | 22 | DigitalIn big_button(PC_3); // Enable button an backside |
altb2 | 0:d2e117716219 | 23 | bool key_was_pressed = false; // |
altb2 | 0:d2e117716219 | 24 | float Ts=.0002f; // sampling time |
altb2 | 0:d2e117716219 | 25 | void pressed(void); |
altb2 | 0:d2e117716219 | 26 | void released(void); |
altb2 | 0:d2e117716219 | 27 | //------------- DEFINE FILTERS ---------------- |
altb2 | 0:d2e117716219 | 28 | // missing |
altb2 | 0:d2e117716219 | 29 | //------------- Define In/Out ----------------- |
altb2 | 0:d2e117716219 | 30 | AnalogOut i_des1(PA_5); |
altb2 | 0:d2e117716219 | 31 | AnalogOut i_des2(PA_4); |
altb2 | 0:d2e117716219 | 32 | //AnalogIn i_act1(PA_3); |
altb2 | 0:d2e117716219 | 33 | //AnalogIn i_act2(PC_0); |
altb2 | 0:d2e117716219 | 34 | //FastPWM i_des1(PB_10); |
altb2 | 0:d2e117716219 | 35 | //FastPWM i_des2(PA_15); |
altb2 | 0:d2e117716219 | 36 | DigitalOut i_enable(PC_4); |
altb2 | 0:d2e117716219 | 37 | DigitalOut laser_on(PB_0); |
altb2 | 0:d2e117716219 | 38 | ///------------- Encoder ----------------------- |
altb2 | 0:d2e117716219 | 39 | EncoderCounter counter1(PA_6, PC_7); // initialize counter on PA_6 and PC_7 |
altb2 | 0:d2e117716219 | 40 | InterruptIn indexpulse1(PA_8); |
altb2 | 0:d2e117716219 | 41 | EncoderCounterIndex index1(counter1,indexpulse1); |
altb2 | 0:d2e117716219 | 42 | // ------------------------------------ |
altb2 | 0:d2e117716219 | 43 | EncoderCounter counter2(PB_6, PB_7); // initialize counter on PB_6 and PB_7 |
altb2 | 0:d2e117716219 | 44 | InterruptIn indexpulse2(PB_4); |
altb2 | 0:d2e117716219 | 45 | EncoderCounterIndex index2(counter2,indexpulse2); // initialize counter on PA_6 and PC_7 |
altb2 | 0:d2e117716219 | 46 | // ------------------------------------ |
altb2 | 0:d2e117716219 | 47 | DiffCounter diff1(0.0005f,Ts,4000); // discrete differentiate, based on encoder data |
altb2 | 0:d2e117716219 | 48 | DiffCounter diff2(0.0005f,Ts,4000); // discrete differentiate, based on encoder data |
altb2 | 0:d2e117716219 | 49 | //LinearCharacteristics i2pwm(-1.0,1.0,0.02,0.98,.02,.98); |
altb2 | 0:d2e117716219 | 50 | LinearCharacteristics i2u(-.80,.80,0.0f,1.0f); |
altb2 | 0:d2e117716219 | 51 | LinearCharacteristics u2i(0.0,1.0,-1.0,1.0); |
altb2 | 0:d2e117716219 | 52 | |
altb2 | 0:d2e117716219 | 53 | Unwrapper_2pi uw2pi1; |
altb2 | 0:d2e117716219 | 54 | Unwrapper_2pi uw2pi2; |
altb2 | 0:d2e117716219 | 55 | //------------------------------------------ |
altb2 | 0:d2e117716219 | 56 | // ----- User defined functions ----------- |
altb2 | 0:d2e117716219 | 57 | ControllerLoop loop(Ts); // this is forthe main controller loop |
altb2 | 0:d2e117716219 | 58 | uart_comm_thread uart_com(&serial_port,.05f); // this is the communication thread |
altb2 | 0:d2e117716219 | 59 | Timer glob_ti; // the global timer |
altb2 | 0:d2e117716219 | 60 | path_1d p1; // pathplanner (under constr.) |
altb2 | 0:d2e117716219 | 61 | path_1d p2; // pathplanner (under constr.) |
altb2 | 0:d2e117716219 | 62 | path_1d *current_path; |
altb2 | 0:d2e117716219 | 63 | // --------- GPA ----------------------------- |
altb2 | 0:d2e117716219 | 64 | //init values: f0, f1, nbPts, A0, A1, Ts |
altb2 | 0:d2e117716219 | 65 | GPA myGPA(5 , 2400, 40, 25, 25, Ts); |
altb2 | 0:d2e117716219 | 66 | float exc=0.0; // excitation GPA |
altb2 | 0:d2e117716219 | 67 | //------------------------------------------------------------------------------ |
altb2 | 0:d2e117716219 | 68 | // --------- Mirror kinematik, define values, trafos etc there |
altb2 | 0:d2e117716219 | 69 | Mirror_Kinematic mk; |
altb2 | 0:d2e117716219 | 70 | //------------------------------------------------------------------------------ |
altb2 | 0:d2e117716219 | 71 | // --------- data: overall data structure for x-change |
altb2 | 0:d2e117716219 | 72 | DATA_Xchange data; |
altb2 | 0:d2e117716219 | 73 | |
altb2 | 0:d2e117716219 | 74 | //****************************************************************************** |
altb2 | 0:d2e117716219 | 75 | //---------- main loop ------------- |
altb2 | 0:d2e117716219 | 76 | //****************************************************************************** |
altb2 | 0:d2e117716219 | 77 | int main() |
altb2 | 0:d2e117716219 | 78 | { |
altb2 | 0:d2e117716219 | 79 | serial_port.set_baud(115200); |
altb2 | 0:d2e117716219 | 80 | serial_port.set_format( |
altb2 | 0:d2e117716219 | 81 | /* bits */ 8, |
altb2 | 0:d2e117716219 | 82 | /* parity */ BufferedSerial::None, |
altb2 | 0:d2e117716219 | 83 | /* stop bit */ 1 |
altb2 | 0:d2e117716219 | 84 | ); |
altb2 | 0:d2e117716219 | 85 | serial_port.set_blocking(false); // force to send whenever possible and data is there |
altb2 | 0:d2e117716219 | 86 | i_enable = 0; // disable current first |
altb2 | 0:d2e117716219 | 87 | counter1.reset(); // encoder reset |
altb2 | 0:d2e117716219 | 88 | counter2.reset(); // encoder reset |
altb2 | 0:d2e117716219 | 89 | mk.set_offsets(0,0); // individal set values for global position |
altb2 | 0:d2e117716219 | 90 | glob_ti.start(); |
altb2 | 0:d2e117716219 | 91 | glob_ti.reset(); |
altb2 | 0:d2e117716219 | 92 | loop.init_controllers(); |
altb2 | 0:d2e117716219 | 93 | uart_com.start_uart(); |
altb2 | 0:d2e117716219 | 94 | loop.start_loop(); |
altb2 | 0:d2e117716219 | 95 | i_des1.write(i2u(0)); |
altb2 | 0:d2e117716219 | 96 | i_des2.write(i2u(0)); |
altb2 | 0:d2e117716219 | 97 | ThisThread::sleep_for(200); |
altb2 | 0:d2e117716219 | 98 | uart_com.send_text((char *)"Start Mirroractuator 1.1"); |
altb2 | 0:d2e117716219 | 99 | /* p1.initialize(300,10,A,0,0,0); |
altb2 | 0:d2e117716219 | 100 | p2.initialize(300,10,-A,0,0,A);*/ |
altb2 | 0:d2e117716219 | 101 | laser_on = 0; |
altb2 | 0:d2e117716219 | 102 | //for(int wk =0;wk<5;wk++) |
altb2 | 0:d2e117716219 | 103 | while(0) |
altb2 | 0:d2e117716219 | 104 | { |
altb2 | 0:d2e117716219 | 105 | short c1 = counter1; // get counts from Encoder |
altb2 | 0:d2e117716219 | 106 | short c2 = counter2; // get counts from Encoder |
altb2 | 0:d2e117716219 | 107 | current_path = &p1; |
altb2 | 0:d2e117716219 | 108 | current_path->start(glob_ti.read()); |
altb2 | 0:d2e117716219 | 109 | while(!current_path->finished) |
altb2 | 0:d2e117716219 | 110 | ThisThread::sleep_for(100); |
altb2 | 0:d2e117716219 | 111 | current_path = &p2; |
altb2 | 0:d2e117716219 | 112 | current_path->start(glob_ti.read()); |
altb2 | 0:d2e117716219 | 113 | while(!current_path->finished) |
altb2 | 0:d2e117716219 | 114 | ThisThread::sleep_for(100); |
altb2 | 0:d2e117716219 | 115 | ThisThread::sleep_for(100); |
altb2 | 0:d2e117716219 | 116 | laser_on = !laser_on; |
altb2 | 0:d2e117716219 | 117 | } // end of while(..) |
altb2 | 0:d2e117716219 | 118 | i_enable = 0; |
altb2 | 0:d2e117716219 | 119 | while(1) |
altb2 | 0:d2e117716219 | 120 | { |
altb2 | 0:d2e117716219 | 121 | ThisThread::sleep_for(200); |
altb2 | 0:d2e117716219 | 122 | } |
altb2 | 0:d2e117716219 | 123 | } // END OF main |