File content as of revision 3:5cb5f0f22c5a:

//#includes--------------------------------------------------------------------------------------------------------
#include "mbed.h"
#include "MODSERIAL.h" //for more efficient serial comm
#include "FastPWM.h" //To fix a bug in the standard mbed lib concerning pwm
#include "QEI.h"

//#efines--------------------------------------------------------------------------------------------------------
#define PWMCYCLETIMEUS 50 //20 kHz PWM cycle time
#define MAX_ENCODER_POS_RIGHT -1453 //Encoder counts at right end-stop
#define MAX_ENCODER_POS_LEFT 1333 //Encoder counts at left end-stop
#define LOOPDURATIONUS 200 //microsec
#define LOOPDURATION ((float)LOOPDURATIONUS*0.000001f) //sec
#define LEDPERIODS 1000 //How many loops before changing the LED state
#define CALIBPWM 0.085f //12.5% of max PWM is the calibration PWM
#define ENCODER_CPR 2048.0f //Counts per revolution for the encode (1x quad, to avoid missing counts)
#define TWO_PI 6.283185307179586476925286766559f //rad
#define WORKSPACEBOUND 7.0f //workspace bound for admittane model
#define MAXVEL 100.0f //maximal admittance control velocity in rad/s
#define FORCESENSORGAIN 15.134f //N per measured unit

//Low pass filter filter coeffs, 2nd order, 200 Hz
double b[3] = {0.003621681514929, 0.007243363029857, 0.003621681514929};
double a[3] = {1.00000000000000, -1.822694925196308, 0.837181651256023};

//Pins and interrupts--------------------------------------------------------------------------------------------------------
// Remember that pins D4,D5,D6,D7 are internally connected to the drivers! Do not use D6 and D7.
Ticker mainLoop; //Main loop runs at fixed fequency

//Inputs
AnalogIn    measuredForce(A5);  //Pin that measures analog force

QEI enc(PTC12,PTC4,NC,1024,QEI::X2_ENCODING);

//Outputs
DigitalOut  motorDir(D4);       //Motor Direction Pin
FastPWM     motorPWM(D5);       //Motor PWM out pin

//Communication for teleoperation
//Ethernet eth;

//For DBG
DigitalOut  dbgPin(D1);         //Possible debug pin to check with scope
MODSERIAL   pc(USBTX, USBRX);   //Serial communication with host PC
DigitalOut  ledRed(LED_RED);    //LED RED
DigitalOut  ledBlue(LED_BLUE);  //LED BLUE
DigitalOut  ledGreen(LED_GREEN);//LED GREEN

//Enums--------------------------------------------------------------------------------------------------------
enum States {   stateIdle,                  stateCalibration,       stateHoming,            stateOPImpedance,
                stateOPAdmittance,          stateOPTeleOpPosPos,    stateOPTeleOpImpedance,
                stateOPTeleOpAdmittance,    stateFailed=99
            }; //System states
enum LedColors { colorCyan, colorMagenta, colorBlue, colorYellow, colorGreen, colorRed, colorBlack, colorWhite}; //Possible LED colors

//Global variables--------------------------------------------------------------------------------------------------------
//General
long loopCounter = 0; //How many loops have passed. Will wrap around after 2^32-1
int currentState = stateIdle; //Current state of state machine
int statePrev = -1; //Previous state of state machine
float stateTime = 0.0f; //How long have we been in this state?
int ledCnt = 0; //Loops for LED state
bool ledState = false; //Current LED state (on/off = true/false)
Timer t; // Main timer to measure time

//Controller references
float refPos = 0.0f; //System reference position
float refVel = 0.0f; //System reference velocity
float refAcc = 0.0f; //System reference acceleration
float u = 0.0f; //Control signal (PWM duty cycle)

//Controller gains
const float K_p = 4.5f; //Proportional position gain
const float K_d = 0.03f; //Differential position gain
const float K_i = 0.000f; //Integral position gain
const float I_ff = 0.0000; //Feed-forward mass
float posError = 0.0f;
float velError = 0.0f;
float integratedError = 0.0f;

//Virtual model properties
const float virtualMass = 0.0015f;
const float virtualStiffness = 50.0f;
const float virtualDamping = 3.0f;
const float springPos = 0.0f;

//Measured variables
float motorPos = 0.0f;
float motorPos_prev = 0.0f;
float motorVel = 0.0f;
float force = 0.0f;
long encoderPos = 0; //Current encoder position
long encoderPos_prev = 0; //Previous sample encoder position
float encoderVel = 0.0f; // Estimated encoder velocity

//DEbug and communication variables (unused?)
char buffer[0x80]; //128 byte buffer (necessary?)

//Functions--------------------------------------------------------------------------------------------------------

