File content as of revision 24:830e4dfa112a:

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

using std::vector;

Serial pc(USBTX,USBRX);
DigitalIn button(SW3) ;
InterruptIn sw2(SW2);
InterruptIn sw3(SW3);

//Define objects
AnalogIn    emg0( A0 );
AnalogIn    emg1( A1 );
QEI Enc1(D12, D13, NC, 32);
QEI Enc2(D10, D11, NC, 32);
DigitalOut M1(D4);
DigitalOut M2(D7);
PwmOut E1(D5);
PwmOut E2(D6);


Ticker      ticker_calibration;   // Ticker to send the EMG signals to screen
Ticker      sample_timer;           // Ticker for reading out EMG
HIDScope    scope( 2 );
DigitalOut  led(LED_RED);
DigitalOut  controlLED(LED_GREEN);
Timer timer;

volatile double emg1_filtered;      //measured value of the first emg
volatile double emg2_filtered;      //measured value of the second emg
volatile double emg1_cal = 0.8;     // calibrated value of first emg
volatile double emg2_cal = 0.8 ;    // calibrated value of second emg
double Pi = 3.14159265359;
double gearRatio = 40/9;

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

double speedy = 3 ;
double speedx = 3 ;



void sample() ;
void EMGcalibration() ;
void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);
double calcRot1(double xDes, double yDes);
double calcRot2(double xDes, double yDes);
void plotPath(double xStart, double yStart, double xEnd, double yEnd, double *xPath[], double *yPath[]);
void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed);
void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes);
double calcX(double rot1, double rot2);
double calcY(double rot1, double rot2);
void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed);
void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation);
void EMG_move();

void changespeedy(){
    speedy = speedy*-1;
    }
void changespeedx(){
    speedx = speedx*-1;
    }

int main()
{


    pc.baud(115200);
    sw2.mode(PullUp);
    sample_timer.attach(&sample, 0.002);


    controlLED = 1;
    while(button >= 1) {
        led = 1 ;
    }
    led = 0 ;
    EMGcalibration();
    led = 1 ;
    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);
    sw2.rise(changespeedy);
    sw3.rise(changespeedx);
    while(true) {
        

        while(emg1_filtered >= 0.5*emg1_cal) {
            //controlLED = !controlLED ;
            moveWithSpeed(&xStart, &yStart, 0, speedy);
            pc.printf("ystart: %g \n\r",yStart);
        }
        while(emg2_filtered >= 0.5*emg2_cal) {
            moveWithSpeed(&xStart, &yStart, speedx, 0);
            }

        rot1 = calcRot1(xStart, yStart);
        rot2 = calcRot2(xStart, yStart);
        moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
        moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
    }
}





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 EMGcalibration()
{

    Timer tijd;
    tijd.start();
    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 ;
            }
    } while(tijd<10);
}


//function to mave a motor to a certain number of rotations, counted from the start of the program.
//parameters:
//DigitalOut *M = pointer to direction cpntol pin of motor
//PwmOut *E = pointer to speed contorl pin of motor
//QEI *Enc = pointer to encoder of motor
//double rotDes = desired rotation
void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
{
    double Kp = 30;  //180 & 10 werkt zonder hulp
    double Ki = 0;
    double Kd = 2;

    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 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);
}


//double that calculates the rotation of one arm.
//parameters:
//double xDes = ofset of the arm's shaft in cm in the x direction
//double yDes = ofset of the arm's shaft in cm in the y direction
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 moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
{
    double rot1;
    double rot2;

    Timer t;

    vector<double> desiredLocation;

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

    fill(pidInfo1.begin(), pidInfo1.begin()+3, 0);
    fill(pidInfo2.begin(), pidInfo2.begin()+3, 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);
        wait(0.01);
    }

}



double calcX(double rot1, double rot2)
{
    return 20*cos((rot1/gearRatio)*2*Pi)+20*cos((-rot2/gearRatio)*2*Pi);
}

double calcY(double rot1, double rot2)
{
    return 20*sin((rot1/gearRatio)*2*Pi)+20*sin((-rot2/gearRatio)*2*Pi);
}


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));
}