Diff: main.cpp

Revision:

9:57b261ee4127

Parent:

8:9b1bf2949a53

--- a/main.cpp	Thu Oct 31 14:11:18 2019 +0000
+++ b/main.cpp	Thu Oct 31 16:56:37 2019 +0000
@@ -28,27 +28,31 @@
 MODSERIAL pc(USBTX, USBRX);
 
 
-void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, double rotDes);
+void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, 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, double speed);
 void moveMotorContinuously(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, vector<double> *pidInfo, Timer *t, double rotDes);
-void moveWithSpeed(xSpeed, ySpeed);
+void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed);
 
 // main() runs in its own thread in the OS
 int main()
 {
     pc.baud(115200);
-    pc.printf("Start\n\r");
-    double rot1 = calcRot1(0, 20);
-    double rot2 = calcRot2(0, 20);
+    pc.printf("Start\n\r"); 
+        
+    double xStart = 0;
+    double yStart = 20;
+    
+    double rot1 = calcRot1(xStart, yStart);
+    double rot2 = calcRot2(xStart, yStart);
     
     moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
     moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
     
-    while(1){
-        moveWithSpeed(*xStart, *yStart, xSpeed, ySpeed,)
+    while (true) {
+        moveWithSpeed(&xStart, &yStart, 3, 3);
     }
 }
 
@@ -139,7 +143,7 @@
     pidInfo->push_back(pErrorC);
     pidInfo->push_back(iError);
     pidInfo->push_back(tieme);
-    pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot)
+    pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot);
 }
 
 
@@ -187,11 +191,11 @@
 
     vector<double> desiredLocation;
 
-    vector<double> pidInfo1 (3);
-    vector<double> pidInfo2 (3);
+    vector<double> pidInfo1 (4);
+    vector<double> pidInfo2 (4);
 
-    fill(pidInfo1.begin(), pidInfo1.begin()+3, 0);
-    fill(pidInfo2.begin(), pidInfo2.begin()+3, 0);
+    fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
+    fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
 
     double pathLength = sqrt(pow((xStart - xEnd), 2.0)+pow((yStart - yEnd), 2.0));
 
@@ -219,32 +223,50 @@
 
 }
 
-void moveWithSpeed(*xStart, *yStart, xSpeed, ySpeed,){
+double calcX(double rot1, double rot2){
+    return 20*cos((rot1/gearRatio)*2*Pi)+20*cos((-rot2/gearRatio)*2*Pi);
+}
+
+double calcY(double rot1, double rot2){
+    return 20*sin((rot1/gearRatio)*2*Pi)+20*sin((-rot2/gearRatio)*2*Pi);
+}
+
+void moveWithSpeed(double *xStart, double *yStart, double xSpeed, double ySpeed){
     Timer t;
     
-    vector<double> pidInfo1 (3);
-    vector<double> pidInfo2 (3);
+    vector<double> pidInfo1 (4);
+    vector<double> pidInfo2 (4);
+
+    fill(pidInfo1.begin(), pidInfo1.begin()+4, 0);
+    fill(pidInfo2.begin(), pidInfo2.begin()+4, 0);
     
-    xC = *xStart;
-    yC = *yStart;
+    double xC = *xStart;
+    double yC = *yStart;
     
     double rot1;
     double rot2;
+    
+    double tiemeP;
+    double tiemeC;
+    double dt;
+    
     t.start();
+    
     tiemeP = t.read();
     while(t.read() < 0.1){
-        tiemeC = t.read;
+        tiemeC = t.read();
         dt = tiemeC - tiemeP;
-        xC = xC+xspeed*dt;
+        xC = xC+xSpeed*dt;
         yC = yC+ySpeed*dt;
         rot1 = calcRot1(xC, yC);
         rot2 = calcRot2(xC, yC);
         moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
         moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
         tiemeP = tiemeC;
+        wait(0.01);
     }
     
-    *xStart = xC;
-    *yStart = yC;  
+    *xStart = calcX(pidInfo1.at(3), pidInfo2.at(3));
+    *yStart = calcY(pidInfo1.at(3), pidInfo2.at(3));
 }