Diff: main.cpp

Revision:

1:7cafc9042056

Parent:

0:f492ec86159e

Child:

2:83dd9068b6c5

--- a/main.cpp	Fri Oct 25 08:07:43 2013 +0000
+++ b/main.cpp	Fri Oct 25 09:22:16 2013 +0000
@@ -52,38 +52,51 @@
     /* variable to store pwm value in*/
     float pwm_to_motorA;
     float pwm_to_motorB;
-    int32_t beginpositiemotorA_t0;
-    int32_t beginpositiemotorB_t0;
+    int32_t positionmotorA_t0;
+    int32_t positionmotorB_t0;
+    int32_t positionmotorA_t_1;
+    int32_t positionmotorB_t_1;
+    int32_t startingpositionmotorA;
+    int32_t startingpositionmotorB;
+    int32_t velocity_motorA;
+    int32_t velocity_motorB;
+
     //START OF CODE
 
     /*Set the baudrate (use this number in RealTerm too! */
     pc.baud(921600);
 
 
-// wil dat 20 seconden de motor rechtsom gaat draaien
+    // in dit stukje code zorgen we ervoor dat de startpositie wordt bereikt.
     motordirA.write(0);
+    pwm_motorA.write(.05);
+    positionmotorA_t0 = motorA.getPosition();
+    do {
+        wait(0.02);
+        positionmotorA_t_1 = positionmotorA_t0 ;
+        positionmotorA_t0 = motorA.getPosition();
+        velocity_motorA = (positionmotorA_t0 - positionmotorA_t_1) / 0.02;
+    } while(velocity_motorA <= 0.01);
+    startingpositionmotorA = motorA.getPosition();
+
     motordirB.write(0);
-    pwm_motorA.write(.1);
-    pwm_motorB.write(.1);
-    
-    do
-    {
-     //wacht
-     //schuif t0 / t1
-     lees encoder
-     
-    }while(posititie ongelijk);
-            
-    
-    
-    
-    
+    pwm_motorB.write(.05);
+    positionmotorB_t0 = motorB.getPosition();
+    do {
+        wait(0.02);
+        positionmotorB_t_1 = positionmotorB_t0;
+        positionmotorB_t0 = motorB.getPosition();
+        velocity_motorB = (positionmotorB_t0 - positionmotorB_t_1) / 0.02;
+    } while(velocity_motorB <= 0.01);
+    startingpositionmotorB = motorB.getPosition();
+
+
     /*Create a ticker, and let it call the     */
     /*function 'setlooptimerflag' every 0.01s  */
     Ticker looptimer;
     looptimer.attach(setlooptimerflag,0.01);
 
-        //INFINITE LOOP
+    //INFINITE LOOP
     while(1) {
         /* Wait until looptimer flag is true. */
         /* '!=' means not-is-equal            */