Mirror actuator for RT2 lab
Dependencies: Library_Cntrl Library_Misc
main.cpp
- Committer:
- altb2
- Date:
- 2021-02-25
- Revision:
- 5:768e10f6d372
- Parent:
- 4:3d8dd3d17564
- Child:
- 6:9ebeffe446e4
File content as of revision 5:768e10f6d372:
#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" Serial pc(USBTX, USBRX,115200); InterruptIn button(USER_BUTTON); // User Button, short and long presses! bool key_was_pressed = false; float Ts=.001; // sampling time void pressed(void); void released(void); float vel; //------------- DEFINE FILTERS ---------------- // missing //----------------------------------- Timer glob_ti; 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; //------------------------------------------ // ----- User defined functions ----------- ControllerLoop loop(Ts); path_1d p1; path_1d p2; path_1d *current_path; float A = 2.7; float dc=0.0; //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); //****************************************************************************** //---------- main loop ------------- //****************************************************************************** int main() { i_des1.period_us(200); i_des2.period_us(200); // PWM Frequency of i_desired i_enable = 0; // disable current first counter1.reset(); // encoder reset counter2.reset(); // encoder reset ThisThread::sleep_for(100); i_enable = 1; glob_ti.start(); glob_ti.reset(); printf("Start Mirror\r\n"); p1.initialize(300,10,A,0,0,0); p2.initialize(300,10,-A,0,0,A); for(int wk =0;wk<10;wk++) { 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); } i_enable = 0; while(1) wait(1); } // END OF main