Revision 8:ce83823803a3, committed 2019-10-31

Comitter:

JonaVonk

Date:

Thu Oct 31 18:22:49 2019 +0000

Parent:

7:13bb9bf83f58

Commit message:

calibrate

Changed in this revision

diff -r 13bb9bf83f58 -r ce83823803a3 main.cpp
--- a/main.cpp	Thu Oct 31 11:32:08 2019 +0000
+++ b/main.cpp	Thu Oct 31 18:22:49 2019 +0000
@@ -30,74 +30,24 @@
 void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double MotorPWM);
 void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2);
 void moveMotorToPoint(DigitalOut *M, PwmOut *E, QEI *Enc, double initRot, double dir, double rotDes);
+void calibrateMotor();
 
 // main() runs in its own thread in the OS
 int main()
 {
     pc.baud(115200);
-    pc.printf("Start\n\r");
-    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
-    Enc1.reset();
-    moveMotorToPoint(&M1, &E1, &Enc1, 0, 1, 0.2);
-    moveMotorsToStop(&M2, &E2, &Enc2, 0.05, &M1, &E1, &Enc1, 0.3);
-    pc.printf("end\n\r");
+    calibrateMotor();
 }
 
-
-void moveMotorsToStop(DigitalOut *M1, PwmOut *E1, QEI *Enc1, double speed1, DigitalOut *M2, PwmOut *E2, QEI *Enc2, double speed2)
-{
-    Timer t;
-
-    double posC1;
-    double posP1 = Enc1->getPulses()/(32*131.25);
-    double vel1 = 0;
-    double MotorPWM1;
-
-    double posC2;
-    double posP2 = Enc2->getPulses()/(32*131.25);
-    double vel2 = 0;
-    double MotorPWM2;
-    
-    int hasNotMoved = 0;
-
+void calibrateMotor(){
+    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
+    moveMotorToStop(&M2, &E2, &Enc2, 0.2);
+    Enc2.reset();
+    moveMotorToStop(&M1, &E1, &Enc1, -0.1);
+    Enc1.reset();
+    pc.printf("%f", Enc1.getPulses());
+}
 
-    t.start();
-    do {
-        MotorPWM1 = speed1 - vel1*0.5;
-        if(MotorPWM1 > 0) {
-            *M1 = 0;
-            *E1 = MotorPWM1;
-        } else {
-            *M1 = 1;
-            *E1 = -MotorPWM1;
-        }
-        MotorPWM2 = speed2 - vel2*0.5;
-        if(MotorPWM2 > 0) {
-            *M2 = 0;
-            *E2 = MotorPWM2;
-        } else {
-            *M2 = 1;
-            *E2 = -MotorPWM2;
-        }
-        wait(0.01);
-        posC1 = Enc1->getPulses()/(32*131.25);
-        vel1 = (posC1 - posP1)/t.read();
-        posP1 = posC1;
-        //pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
-        posC2 = Enc2->getPulses()/(32*131.25);
-        vel2 = (posC2 - posP2)/t.read();
-        t.reset();
-        posP2 = posC2;
-        //pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
-        if(abs(vel1) > 0.001) {
-            hasNotMoved = 0;
-        } else {
-            hasNotMoved++;
-        }
-    } while(abs(vel1) > 0.001 || hasNotMoved < 10);
-    *E1 = 0;
-    *E2 = 0;
-}
 
 void moveMotorToStop(DigitalOut *M, PwmOut *E, QEI *Enc, double speed)
 {
@@ -114,7 +64,7 @@
 
     t.start();
     do {
-        MotorPWM = speed - vel*0.5;
+        MotorPWM = speed - vel*0.7;
         if(MotorPWM > 0) {
             *M = 0;
             *E = MotorPWM;
@@ -127,13 +77,13 @@
         vel = (posC - posP)/t.read();
         t.reset();
         posP = posC;
-        pc.printf("v: %d hm: %d\n\r", (abs(vel) > 0.001), hasNotMoved);
+        pc.printf("v: %f hm: %d\n\r", MotorPWM, hasNotMoved);
         if(abs(vel) > 0.001) {
             hasNotMoved = 0;
         } else {
             hasNotMoved++;
         }
-    } while(abs(vel) > 0.001 || hasNotMoved < 10);
+    } while(hasNotMoved < 6);
     *E = 0;
 }