Diff: main.cpp

Revision:

3:06f794380b5d

Parent:

2:96e093a48314

Child:

4:55e6707949dd

--- a/main.cpp	Fri Oct 25 09:37:26 2019 +0000
+++ b/main.cpp	Fri Oct 25 10:34:09 2019 +0000
@@ -26,7 +26,7 @@
 MODSERIAL pc(USBTX, USBRX);
 
 
-void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
+//void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float 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(0, 0, 0, 20);
+    moveAlongPath(30, 0, 0, 20.0);
     pc.printf("End");
     
 }
@@ -49,7 +49,7 @@
 //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)
+/*void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float initRot, float rotDes)
 {
     float pErrorC;
     float pErrorP = 0;
@@ -88,9 +88,10 @@
         rotC = Enc->getPulses()/(32*131.25) + initRot;
         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);
     t.stop();
-}
+}*/
 
 
 //double that calculates the rotation of one arm.
@@ -108,15 +109,19 @@
 //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);
-    double xStep = (xEnd - xStart)/noSteps;
-    double yStep = (yEnd - yStart)/noSteps;
+    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);
@@ -126,6 +131,7 @@
 
 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;
@@ -139,10 +145,10 @@
         pc.printf("to go angle: %i / %i \n\r", i, xPath.size());
     }
 
-    for(int i = 0; i < xPath.size(); 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());
-    }
+    }*/
 }