Ruprecht Altenburger / Mbed OS mirror_actuator_stud

template for students for mirror actuator

Dependencies:   FastPWM

main.cpp@3:d672a96eeecc, 2022-03-06 (annotated)

Committer:
altb2
Date:
Sun Mar 06 08:57:28 2022 +0000
Revision:
3:d672a96eeecc
Parent:
2:c4c4cc1bff45 
MirrorActuator Stud FS21

Who changed what in which revision?

UserRevisionLine numberNew 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 1:a7fc1afe0575 65 GPA myGPA(5 , 2400, 40, 60, 50, Ts);
altb2 0:d2e117716219 66 //------------------------------------------------------------------------------
altb2 0:d2e117716219 67 // --------- Mirror kinematik, define values, trafos etc there
altb2 0:d2e117716219 68 Mirror_Kinematic mk;
altb2 0:d2e117716219 69 //------------------------------------------------------------------------------
altb2 0:d2e117716219 70 // --------- data: overall data structure for x-change
altb2 0:d2e117716219 71 DATA_Xchange data;
altb2 0:d2e117716219 72
altb2 0:d2e117716219 73 //******************************************************************************
altb2 0:d2e117716219 74 //---------- main loop -------------
altb2 0:d2e117716219 75 //******************************************************************************
altb2 2:c4c4cc1bff45 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 2:c4c4cc1bff45 89 mk.set_offsets(982,-167); // individal set values for global position
altb2 2:c4c4cc1bff45 90 mk.trafo_is_on = true;
altb2 0:d2e117716219 91 glob_ti.start();
altb2 0:d2e117716219 92 glob_ti.reset();
altb2 0:d2e117716219 93 loop.init_controllers();
altb2 0:d2e117716219 94 uart_com.start_uart();
altb2 0:d2e117716219 95 loop.start_loop();
altb2 0:d2e117716219 96 i_des1.write(i2u(0));
altb2 0:d2e117716219 97 i_des2.write(i2u(0));
altb2 0:d2e117716219 98 ThisThread::sleep_for(200);
altb2 1:a7fc1afe0575 99 uart_com.send_text((char *)"Start Mirroractuator 1.2");
altb2 0:d2e117716219 100 /* p1.initialize(300,10,A,0,0,0);
altb2 0:d2e117716219 101 p2.initialize(300,10,-A,0,0,A);*/
altb2 2:c4c4cc1bff45 102 laser_on = true;
altb2 0:d2e117716219 103 //for(int wk =0;wk<5;wk++)
altb2 0:d2e117716219 104 while(0)
altb2 0:d2e117716219 105 {
altb2 0:d2e117716219 106 short c1 = counter1; // get counts from Encoder
altb2 0:d2e117716219 107 short c2 = counter2; // get counts from Encoder
altb2 0:d2e117716219 108 current_path = &p1;
altb2 0:d2e117716219 109 current_path->start(glob_ti.read());
altb2 0:d2e117716219 110 while(!current_path->finished)
altb2 0:d2e117716219 111 ThisThread::sleep_for(100);
altb2 0:d2e117716219 112 current_path = &p2;
altb2 0:d2e117716219 113 current_path->start(glob_ti.read());
altb2 0:d2e117716219 114 while(!current_path->finished)
altb2 0:d2e117716219 115 ThisThread::sleep_for(100);
altb2 0:d2e117716219 116 ThisThread::sleep_for(100);
altb2 0:d2e117716219 117 laser_on = !laser_on;
altb2 0:d2e117716219 118 } // end of while(..)
altb2 0:d2e117716219 119 i_enable = 0;
altb2 0:d2e117716219 120 while(1)
altb2 0:d2e117716219 121 {
altb2 0:d2e117716219 122 ThisThread::sleep_for(200);
altb2 0:d2e117716219 123 }
altb2 0:d2e117716219 124 } // END OF main