Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
main.cpp
- Committer:
- altb2
- Date:
- 2021-04-01
- Revision:
- 6:9ebeffe446e4
- Parent:
- 5:768e10f6d372
- Child:
- 7:942fd77d5e19
File content as of revision 6:9ebeffe446e4:
#include "mbed.h" #include "math.h" //------------------------------------------ #define PI 3.1415927f //------------------------------------------ #include "EncoderCounter.h" #include "EncoderCounterIndex.h" #include "DiffCounter.h" #include "IIR_filter.h" #include "LinearCharacteristics.h" #include "PID_Cntrl.h" #include "Unwrapper_2pi.h" #include "path_1d.h" #include "GPA.h" #include "ControllerLoop.h" #include "Mirror_Kinematic.h" #include "data_structs.h" #include "uart_comm_thread.h" static BufferedSerial serial_port(USBTX, USBRX); InterruptIn button(USER_BUTTON); // User Button, short and long presses! bool key_was_pressed = false; float Ts=.0002f; // sampling time void pressed(void); void released(void); DigitalOut led1(LED1); DigitalOut led2(LED2); //------------- DEFINE FILTERS ---------------- // missing //------------- Define In/Out ----------------- //AnalogOut i_des1(PA_5); AnalogOut i_des2(PA_4); DigitalOut i_enable(PC_4); DigitalOut laser_on(PB_0); ///------------- Encoder ----------------------- EncoderCounter counter1(PA_6, PC_7); // initialize counter on PA_6 and PC_7 InterruptIn indexpulse1(PA_8); EncoderCounterIndex index1(counter1,indexpulse1); // ------------------------------------ EncoderCounter counter2(PB_6, PB_7); // initialize counter on PB_6 and PB_7 InterruptIn indexpulse2(PB_4); EncoderCounterIndex index2(counter2,indexpulse2); // initialize counter on PA_6 and PC_7 // ------------------------------------ DiffCounter diff1(0.0005f,Ts,4000); // discrete differentiate, based on encoder data DiffCounter diff2(0.0005f,Ts,4000); // discrete differentiate, based on encoder data //LinearCharacteristics i2pwm(-1.0,1.0,0.02,0.98,.02,.98); LinearCharacteristics i2u(-1.0,1.0,0.0,1.0,0.0,1.0); Unwrapper_2pi uw2pi1; Unwrapper_2pi uw2pi2; //------------------------------------------ // ----- User defined functions ----------- ControllerLoop loop(Ts); uart_comm_thread uart_com(&serial_port,.5f); Ticker do_referencing; Timer glob_ti; path_1d p1; path_1d p2; path_1d *current_path; float A = 2.7; float dc=0.0; Mirror_Kinematic mc; DATA_Xchange data; //GPA myGPA(1, 2500, 100, 30, 20, Ts); //GPA myGPA(5, 2500, 80, 0.3, 0.3, Ts); float exc=0.0; // excitation GPA // f1 f2 N A1 A2 Ts // ***************************************************************************** //****************************************************************************** //---------- main loop ------------- //****************************************************************************** int main() { serial_port.set_baud(115200); serial_port.set_format( /* bits */ 8, /* parity */ BufferedSerial::None, /* stop bit */ 1 ); serial_port.set_blocking(false); // force to send whenever possible and data is there i_enable = 0; // disable current first counter1.reset(); // encoder reset counter2.reset(); // encoder reset i_enable = 0; mc.set_offsets(2956,2343); glob_ti.start(); glob_ti.reset(); printf("Start Mirroractuator 1.0\r\n"); loop.init_controllers(); uart_com.start_uart(); loop.start_loop(); //i_des1.write(i2u(0)); i_des2.write(i2u(0)); led1 = 1; led2 = 1; /* p1.initialize(300,10,A,0,0,0); p2.initialize(300,10,-A,0,0,A); laser_on = 0; //for(int wk =0;wk<5;wk++) while(0) { short c1 = counter1; // get counts from Encoder short c2 = counter2; // get counts from Encoder /*current_path = &p1; current_path->start(glob_ti.read()); while(!current_path->finished) wait(.1); current_path = &p2; current_path->start(glob_ti.read()); while(!current_path->finished) wait(.1); ThisThread::sleep_for(100); // printf("c1,c2: %d %d \r\n",c1,c2); wait(.25); laser_on = !laser_on; } i_enable = 0;*/ while(1) ThisThread::sleep_for(2000); } // END OF main