Diff: main.cpp

Revision:

6:105b306350c6

Parent:

5:5082d694e643

Child:

7:13bb9bf83f58

--- a/main.cpp	Tue Oct 29 08:17:34 2019 +0000
+++ b/main.cpp	Tue Oct 29 11:26:43 2019 +0000
@@ -20,7 +20,7 @@
 PwmOut E2(D6);
 
 double initRot1 = 0;
-double initRot2 = (48.5 + 90)/360;
+double initRot2 = (48.5 + 90)/60;
 
 
 MODSERIAL pc(USBTX, USBRX);
@@ -56,9 +56,9 @@
     double iError = 0;
     double dError;
 
-    double Kp = 1;
+    double Kp = 30;
     double Ki = 0;
-    double Kd = 0.0;
+    double Kd = 5;
     
     double U_p;
     double U_i;
@@ -66,7 +66,7 @@
 
     double rotC = Enc->getPulses()/(32*131.25) + initRot;
     double MotorPWM;
-    pc.printf("rotDes: %f\n\r", rotDes);
+    //pc.printf("rotDes: %f\n\r", rotDes);
 
     Timer t;
     double tieme = 0;
@@ -93,61 +93,11 @@
             *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.001); //pErrorC > 0.02 || pErrorC < -0.02 ||dError > 0.01 || dError < -0.01);
+    } while (abs(MotorPWM) > 0.029 || abs(dError) > 0.001);; //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 moveMotorTo1(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
-{
-    double pErrorC;
-    double pErrorP = 0;
-    double iError = 0;
-    double dError;
-
-    double Kp = 1;
-    double Ki = 0;
-    double Kd = 0.0;
-    
-    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;
-
-    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;
-        }
-        //printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
-    } while (abs(MotorPWM)>0.001); //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);
+    //pc.printf("U_p: %f U_i: %f U_d: %f motorPWM: %f\n\r", pErrorC, iError, dError, MotorPWM);
     t.stop();
 }
 
@@ -173,7 +123,7 @@
 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.01);
+    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++) {
@@ -194,11 +144,14 @@
     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));
-        moveMotorTo1(&M2, &E2, &Enc2, initRot2, -1, rot2Path.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));
         
     }
 }