Diff: main.cpp

Revision:

5:5082d694e643

Parent:

4:55e6707949dd

Child:

6:105b306350c6

--- a/main.cpp	Fri Oct 25 10:35:11 2019 +0000
+++ b/main.cpp	Tue Oct 29 08:17:34 2019 +0000
@@ -20,13 +20,13 @@
 PwmOut E2(D6);
 
 double initRot1 = 0;
-double initRot2 = 48.5/360;
+double initRot2 = (48.5 + 90)/360;
 
 
 MODSERIAL pc(USBTX, USBRX);
 
 
-//void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
+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[]);
@@ -37,7 +37,7 @@
 {
     pc.baud(115200);
     pc.printf("Start\n\r");
-    moveAlongPath(30, 0, 0, 20.0);
+    moveAlongPath(7, 13, 0, 20);
     pc.printf("End");
     
 }
@@ -48,33 +48,43 @@
 //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
-//float rotDes = desired rotation
-void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float initRot, float rotDes)
+//double rotDes = desired rotation
+void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes)
 {
-    float pErrorC;
-    float pErrorP = 0;
-    float iError = 0;
-    float dError;
+    double pErrorC;
+    double pErrorP = 0;
+    double iError = 0;
+    double dError;
 
-    float Kp = 5;
-    float Ki = 0.01;
-    float Kd = 0.7;
+    double Kp = 1;
+    double Ki = 0;
+    double Kd = 0.0;
+    
+    double U_p;
+    double U_i;
+    double U_d;
 
-    float rotC = Enc->getPulses()/(32*131.25) + initRot;
-    float rotP = 0;
-    float MotorPWM;
+    double rotC = Enc->getPulses()/(32*131.25) + initRot;
+    double MotorPWM;
+    pc.printf("rotDes: %f\n\r", rotDes);
 
     Timer t;
-    float tieme = 0;
+    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;
-
-        MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
+        
+        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;
@@ -83,13 +93,61 @@
             *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);
+    t.stop();
+}
 
-        rotP = rotC;
+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();
-        pc.printf("pError: %f iError: %f dError: %f\n\r", pErrorC, iError, dError);
-    } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
+        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);
     t.stop();
 }
 
@@ -109,29 +167,23 @@
 //double yDes = ofset of the arm's shaft in cm in the y direction
 double calcRot2(double xDes, double yDes)
 {
-    pc.printf("rot2: %f", 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)));
     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)
 {
-    pc.printf("xS: %f xE: %f yS: %f yE: %f\n\r", xStart, xEnd, yStart,yEnd);
     double lPath = sqrt(pow(xEnd-xStart, 2.0) + pow(yEnd-yStart, 2.0));
-    int noSteps = int(lPath/0.1);
+    int noSteps = int(lPath/0.01);
     double xStep = (xEnd - xStart)/double(noSteps);
     double yStep = (yEnd - yStart)/double(noSteps);
-    pc.printf("xStep: %f yStep: %f\n\r", xStep, yStep);
-    pc.printf("dx: %f dy: %f\n\r", xEnd - xStart, yEnd - yStart);
     for(int i = 0; i<=noSteps; i++) {
         xPath->push_back(xStart + i*xStep);
         yPath->push_back(yStart + i*yStep);
-        pc.printf("to go plotPath: %i / %i \n\r", i, noSteps);
     }
 }
 
 void moveAlongPath(double xStart, double yStart, double xEnd, double yEnd)
 {
-    pc.printf("xS: %f xE: %f yS: %f yE: %f \n\r", xStart, xEnd, yStart,yEnd);
     vector<double> xPath;
     vector<double> yPath;
     vector<double> rot1Path;
@@ -140,15 +192,14 @@
     plotPath(xStart, yStart, xEnd, yEnd, &xPath, &yPath);
 
     for(int i = 0; i < xPath.size(); i++) {
-        rot1Path.push_back(calcRot1(xPath[i], yPath[i]));
-        rot2Path.push_back(calcRot2(xPath[i], yPath[i]));
-        pc.printf("to go angle: %i / %i \n\r", i, xPath.size());
+        rot1Path.push_back(calcRot1(xPath.at(i), yPath.at(i)));
+        rot2Path.push_back(calcRot2(xPath.at(i), yPath.at(i)));
     }
 
     for(int i = 0; i < xPath.size(); i++) {
-        moveMotorTo(&M1, &E1, &Enc1, initRot1, rot1Path.at(i));
-        moveMotorTo(&M2, &E2, &Enc2, initRot2, rot2Path.at(i));
-        pc.printf("to go move: %i / %i", i, rot1Path.size());
+        moveMotorTo(&M1, &E1, &Enc1, initRot1, 1, rot1Path.at(i));
+        moveMotorTo1(&M2, &E2, &Enc2, initRot2, -1, rot2Path.at(i));
+        
     }
 }