Revision 9:0e838367ab6a, committed 2019-11-01

Comitter:

JonaVonk

Date:

Fri Nov 01 10:23:02 2019 +0000

Parent:

8:b40067b8a72d

Commit message:

deze werkt bijna

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BiQuad4th_order.lib	Fri Nov 01 10:23:02 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Mirjam/code/BiQuad4th_order/#ac4bef72d6af

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter/FilterDesign.cpp	Fri Nov 01 10:23:02 2019 +0000
@@ -0,0 +1,52 @@
+#include "FilterDesign.h"
+#include "BiQuad.h"
+#include "BiQuad4.h"
+
+
+
+// Notch filter on 50 Hz
+double nb0 = 0.999103206817809;
+double nb1 = -1.994263409725146;
+double nb2 = 0.999103206817809;
+double na1 = -1.994263409725146;
+double na2 = 0.998206413635618;
+
+// 4th order Butterworth High pass 9 Hz
+double hpb0 = 0.862550850547179;
+double hpb1 = -3.45020340218871;
+double hpb2 = 5.17530510328307; 
+double hpb3 = -3.45020340218871;
+double hpb4 = 0.862550850547179;
+double hpa1 = -3.70453845798319;
+double hpa2 = 5.15648369715094;
+double hpa3 = -3.1957974837626;
+double hpa4 = 0.743993969858123;
+
+//4th order Butterworth low pass 10 Hz
+double lpb0 = 0.0000132937288987445 ;
+double lpb1 = 0.0000531749155949779 ;
+double lpb2 = 0.0000797623733924668; 
+double lpb3 = 0.0000531749155949779;
+double lpb4 = 0.0000132937288987445;
+double lpa1 = -3.67172908916193;
+double lpa2 = 5.06799838673419;
+double lpa3 = -3.11596692520174;
+double lpa4 = 0.719910327291871;
+
+// Multiplication with the gain
+double gain = 10.00000;     
+
+BiQuad notch50(nb0, nb1, nb2, na1, na2);                         
+BiQuad4 highpass(hpb0, hpb1, hpb2, hpb3, hpb4, hpa1, hpa2, hpa3, hpa4);
+BiQuad4 lowpass(lpb0, lpb1, lpb2, lpb3, lpb4, lpa1, lpa2, lpa3, lpa4);
+
+double FilterDesign(double u)
+{   
+    double y_n = notch50.step(u);       // First the notchfilter
+    double y_hp = highpass.step(y_n);   // Secondly the highpassfilter
+    double y_abs = abs(y_hp);           // Make the signal values absolute
+    double y_lp = lowpass.step(y_abs);  // Then a lowpass filter
+    double y_gain = y_lp*gain;          // Multiply by a gain
+        
+    return y_gain;                      // Return this filtered value
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter/FilterDesign.h	Fri Nov 01 10:23:02 2019 +0000
@@ -0,0 +1,3 @@
+#include "mbed.h"
+// .h file to design a filter for the first EMG signal
+double FilterDesign(double u);
\ No newline at end of file

--- a/main.cpp	Thu Oct 31 18:22:28 2019 +0000
+++ b/main.cpp	Fri Nov 01 10:23:02 2019 +0000
@@ -1,13 +1,53 @@
 #include "mbed.h"
-//#include "HIDScope.h"
+#include "FilterDesign.h"
+#include "HIDScope.h"
 #include "QEI.h"
 #include "MODSERIAL.h"
-//#include "BiQuad.h"
-//#include "FastPWM.h"
+#include "BiQuad4.h"
+#include "BiQuad.h"
+#include "FastPWM.h"
 #include <vector>
 
-using std::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);
@@ -24,29 +64,181 @@
 double initRot1 = 0;
 double initRot2 = -gearRatio*(180 - 48.5)/360;
 
-
-MODSERIAL pc(USBTX, USBRX);
+double xCurrent;
+double yCurrent;
 
-
-void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, 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 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);
-double calcX(double rot1, double rot2);
-double calcX(double rot1, double rot2);
+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;
 
-// main() runs in its own thread in the OS
+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) {
-        playDemo;
+        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);
@@ -55,18 +247,262 @@
     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);
+}
 
-//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 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 = 30;  //180 & 10 werkt zonder hulp
+    double Kp = 2;  //180 & 10 werkt zonder hulp
     double Ki = 0;
-    double Kd = 2;
+    double Kd = 0.1;
 
     double pErrorC;
     double pErrorP = 0;
@@ -106,130 +542,20 @@
             *E = -MotorPWM;
         }
         wait(0.01);
-        printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
+        //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)
+void flipx()
 {
-    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);
+    xDir = -xDir;
 
 }
 
-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 moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
+void flipy()
 {
-    double rot1;
-    double rot2;
-
-    Timer t;
-
-    vector<double> desiredLocation;
-
-    vector<double> pidInfo1 (4);
-    vector<double> pidInfo2 (4);
-
-    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);
-    }
-
-}
-
+    yDir = -yDir;
+}
\ No newline at end of file