main.cpp

00001 #include "mbed.h"
00002 #include "math.h" 
00003 //------------------------------------------
00004 #define PI 3.1415927f
00005 //------------------------------------------
00006 #include "EncoderCounter.h"
00007 #include "EncoderCounterIndex.h"
00008 #include "DiffCounter.h"
00009 #include "IIR_filter.h"
00010 #include "LinearCharacteristics.h"
00011 #include "PID_Cntrl.h"
00012 #include "Unwrapper_2pi.h"
00013 #include "path_1d.h"
00014 #include "GPA.h"
00015 #include "ControllerLoop.h"
00016 #include "Mirror_Kinematic.h"
00017 #include "data_structs.h"
00018 #include "uart_comm_thread.h"
00019 #include "FastPWM.h"
00020  
00021 static BufferedSerial serial_port(USBTX, USBRX);
00022 DigitalIn big_button(PC_3);         // Enable button an backside
00023 bool key_was_pressed = false;       //
00024 float Ts=.0002f;                    // sampling time
00025 void pressed(void);
00026 void released(void); 
00027 
00028 
00029 //------------- DEFINE FILTERS ----------------
00030 // missing
00031 //------------- Define In/Out -----------------
00032 AnalogOut i_des1(PA_5);
00033 AnalogOut i_des2(PA_4);
00034 //AnalogIn i_act1(PA_3);
00035 //AnalogIn i_act2(PC_0);
00036 //FastPWM i_des1(PB_10);
00037 //FastPWM i_des2(PA_15);
00038 DigitalOut i_enable(PC_4);
00039 DigitalOut laser_on(PB_0);
00040 ///------------- Encoder -----------------------
00041 EncoderCounter counter1(PA_6, PC_7);    // initialize counter on PA_6 and PC_7
00042 InterruptIn indexpulse1(PA_8);
00043 EncoderCounterIndex index1(counter1,indexpulse1);   
00044 // ------------------------------------
00045 EncoderCounter counter2(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
00046 InterruptIn indexpulse2(PB_4);
00047 EncoderCounterIndex index2(counter2,indexpulse2);    // initialize counter on PA_6 and PC_7
00048 // ------------------------------------
00049 DiffCounter diff1(0.0005f,Ts,4000);              // discrete differentiate, based on encoder data
00050 DiffCounter diff2(0.0005f,Ts,4000);              // discrete differentiate, based on encoder data
00051 //LinearCharacteristics i2pwm(-1.0,1.0,0.02,0.98,.02,.98);
00052 LinearCharacteristics i2u(-.80,.80,0.0f,1.0f);
00053 LinearCharacteristics u2i(0.0,1.0,-1.0,1.0);
00054 
00055 Unwrapper_2pi uw2pi1;
00056 Unwrapper_2pi uw2pi2;
00057 //------------------------------------------
00058 // ----- User defined functions -----------
00059 ControllerLoop loop(Ts);                        // this is forthe main controller loop
00060 uart_comm_thread uart_com(&serial_port,.05f);   // this is the communication thread
00061 Timer glob_ti;                                  // the global timer
00062 path_1d p1;             // pathplanner (under constr.)
00063 path_1d p2;             // pathplanner (under constr.)
00064 path_1d *current_path;
00065 // --------- GPA -----------------------------
00066 //init values: f0,   f1, nbPts, A0, A1, Ts
00067 GPA      myGPA(5 , 2400,    40, 60, 50, Ts);
00068 float exc=0.0;              // excitation GPA
00069 //------------------------------------------------------------------------------
00070 // --------- Mirror kinematik, define values, trafos etc there
00071 Mirror_Kinematic mk;
00072 //------------------------------------------------------------------------------
00073 // --------- data: overall data structure for x-change
00074 DATA_Xchange data;
00075 
00076 //******************************************************************************
00077 //---------- main loop -------------
00078 //******************************************************************************
00079 int main()
00080 {
00081     serial_port.set_baud(115200);
00082     serial_port.set_format(
00083         /* bits */ 8,
00084         /* parity */ BufferedSerial::None,
00085         /* stop bit */ 1
00086     );
00087     serial_port.set_blocking(false); // force to send whenever possible and data is there
00088     i_enable = 0;       // disable current first
00089     counter1.reset();   // encoder reset
00090     counter2.reset();   // encoder reset
00091     mk.set_offsets(1473,3257);          // individal set values for global position
00092     glob_ti.start();
00093     glob_ti.reset();
00094     loop.init_controllers();
00095     //uart_com.start_uart();        // communication with MATLAB
00096     printf("hello world \n");
00097     loop.start_loop();
00098     i_des1.write(i2u(0));
00099     i_des2.write(i2u(0));
00100     ThisThread::sleep_for(200);
00101     //uart_com.send_text((char *)"Start Mirroractuator 1.1");   // communciation with MATLAB
00102    /* p1.initialize(300,10,A,0,0,0);
00103     p2.initialize(300,10,-A,0,0,A);*/
00104     laser_on = 0;
00105     //for(int wk =0;wk<5;wk++)
00106     while(0)
00107         {
00108         short c1 = counter1;            // get counts from Encoder
00109         short c2 = counter2;            // get counts from Encoder
00110         current_path = &p1;
00111         current_path->start(glob_ti.read());
00112         while(!current_path->finished)
00113             ThisThread::sleep_for(100);
00114         current_path = &p2;
00115         current_path->start(glob_ti.read());
00116         while(!current_path->finished)
00117             ThisThread::sleep_for(100);
00118         ThisThread::sleep_for(100);
00119         laser_on = !laser_on;
00120         }   // end of while(..)
00121         i_enable = 0;
00122     while(1)
00123         {
00124         ThisThread::sleep_for(200);
00125         }
00126     //  *** create CSV file of data
00127  
00128         /*FILE *f = fopen ("test.csv", "a");
00129 
00130         // inform user
00131         if (!f) {
00132             printf ("failed\n");
00133         } else {
00134             printf ("success\n");
00135         }
00136 
00137         fprintf (f, " test\n");
00138         fclose (f);
00139         return 0;*/ 
00140      
00141 }   // END OF main