File content as of revision 4:37df0f6a1bc3:

#include "mbed.h"
#include "EncoderCounter.h"
#include "PID_Cntrl.h"
#include "LinearCharacteristics.h"
#include "GPA.h"
//------------------------------------------
#define PI 3.1415927f
//------------------------------------------

/* GRT: Control of voice-coil with PID-T1 - Controller
 */
Serial pc(SERIAL_TX, SERIAL_RX);        // serial connection via USB - programmer
InterruptIn button(USER_BUTTON);        // User Button, short presses: reduce speed, long presses: increase speed
AnalogOut out(PA_5);                    // Analog OUT on PA_5   1.6 V -> 0A 3.2A -> 4A (see ESCON)

bool key_was_pressed = false;
bool controller_active = false;
void pressed(void);                     // user Button pressed
void released(void);                    // user Button released

//------------------------------------------
// ... here define variables like gains etc.
//------------------------------------------
LinearCharacteristics i2u(-4.0f, 4.0f, 0.0f, 3.2f/3.3f);         // output is normalized output
//------------------------------------------
Ticker  ControllerLoopTimer;            // interrupt for control loop
EncoderCounter counter1(PB_6, PB_7);    // initialize counter on PB_6 and PB_7
Timer t_but;                            // define timer for button
// ----- User defined functions -----------
void updateLoop(void);   // loop for State machine (via interrupt)
float Ts = 0.0005f;                     // sample time of main loop
uint16_t k = 0;
// initiate PIDT1 controller objects
// PID_Cntrl dt1(0.0f, 0.0f, 0.0109f, 0.000625f, Ts, -3.0f, 3.0f);
// PID_Cntrl pi1(1.35f, 13.5f, 0.0f, 1.0f, Ts, -3.0f, 3.0f);
PID_Cntrl dt1(0.0f, 0.0f, 0.0126f, 0.00067f, Ts, -3.0f, 3.0f);
PID_Cntrl pi1(3.16f, 15.1f, 0.0f, 1.0f, Ts, -3.0f, 3.0f);
float w = 0.5f;
/*
// low frequency region until 20 Hz with more amplitude
float fMin = 0.3f;
float fMax = 20.0f;
int   NfexcDes = 60;
float Aexc0 = 3.0f;
float Aexc1 = 0.8f; // Aexc0/fMax;
GPA   gpa(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
float w = 0.0f;
float exc = 0.0f;
*/
/*
// all frequencies with less amplitude (until approx. 20 Hz bad quality)
float fMin = 1.0f;
float fMax = 0.99f/2.0f/Ts;
int   NfexcDes = 200;
float Aexc0 = 0.8f;
float Aexc1 = 0.2f; // Aexc0/fMax;
GPA   gpa(fMin, fMax, NfexcDes, Aexc0, Aexc1, Ts);
float w = 0.0f;
float exc = 0.0f;
*/
//******************************************************************************
//---------- main loop -------------
//******************************************************************************
int main()
{
    pc.baud(115200);   // for serial comm.
    counter1.reset();   // encoder reset
    out.write(i2u(0.0));
    button.fall(&pressed);          // attach key pressed function
    button.rise(&released);         // attach key pressed function
    pc.printf("Start controller now or reset...\r\n");
    ControllerLoopTimer.attach(&updateLoop, Ts); //Assume Fs = ...;
    while(1);
}   // END OF main
//******************************************************************************
//---------- main loop (called via interrupt) -------------
//******************************************************************************
void updateLoop(void)
{
    float x = (float)(counter1)/1000.0f;  // get counts from Encoder
    float i_des = 0.0f;         // default: set motor current to zero (will be overwritten)
    if(controller_active) {
        // controller update
        i_des = pi1(w - x) - dt1(x);
        // i_des = pi1(exc - x) - dt1(x);
        // exc = gpa(i_des, x);
    }
    out.write(i2u(i_des));
    if(++k>1000) {
        pc.printf("x: %1.3f, i: %1.4f\r\n",x,i_des);
        k = 0;
        w = -w;
    }
} // END OF updateLoop(void)

//******************************************************************************


// start timer as soon as Button is pressed
void pressed()
{
    t_but.start();
}
// Falling edge of button: enable/disable controller
void released()
{
    // readout, stop and reset timer
    float ButtonTime = t_but.read();
    t_but.stop();
    t_but.reset();
    if(ButtonTime > 0.05f) {
        controller_active = !controller_active;
        if(controller_active) {
            pc.printf("Controller actived\r\n");
            // reset controller here!!!
            dt1.reset(0.0);
            pi1.reset(0.0);
            // gpa.reset();
            // exc = 0.0f;
        } else
            pc.printf("Controller disabled\r\n");
    }
}