Diff: main.cpp

Revision:

28:b650e7f6c269

Parent:

27:44ab9ebf07eb

Child:

29:3284cda80b4a

--- a/main.cpp	Wed Dec 18 16:58:47 2019 +0000
+++ b/main.cpp	Thu Dec 19 12:59:48 2019 +0000
@@ -56,12 +56,12 @@
     }
     switch(HallState) {
         case 0:
-            if(HEA1 == NOPULSE) {
+            if(HEA1 == PULSE) {
                 HallState = 1; //change state
             }
             break;
         case 1:
-            if(HEA1 == PULSE) {
+            if(HEA1 == NOPULSE) {
                 HallState = 0; //change state
                 n++; // add 1 to the number of pulses counted (resets after 9 pulsees)
                 pulseA++; // add 1 to the number of pulses counted for the duration
@@ -69,91 +69,57 @@
             }
             break;
     }
-    if (n < 9) return;  //returns to main until n=9
-    TA[1] = timerA.read_us(); // time after 9 pulses
-    TAA = (TA[1]-TA[0]); // time taken to do 9 pulses (3 shaft rotations)
+    if (n < 4) return;
+    TA[1] = timerA.read_us(); //reads timer at the end of one shaft rotation
+    timerA.stop();
+    TAA = (TA[1]-TA[0]); // time taken to do one shaft rotation
 
-    fA = 2.0f/ (TAA *(float)1.0E-6); //frequency of shaft rotation
-    //calcultes speed
-    if(durA == 0) {
-        speedA[durA] = fA/20.8;
-        durA++;
-        return;
-    } else if(durA == 1) {
-        speedA[durA] = fA/20.8;
-        durA++;
-        return;
-    } else if(durA == 2) {
-        speedA[durA] = fA/20.8;
-        durA = 0;
-    }
-    for(int xA=0; xA<3; xA++) {
-        sumA+=speedA[xA];
-    }
-    sumA = sumA/3;
+    fA = 1.0f/ (TAA *(float)1.0E-6); //frequency of shaft rotation
+    sumA = fA/20.8;
     //Reset count
-    n=0; //note to mkike put this after each if statements
+    n=0;
 }
-
-void timeB()
+void timeB() // this funtion calulates the rotation speed and distance of wheel A
 {
     static int nB=0;            //Number of pulse sets
-    static int HallStateB = 0;
-    //**********************
-    //TIME THE FULL SEQUENCE
-    //**********************
+    static int HallStateB = 0;   //the hall effect current state
     if (nB==0) {
         //Reset timer and Start
-        timerB.reset();
-        timerB.start();
-        TB[0] = timerB.read_us();
+        timerB.reset(); //resets timerA
+        timerB.start(); // starts timerA
+        TB[0] = timerB.read_us(); //reads timer from the beginning of the beginning of the first pulse
     }
     switch(HallStateB) {
         case 0:
             if(HEB1 == PULSE) {
-                HallStateB = 1;
+                HallStateB = 1; //change state
             }
             break;
         case 1:
             if(HEB1 == NOPULSE) {
-                HallStateB = 0;
-                nB++;
-                pulseB++;
-                travB = ((176/20.8)/3)*pulseB;
+                HallStateB = 0; //change state
+                nB++; // add 1 to the number of pulses counted (resets after 9 pulsees)
+                pulseB++; // add 1 to the number of pulses counted for the duration
+                travB = ((176/20.8)/3)*pulseB; // gives the distance travelled by wheel B in mm
             }
             break;
     }
-    if (nB < 9) return;
-    TB[1] = timerB.read_us();
-    TBB = (TB[1]-TB[0]);
-    // Calculate speeed
-    fB = 2.0f/ (TBB *(float)1.0E-6);
-    if(durB == 0) {
-        speedB[durB] = fB/20.8;
-        durB++;
-        return;
-    } else if(durB == 1) {
-        speedB[durB] = fB/20.8;
-        durB++;
-        return;
-    } else if(durB == 2) {
-        speedB[durB] = fB/20.8;
-        durB = 0;
-    }
-    for(int xB=0; xB<3; xB++) {
-        sumB+=speedB[xB];
-    }
-    sumB = sumB/3;
+    if (nB < 4) return;
+    TB[1] = timerB.read_us(); //reads timer at the end of one shaft rotation
+    timerB.stop();
+    TBB = (TB[1]-TB[0]); // time taken to do one shaft rotation
+
+    fB = 1.0f/ (TBB *(float)1.0E-6); //frequency of shaft rotation
+    sumB = fB/20.8;
     //Reset count
-    nB=0; // note to mike put this in if statements
+    nB=0; 
 }
-
 void oneRPS()
 {
     float deltaA = 1.0f-sumA;         //Error
     float deltaB = 1.0f-sumB;
-    dutyA = dutyA + deltaA*0.0001f;    //Increase duty in proportion to the error
-    dutyB = dutyB + deltaB*0.0001f;    //Increase duty in proportion to the error
+    dutyA = dutyA + deltaA*0.01f;    //Increase duty in proportion to the error
+    dutyB = dutyB + deltaB*0.01f;    //Increase duty in proportion to the error
     //Clamp the max and min values of duty and 0.0 and 1.0 respectively
     dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
     dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
@@ -166,9 +132,9 @@
 void cornerRPS()
 {
     float deltaA = 1.2f-sumA;         //Error
-    float deltaB = 0.8f-sumB;
-    dutyA = dutyA + deltaA*0.0001f;    //Increase duty in proportion to the error
-    dutyB = dutyB + deltaB*0.0001f;    //Increase duty in proportion to the error
+    float deltaB = 0.55f-sumB;
+    dutyA = dutyA + deltaA*0.001f;    //Increase duty in proportion to the error
+    dutyB = dutyB + deltaB*0.001f;    //Increase duty in proportion to the error
     //Clamp the max and min values of duty and 0.0 and 1.0 respectively
     dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
     dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
@@ -194,11 +160,12 @@
 }
 void turn() // this sets the weheel speed to roughly what is needed to make the turn so that the program doesnt have to make any major adjustments
 {
-    dutyA = 0.5556f;
-    dutyB = 0.28f;
+    dutyA = 0.556f;
+    dutyB = 0.27f;
     PWMA.write(dutyA);          //Set duty cycle (%)
     PWMB.write(dutyB);
 }
+
 int main()
 {
 
@@ -225,14 +192,15 @@
     //Wait - give time to start running
     wait(1.0);
     //Main polling loop
-    dutyA = 0.463f;
-    dutyB = 0.457f;
-    PWMA.write(dutyA);          //Set duty cycle (%)
-    PWMB.write(dutyB);
+
     while(1) {
+        dutyA = 0.463f;
+        dutyB = 0.457f;
+        PWMA.write(dutyA);          //Set duty cycle (%)
+        PWMB.write(dutyB);
+        oneRPS();
         timeA();
         timeB();
-        cornerRPS();
         terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
         while(travA <= 1250) {
             timeA();
@@ -250,7 +218,7 @@
         }
         reset();
         straight();
-        while(travA <= 1118) {
+        while(travA <= 1457) {
             timeB();
             timeA();
             oneRPS();
@@ -258,7 +226,7 @@
         }
         reset();
         turn();
-        while(travA <= 453.7) {
+        while(travA <= 453.8) {
             timeA();
             timeB();
             cornerRPS();
@@ -266,7 +234,7 @@
         }
         reset();
         straight();
-        while(travA <= 500) {
+        while(travA <= 750) {
             timeB();
             timeA();
             oneRPS();
@@ -274,7 +242,7 @@
         }
         reset();
         turn();
-        while(travA <= 350.29) {
+        while(travA <= 349) {
             timeA();
             timeB();
             cornerRPS();