Diff: main.cpp

Revision:

7:80baf171503c

Parent:

6:105b306350c6

Child:

8:b40067b8a72d

--- a/main.cpp	Tue Oct 29 11:26:43 2019 +0000
+++ b/main.cpp	Thu Oct 31 13:05:44 2019 +0000
@@ -19,8 +19,10 @@
 PwmOut E1(D5);
 PwmOut E2(D6);
 
+double gearRatio = 40/9;
+
 double initRot1 = 0;
-double initRot2 = (48.5 + 90)/60;
+double initRot2 = -gearRatio*(180 - 48.5)/360;
 
 
 MODSERIAL pc(USBTX, USBRX);
@@ -30,16 +32,22 @@
 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);
+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);
 
 // main() runs in its own thread in the OS
 int main()
 {
     pc.baud(115200);
     pc.printf("Start\n\r");
-    moveAlongPath(7, 13, 0, 20);
-    pc.printf("End");
-    
+    while(1){
+    moveAlongPath(10, 30, -10, 30, 3);
+    moveAlongPath(-10, 30, -10, 20, 3);
+    moveAlongPath(-10, 20, 10, 20, 3);
+    moveAlongPath(10, 20, 10, 30, 3);
+    pc.printf("\n\r start over\n\r");
+    }
+
 }
 
 
@@ -49,24 +57,23 @@
 //PwmOut *E = pointer to speed contorl pin of motor
 //QEI *Enc = pointer to encoder of motor
 //double rotDes = desired rotation
-void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
+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 Kp = 30;
-    double Ki = 0;
-    double Kd = 5;
-    
     double U_p;
     double U_i;
     double U_d;
 
     double rotC = Enc->getPulses()/(32*131.25) + initRot;
     double MotorPWM;
-    //pc.printf("rotDes: %f\n\r", rotDes);
 
     Timer t;
     double tieme = 0;
@@ -80,7 +87,7 @@
         t.reset();
         iError = iError + pErrorC*tieme;
         dError = (pErrorC - pErrorP)/tieme;
-        
+
         U_p = pErrorC*Kp;
         U_i = iError*Ki;
         U_d = dError*Kd;
@@ -94,13 +101,44 @@
             *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.029 || abs(dError) > 0.001);; //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -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);
+}
+
 
 //double that calculates the rotation of one arm.
 //parameters:
@@ -108,51 +146,73 @@
 //double yDes = ofset of the arm's shaft in cm in the y direction
 double calcRot1(double xDes, double yDes)
 {
-    return 6*((atan(yDes/xDes) - 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 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 that calculates the rotation of the other 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 calcRot2(double xDes, double yDes)
 {
-    return 6*((atan(yDes/xDes) + 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 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 plotPath(double xStart, double yStart, double xEnd, double yEnd, vector<double> *xPath, vector<double> *yPath)
+void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation)
 {
-    double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0));
-    int noSteps = int(lPath/0.1);
-    double xStep = (xEnd - xStart)/double(noSteps);
-    double yStep = (yEnd - yStart)/double(noSteps);
-    for(int i = 0; i<=noSteps; i++) {
-        xPath->push_back(xStart + i*xStep);
-        yPath->push_back(yStart + i*yStep);
-    }
+    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)
+void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
 {
-    vector<double> xPath;
-    vector<double> yPath;
-    vector<double> rot1Path;
-    vector<double> rot2Path;
-    
-    plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath);
+    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));
 
-    for(int i = 0; i < xPath.size(); i++) {
-        rot1Path.push_back(calcRot1(xPath.at(i), yPath.at(i)));
-        rot2Path.push_back(calcRot2(xPath.at(i), yPath.at(i)));
-        pc.printf("Rot1: %f\t Rot2:%f\n\r", rot1Path.at(i), rot2Path.at(i));
+    //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);
     }
 
-    for(int i = 0; i < xPath.size(); i++) {
-        moveMotorTo(&M1, &E1, &Enc1, initRot1, 1, rot1Path.at(i));
-        pc.printf("mot1: %f", rot1Path.at(i));
-        moveMotorTo(&M2, &E2, &Enc2, initRot2, -1, rot2Path.at(i));
-        pc.printf("mot2: %f", rot2Path.at(i));
-        
-    }
 }