Diff: main.cpp

Revision:

8:b40067b8a72d

Parent:

7:80baf171503c

Child:

9:0e838367ab6a

diff -r 80baf171503c -r b40067b8a72d main.cpp
--- a/main.cpp	Thu Oct 31 13:05:44 2019 +0000
+++ b/main.cpp	Thu Oct 31 18:22:28 2019 +0000
@@ -34,20 +34,25 @@
 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);
+double calcX(double rot1, double rot2);
+double calcX(double rot1, double rot2);
 
 // main() runs in its own thread in the OS
 int main()
 {
     pc.baud(115200);
-    pc.printf("Start\n\r");
-    while(1){
+    while(1) {
+        playDemo;
+    }
+
+}
+
+void playDemo()
+{
     moveAlongPath(10, 30, -10, 30, 3);
     moveAlongPath(-10, 30, -10, 20, 3);
     moveAlongPath(-10, 20, 10, 20, 3);
     moveAlongPath(10, 20, 10, 30, 3);
-    pc.printf("\n\r start over\n\r");
-    }
-
 }
 
 
@@ -137,6 +142,7 @@
     pidInfo->push_back(pErrorC);
     pidInfo->push_back(iError);
     pidInfo->push_back(tieme);
+    pidInfo->push_back(Enc->getPulses()/(32*131.25) + initRot);
 }
 
 
@@ -175,6 +181,16 @@
 
 }
 
+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 moveAlongPath(double xStart, double yStart, double xEnd, double yEnd, double speed)
 {
     double rot1;
@@ -184,11 +200,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));
 
@@ -209,8 +225,9 @@
         //pc.printf("\n\r Rot1: %f", rot1);
         moveMotorContinuously(&M1, &E1, &Enc1, initRot2, 1, &pidInfo2, &t, -rot2);
         rot2 = calcRot2(desiredLocation.at(0), desiredLocation.at(1));
-        pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2);
+        //pc.printf("\n\r X: %f Y: %f Rot1: %f Rot2 %f", desiredLocation.at(0), desiredLocation.at(1), rot1, rot2);
         moveMotorContinuously(&M2, &E2, &Enc2, initRot1, -1, &pidInfo1, &t, rot1);
+        pc.printf("\n\r xCalc: %f yCalc: %f", calcX(pidInfo1.at(3), pidInfo2.at(3)), calcY(pidInfo1.at(3), pidInfo2.at(3)));
         wait(0.01);
     }