Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
Diff: main.cpp
- Revision:
- 6:9ebeffe446e4
- Parent:
- 5:768e10f6d372
- Child:
- 7:942fd77d5e19
--- a/main.cpp Thu Feb 25 20:28:45 2021 +0000 +++ b/main.cpp Thu Apr 01 13:38:54 2021 +0000 @@ -13,62 +13,62 @@ #include "path_1d.h" #include "GPA.h" #include "ControllerLoop.h" - - +#include "Mirror_Kinematic.h" +#include "data_structs.h" +#include "uart_comm_thread.h" -Serial pc(USBTX, USBRX,115200); +static BufferedSerial serial_port(USBTX, USBRX); InterruptIn button(USER_BUTTON); // User Button, short and long presses! bool key_was_pressed = false; -float Ts=.001; // sampling time +float Ts=.0002f; // sampling time void pressed(void); void released(void); -float vel; +DigitalOut led1(LED1); +DigitalOut led2(LED2); //------------- DEFINE FILTERS ---------------- // missing -//----------------------------------- -Timer glob_ti; +//------------- 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.0008,Ts); // discrete differentiate, based on encoder1 data -DiffCounter diff2(0.0008,Ts); // discrete differentiate, based on encoder2 data -LinearCharacteristics i2pwm(-1.0,1.0,0.01,0.99,.01,.99); -// -PwmOut i_des1(PB_10); -PwmOut i_des2(PA_15); -DigitalOut i_enable(PC_4); -float data[2000][4]; -Unwrapper_2pi uw2pi; +// ------------------------------------ +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 // ***************************************************************************** -void rise_edge(void) - { - //glob_ti.reset(); - } -// ***************************************************************************** -void fall_edge(void) - { - uint32_t time_us = glob_ti.read_us(); - dc = (float)time_us/200.0f; - } -//InterruptIn i_des2(PA_6); + //****************************************************************************** @@ -76,22 +76,39 @@ //****************************************************************************** int main() { - i_des1.period_us(200); - i_des2.period_us(200); // PWM Frequency of i_desired + 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 - ThisThread::sleep_for(100); - i_enable = 1; - + i_enable = 0; + mc.set_offsets(2956,2343); glob_ti.start(); glob_ti.reset(); - printf("Start Mirror\r\n"); - p1.initialize(300,10,A,0,0,0); + 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); - for(int wk =0;wk<10;wk++) + laser_on = 0; + + + //for(int wk =0;wk<5;wk++) + while(0) { - current_path = &p1; + 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); @@ -100,11 +117,14 @@ 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; + i_enable = 0;*/ while(1) - wait(1); - - + ThisThread::sleep_for(2000); } // END OF main