//Velocity filtering
double velocityFilter(float x)
{
    double y = 0.0; //Output
    static double y_1 = 0.0; //Output last loop
    static double y_2 = 0.0; //Output 2 loops ago
    static double x_1 = 0.0; //Input last loop
    static double x_2 = 0.0; //Input two loops ago

    //Finite difference equation for 2nd order Butterworth low-pass
    y = -a[1]*y_1 - a[2]*y_2 + x*b[0] + x_1*b[1] + x_2*b[2];
    y_2 = y_1;
    y_1 = y;
    x_2 = x_1;
    x_1 = x;
    return (float)y;
}

//Limit a function, passed by reference
void limit(float &x, float lower, float upper)
{
    if (x > upper)
        x = upper;
    if (x < lower)
        x = lower;
}

//Set led colors
void setLed(int ledColorArg)
{
    switch(ledColorArg) {
        case colorCyan:
            ledRed = 1;
            ledGreen = 0;
            ledBlue = 0;
            break;
        case colorMagenta:
            ledRed = 0;
            ledGreen = 1;
            ledBlue = 0;
            break;
        case colorBlue:
            ledRed = 1;
            ledGreen = 1;
            ledBlue = 0;
            break;
        case colorYellow:
            ledRed = 0;
            ledGreen = 0;
            ledBlue = 1;
            break;
        case colorGreen:
            ledRed = 1;
            ledGreen = 0;
            ledBlue = 1;
            break;
        case colorRed:
            ledRed = 0;
            ledGreen = 1;
            ledBlue = 1;
            break;
        case colorBlack:
            ledRed = 1;
            ledGreen = 1;
            ledBlue = 1;
            break;
        case colorWhite:
            ledRed = 0;
            ledGreen = 0;
            ledBlue = 0;
            break;
    }
}

void setPWM(float u)
{
    //Possibly limit, if required
    limit(u,-1.0f,1.0f);
    //limit(u,-0.3f,0.3f); //DEBUG
    //Set proper PWM
    if (u >= 0.0f) {
        motorPWM = u;
        motorDir = false;
    } else {
        motorPWM = -u;
        motorDir = true;
    }
}

void doNetwork()
{
    //int size = eth.receive();
    //if(size > 0)
    //{
        //eth.read(buf, size);
        
    //}
}

void mapIn()
{
    //Measure relevant signals
    encoderPos = encoderPos;
    motorPos = (float)enc.getPulses() / ENCODER_CPR * TWO_PI; //motor position in rad
    encoderVel = (float)(encoderPos - encoderPos_prev) / LOOPDURATION; //encoder velocity in counts/s
    encoderPos_prev = encoderPos; //encoder position in counts
    motorVel = encoderVel / ENCODER_CPR * TWO_PI; //motor velocity in rad/s
    motorVel = velocityFilter(motorVel); //filtered motor velocity in rad/s
    motorPos_prev = motorPos; //Previous motor position in rad
    force = -FORCESENSORGAIN*2.0f*(measuredForce - 0.5f); //Measured force
    doNetwork(); //Receive inputs over network (if available)
}

void mapOut()
{
    //Apply control
    setPWM(u);
    //setPWM(0); //DEBUG ONLY
    //Send network packets if required
}

//State functions--------------------------------------------------------------------------------------------------------

//During the wait/idle state
inline void doStateIdle()
{
    //Control
    u = 0.0f;
    
    //Handle LED
    ledCnt++;
    if (ledCnt > LEDPERIODS) {
        ledState = !ledState;
        if (ledState)
            setLed(colorYellow);
        else
            setLed(colorBlack);
        ledCnt = 0;
    }
    
    //State Guard:
    if (t.read() > 3.0f) {
        currentState = stateCalibration;
        stateTime = t.read();
    }
}

inline void doStateCalib()
{
    //Control
    //Send out calibration PWM
    u = CALIBPWM;

    //Handle LED
    ledCnt++;
    if (ledCnt > LEDPERIODS) {
        ledState = !ledState;
        if (ledState)
            setLed(colorBlue);
        else
            setLed(colorBlack);
        ledCnt = 0;
    }

    //State Guard
    if ((abs(motorVel) < 0.001f) && (t.read()-stateTime > 1.0f)) {
        currentState = stateHoming;
        enc.reset(); // set pulses to zero
        
        // NEED TO UPDATE CALIBRATION HERE.
        //encoderPos = -MAX_ENCODER_POS_LEFT; //When calibrated, set encoder pos to the known value
        stateTime = t.read();
        refPos = (float)MAX_ENCODER_POS_LEFT /  ENCODER_CPR * TWO_PI; //Start at the left calibration point
    }
}

