File content as of revision 9:0e838367ab6a:

#include "mbed.h"
#include "FilterDesign.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad4.h"
#include "BiQuad.h"
#include "FastPWM.h"
#include <vector>


MODSERIAL pc(USBTX, USBRX);

// main() runs in its own thread in the OS

//State machine
InterruptIn stateSwitch(SW2);
InterruptIn stepSwitch(SW3);

void stateMachine();
void switchStep();
void switchState();

int noStateSteps = 4;
int stateStep = 0;
int state = 0;

void EMGState();
void demoState();

//Shared steps
void calibrateMotors();
void waiting();

//Demo steps
void playDemo();


//EMG steps
void EMGcalibration();
void moveToInitialPosition();
void moveWithEMG();

//Calibration
void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed);

//play demo
void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed);

//Motors
double Pi = 3.14159265359;

QEI Enc1(D12, D13, NC, 32);
QEI Enc2(D10, D11, NC, 32);

DigitalOut M1(D4);
DigitalOut M2(D7);

PwmOut E1(D5);
PwmOut E2(D6);

double gearRatio = 40/9;

double initRot1 = 0;
double initRot2 = -gearRatio*(180 - 48.5)/360;

double xCurrent;
double yCurrent;

double calcRot1(double xDes, double yDes);
double calcRot2(double xDes, double yDes);

void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation);
void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes);
void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);
void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed);
void flipx();
void flipy();

//EMG
AnalogIn emg0( A0 );
AnalogIn emg1( A3 );

HIDScope scope( 2 );

Ticker ticker_calibration;   // Ticker to send the EMG signals to screen

volatile double emg1_cal = 0.8;
volatile double emg1_filtered;      //measured value of the first emg
volatile double emg2_filtered;      //measured value of the second emg
volatile double emg2_cal = 0.8;

double speedy = 3;
double speedx = 3;

double xDir = 1;
double yDir = 1;

vector<double> pidInfo1 (4);
vector<double> pidInfo2 (4);

Timer tEMGMove;

InterruptIn xSwitch(D8);
InterruptIn ySwitch(D9);

void sample();

//Waiting
DigitalOut r_led(LED_RED);

int main()
{
    stateSwitch.rise(switchState);
    stepSwitch.rise(switchStep);

    xSwitch.mode(PullUp);
    ySwitch.mode(PullUp);

    xSwitch.fall(flipx);
    ySwitch.fall(flipy);

    pc.baud(115200);
    while(1) {
        stateMachine();
    }

}

void switchState()
{
    state++;
    state = state%2;
    stateStep = 0;
    switch(state) {
        //demo mode
        case 0:
            tEMGMove.stop();
            pc.printf("demo\n\r");
            noStateSteps = 4;
            break;
        case 1:
            pc.printf("EMG\n\r");
            noStateSteps = 7;
            fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
            fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
            tEMGMove.reset();
            tEMGMove.start();
            break;
    }
}

void switchStep()
{
    stateStep++;
    stateStep = stateStep%noStateSteps;
}

void stateMachine()
{
    switch (state) {
        case 0:
            demoState();
            break;
        case 1:
            EMGState();
            break;
    }
}

void demoState()
{
    pc.printf("demo %d\n\r", stateStep);
    switch (stateStep) {
        case 0:
            waiting();
            break;
        case 1:
            calibrateMotors();
            stateStep++;
            break;
        case 2:
            waiting();
            break;
        case 3:
            playDemo();
            break;
    }
}

void EMGState()
{
    pc.printf("EMG %d\n\r", stateStep);
    switch(stateStep) {
        case 0:
            waiting();
            break;
        case 1:
            calibrateMotors();
            stateStep++;
            break;
        case 2:
            waiting();
            break;
        case 3:
            EMGcalibration();
            pc.printf("hack");
            stateStep++;
            break;
        case 4:
            waiting();
            break;
        case 5:
            moveToInitialPosition();
            xCurrent = 0;
            yCurrent = 20;
            break;
        case 6:
            moveWithEMG();
            break;
    }
}

void waiting()
{
    r_led = 0;
    E1 =0;
    E2 =0;
    r_led = 1;
}

void calibrateMotors()
{
    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
    moveMotorToStop(&M2, &E2, &Enc2, 0.2);
    Enc2.reset();
    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
    Enc1.reset();
    pc.printf("%f", Enc1.getPulses());
}

