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