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