void playDemo()
{
    moveAlongPath(10, 30, -10, 30, 3);
    moveAlongPath(-10, 30, -10, 20, 3);
    moveAlongPath(-10, 20, 10, 20, 3);
    moveAlongPath(10, 20, 10, 30, 3);
}

void EMGcalibration()
{
    Timer tijd;
    tijd.start(); 
    ticker_calibration.attach(&sample, 0.002);  
    do {
        //ticker_calibration.attach(&sample, 0.002);
        if(emg1_cal < emg1_filtered) {
            emg1_cal = emg1_filtered ;
            pc.printf("EMG_cal : %g \n\r",emg1_cal);
        }
        if(emg2_cal < emg2_filtered) {
            emg2_cal = emg2_filtered ;
        }
        pc.printf("emg1: %f\n\r", emg1_filtered);
    } while(tijd.read()<10);
}


void moveToInitialPosition()
{
    double xStart = 0;
    double yStart = 20;

    double rot1 = calcRot1(xStart, yStart);
    double rot2 = calcRot2(xStart, yStart);

    moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
    moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
}

void moveWithEMG()
{
    if(emg1_filtered >= 0.5*emg1_cal) {
        speedy = 3;
        pc.printf("emg1: %f", emg1_filtered);
    } else {
        speedy = 0;
    }

    if(emg2_filtered >= 0.5*emg2_cal) {
        speedx = 3;
        pc.printf("emg1: %f\n\r", emg2_filtered);
    } else {
        speedx = 0;
    }
    pc.printf("speedx: %f speedy: %f\r\n", speedx, speedy);
    xCurrent = xCurrent + xDir*speedx*0.01;
    yCurrent = yCurrent + yDir*speedy*0.01;
    double rot2 = calcRot2(xCurrent, yCurrent);
    moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &tEMGMove, -rot2);
    double rot1 = calcRot1(xCurrent, yCurrent);
    moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &tEMGMove, rot1);
    wait(0.01);
}

void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed)
{
    Timer t;

    double MotorPWM;

    double posC;
    double posP = Enc->getPulses()/(32*131.25);
    double vel = 0;

    int hasNotMoved = 0;

    t.start();
    do {
        MotorPWM = speed - vel*0.7;
        if(MotorPWM > 0) {
            *M = 0;
            *E = MotorPWM;
        } else {
            *M = 1;
            *E = -MotorPWM;
        }
        wait(0.01);
        posC = Enc->getPulses()/(32*131.25);
        vel = (posC - posP)/t.read();
        t.reset();
        posP = posC;
        //pc.printf("v: %f hm: %d\n\r", MotorPWM, hasNotMoved);
        if(abs(vel) > 0.001) {
            hasNotMoved = 0;
        } else {
            hasNotMoved++;
        }
    } while(hasNotMoved < 6);
    *E = 0;
}

void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
{
    double rot1;
    double rot2;

    Timer t;

    vector<double> desiredLocation;

    fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
    fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);

    double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));

    //Calculate the rotation of the motors at the start of the path
    rot1 = calcRot1(xStart, yStart);
    rot2 = calcRot2(xStart, yStart);
    pc.printf("r1: %f r2: %f", rot1/6, rot2/6);

    //Move arms to the start of the path
    //moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
    //moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);

    //start the timer
    t.start();
    while(pathLength > speed * t.read()) {
        findDesiredLocation(xStart, yStart, xEnd, yEnd, speed, &t, &desiredLocation);
        rot1 = calcRot1(desiredLocation.at(0), desiredLocation.at(1));
        //pc.printf("\n\r Rot1: %f", rot1);
        moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
        rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1));
        //pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2);
        moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
        //pc.printf("\n\r xCalc: %f yCalc: %f", calcX(pidInfo1.at(3), pidInfo2.at(3)), calcY(pidInfo1.at(3), pidInfo2.at(3)));
        wait(0.01);
    }

}

double calcRot1(double xDes, double yDes)
{
    double alpha = atan(yDes/xDes);
    if(alpha < 0) {
        alpha = alpha+Pi;
    }
    //pc.printf("alpha: %f", alpha);
    return gearRatio*((alpha - 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi));
}