inline void doStateHoming()
{
    //Calculate a ramp to location 0 (center) with -2 rad/s
    refVel = -2.00f;
    if (refPos > 0.0f) {
        refPos += refVel*LOOPDURATION;//refVel*LOOPDURATION;
    }
    else {
        refPos = 0.0f;
        refVel = 0.0f;
    }
    
    //Control
    posError = refPos - motorPos;
    integratedError += posError*LOOPDURATION;
    velError = refVel - motorVel;
    u = K_p * posError + K_d * velError + 0*K_i*integratedError;

    //Handle LED
    ledCnt++;
    if (ledCnt > LEDPERIODS) {
        ledState = !ledState;
        if (ledState)
            setLed(colorGreen);
        else
            setLed(colorBlack);
        ledCnt = 0;
    }
    
    //State Guard
    if ((abs(encoderPos) < 6) && (abs(velError) < 0.002f) && (t.read()-stateTime > 1.0f)) {
        integratedError = 0.0f;
        setLed(colorGreen);
        stateTime = t.read();
        currentState = stateOPAdmittance;//stateOPImpedance;
        /*if (doAdmittance) {
            currentState = stateOPAdmittance;
        } else {
            currentState = stateOPImpedance;
        }*/
    }
}

inline void doStateOPAdmittance()
{
    //Virtual Model
    refAcc = force / virtualMass - virtualStiffness * (refPos - springPos) - virtualDamping * refVel;
    refVel += refAcc * LOOPDURATION;
    
    if (refVel > MAXVEL) 
    {
        refVel = MAXVEL;
        refAcc = 0.0f;
    } else if (refVel < -MAXVEL) 
    {
        refVel = -MAXVEL;
        refAcc = 0.0f;
    }

    refPos += refVel * LOOPDURATION;
    if (refPos > WORKSPACEBOUND) 
    {
        refPos = WORKSPACEBOUND;
        refVel = 0.0f;
        refAcc = 0.0f;
    } else if (refPos < -WORKSPACEBOUND) 
    {
        refPos = -WORKSPACEBOUND;
        refVel = 0.0f;
        refAcc = 0.0f;
    }

    //Controller
    posError = refPos - motorPos;
    velError = refVel - motorVel;
    integratedError += posError*LOOPDURATION;
    u = refAcc*I_ff + K_p * posError + K_d * velError  + 0*K_i * integratedError;

    //StateGuard
    //if...
}

inline void doStateOPImpedance()
{
    //Controller is the virtual model in OPEN LOOP impedance
    u = - virtualStiffness * (motorPos - springPos) - virtualDamping * motorVel;
    //StateGuard
    //if...
}

inline void doStateFailed()
{
    //Control
    u = 0.0f;
    
    //Handle LED
    ledCnt++;
    if (ledCnt > LEDPERIODS) {
        ledState = !ledState;
        if (ledState)
            setLed(colorRed);
        else
            setLed(colorBlack);
        ledCnt = 0;
    }

    //No state guard.
}

//Loop Function--------------------------------------------------------------------------------------------------------
void loopFunction()
{
    //This code is run every loop
    loopCounter++;

    //Measure relevant inputs
    mapIn();

    //Set Defaults
    u = 0.0f;

    //Do State Machine
    switch(currentState) {
        case stateIdle:
            doStateIdle();
            break;
        case stateCalibration:
            doStateCalib();
            break;
        case stateHoming:
            doStateHoming();
            break;
        case stateOPImpedance:
            doStateOPImpedance();
            break;
        case stateOPAdmittance:
            doStateOPAdmittance();
            break;
        case stateFailed: //fall through
            doStateFailed();
            break;
    }

    mapOut(); //Set all relevant output pins
}

//Main--------------------------------------------------------------------------------------------------------
int main()
{
    setLed(colorBlack); //Set LED to black
    pc.baud(115200); //set Serial comm
    t.start();
    
    //Set all interrupt requests to 2nd place priority
    for(int n = 0; n < 86; n++) {
        NVIC_SetPriority((IRQn)n,1);
    }
    //Set out motor encoder interrupt to 1st place priority
    NVIC_SetPriority(PORTC_IRQn,0);

    motorPWM.period_us(PWMCYCLETIMEUS); //50 us = 20 kHz
    mainLoop.attach_us(&loopFunction,LOOPDURATIONUS); //200 us = 5 kHz
    
    //Infinite while loop
    while (true) {
        //Print relevant stuff to terminal here, do not print in the in the loopFunction ticker!!
        if (currentState != statePrev) { //When state changes
            pc.printf(">>>>>State Changed to State: %d\r\n",currentState);
            statePrev = currentState; //Update the prev state var
        }
        wait(0.01);
        float millisecondsPassed = (float)loopCounter*(((float)LOOPDURATIONUS) / 1000.0f);
        pc.printf("t:%f;State:%d;EncPos:%d;MotPos:%f;MotVel:%f;Force:%f;RefPos:%f;PosErr:%f;MotPWM:%f\r\n",millisecondsPassed,currentState,encoderPos,motorPos,motorVel,force,refPos,posError,u);
    }
}
//EOF--------------------------------------------------------------------------------------------------------