Diff: main.cpp

Revision:

7:13bb9bf83f58

Parent:

6:105b306350c6

Child:

8:ce83823803a3

diff -r 105b306350c6 -r 13bb9bf83f58 main.cpp
--- a/main.cpp	Tue Oct 29 11:26:43 2019 +0000
+++ b/main.cpp	Thu Oct 31 11:32:08 2019 +0000
@@ -19,54 +19,141 @@
 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);
 
-
-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);
+void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double MotorPWM);
+void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2);
+void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, 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");
-    
+    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
+    Enc1.reset();
+    moveMotorToPoint(&M1, &E1, &Enc1, 0, 1, 0.2);
+    moveMotorsToStop(&M2, &E2, &Enc2, 0.05, &M1, &E1, &Enc1, 0.3);
+    pc.printf("end\n\r");
 }
 
 
-//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 moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
+void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2)
 {
+    Timer t;
+
+    double posC1;
+    double posP1 = Enc1->getPulses()/(32*131.25);
+    double vel1 = 0;
+    double MotorPWM1;
+
+    double posC2;
+    double posP2 = Enc2->getPulses()/(32*131.25);
+    double vel2 = 0;
+    double MotorPWM2;
+    
+    int hasNotMoved = 0;
+
+
+    t.start();
+    do {
+        MotorPWM1 = speed1 - vel1*0.5;
+        if(MotorPWM1 > 0) {
+            *M1 = 0;
+            *E1 = MotorPWM1;
+        } else {
+            *M1 = 1;
+            *E1 = -MotorPWM1;
+        }
+        MotorPWM2 = speed2 - vel2*0.5;
+        if(MotorPWM2 > 0) {
+            *M2 = 0;
+            *E2 = MotorPWM2;
+        } else {
+            *M2 = 1;
+            *E2 = -MotorPWM2;
+        }
+        wait(0.01);
+        posC1 = Enc1->getPulses()/(32*131.25);
+        vel1 = (posC1 - posP1)/t.read();
+        posP1 = posC1;
+        //pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
+        posC2 = Enc2->getPulses()/(32*131.25);
+        vel2 = (posC2 - posP2)/t.read();
+        t.reset();
+        posP2 = posC2;
+        //pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
+        if(abs(vel1) > 0.001) {
+            hasNotMoved = 0;
+        } else {
+            hasNotMoved++;
+        }
+    } while(abs(vel1) > 0.001 || hasNotMoved < 10);
+    *E1 = 0;
+    *E2 = 0;
+}
+
+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.5;
+        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: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
+        if(abs(vel) > 0.001) {
+            hasNotMoved = 0;
+        } else {
+            hasNotMoved++;
+        }
+    } while(abs(vel) > 0.001 || hasNotMoved < 10);
+    *E = 0;
+}
+
+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 +167,7 @@
         t.reset();
         iError = iError + pErrorC*tieme;
         dError = (pErrorC - pErrorP)/tieme;
-        
+
         U_p = pErrorC*Kp;
         U_i = iError*Ki;
         U_d = dError*Kd;
@@ -94,65 +181,9 @@
             *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();
-}
-
-
-//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)
-{
-    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 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));
-};
-
-void plotPath(double xStart, double yStart, double xEnd, double yEnd, vector<double> *xPath, vector<double> *yPath)
-{
-    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);
-    }
-}
-
-void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd)
-{
-    vector<double> xPath;
-    vector<double> yPath;
-    vector<double> rot1Path;
-    vector<double> rot2Path;
-    
-    plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath);
-
-    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));
-    }
-
-    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));
-        
-    }
-}
-
+}
\ No newline at end of file