double calcRot2(double xDes, double yDes)
{
    double alpha = atan(yDes/xDes);
    if(alpha < 0) {
        alpha = alpha+Pi;
    }
    return gearRatio*((alpha + 0.5*(Pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*Pi));
}

void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation)
{
    double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
    double traveledDistance = speed * t->read();
    double ratio = traveledDistance/pathLength;

    desiredLocation->clear();
    desiredLocation->push_back(xStart + (xEnd - xStart)*ratio);
    desiredLocation->push_back(yStart + (yEnd - yStart)*ratio);

}

void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes)
{
    double Kp = 61;  //180 & 10 werkt zonder hulp
    double Ki = 1;
    double Kd = 7;

    double rotC = Enc->getPulses()/(32*131.25) + initRot;

    double pErrorC = rotDes - rotC;

    double tieme = t->read();
    double dt = tieme - pidInfo->at(2);

    double iError = pidInfo->at(1) + pErrorC*dt;
    double dError = (pErrorC - pidInfo->at(0))/dt;

    double MotorPWM = (pErrorC*Kp + iError*Ki + dError*Kd)*dir;

    if(MotorPWM > 0) {
        *M = 0;
        *E = MotorPWM;
    } else {
        *M = 1;
        *E = -MotorPWM;
    }
    pidInfo->clear();
    pidInfo->push_back(pErrorC);
    pidInfo->push_back(iError);
    pidInfo->push_back(tieme);
    pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot);
}

void sample()
{
    emg1_filtered = FilterDesign(emg0.read());
    emg2_filtered = FilterDesign(emg1.read());
    /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
    scope.set(0, emg1_filtered ) ;
    scope.set(1, emg2_filtered );
    /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
    *  Ensure that enough channels are available (HIDScope scope( 2 ))
    *  Finally, send all channels to the PC at once */
    scope.send();
    /* To indicate that the function is working, the LED is toggled */
    //pc.printf("%f", emg1_filtered)
    //led = !led;
}

void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed)
{
    Timer t;

    vector<double> pidInfo1 (4);
    vector<double> pidInfo2 (4);

    fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
    fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);

    double xC = *xStart;
    double yC = *yStart;

    double rot1;
    double rot2;

    double tiemeP;
    double tiemeC;
    double dt;

    t.start();

    tiemeP = t.read();
    while(t.read() < 0.1) {
        tiemeC = t.read();
        dt = tiemeC - tiemeP;
        xC = xC+xSpeed*dt;
        yC = yC+ySpeed*dt;
        rot1 = calcRot1(xC, yC);
        rot2 = calcRot2(xC, yC);
        moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
        moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
        tiemeP = tiemeC;
        wait(0.01);
    }

    *xStart = xC;//calcX(pidInfo1.at(3), pidInfo2.at(3));
    *yStart = yC;//calcY(pidInfo1.at(3), pidInfo2.at(3));
}

void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
{
    double Kp = 2;  //180 & 10 werkt zonder hulp
    double Ki = 0;
    double Kd = 0.1;

    double pErrorC;
    double pErrorP = 0;
    double iError = 0;
    double dError;

    double U_p;
    double U_i;
    double U_d;

    double rotC = Enc->getPulses()/(32*131.25) + initRot;
    double MotorPWM;

    Timer t;
    double tieme = 0;

    t.start();
    do {
        pErrorP = pErrorC;
        rotC = Enc->getPulses()/(32*131.25) + initRot;
        pErrorC = rotDes - rotC;
        tieme = t.read();
        t.reset();
        iError = iError + pErrorC*tieme;
        dError = (pErrorC - pErrorP)/tieme;

        U_p = pErrorC*Kp;
        U_i = iError*Ki;
        U_d = dError*Kd;
        MotorPWM = (U_p + U_i + U_d)*dir;

        if(MotorPWM > 0) {
            *M = 0;
            *E = MotorPWM;
        } else {
            *M = 1;
            *E = -MotorPWM;
        }
        wait(0.01);
        //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
    } while (abs(MotorPWM) > 0.13|| abs(dError > 0.01));; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
    *E = 0;
    //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
    t.stop();
}

void flipx()
{
    xDir = -xDir;

}

void flipy()
{
    yDir = -yDir;
}