Diff: main.cpp

Revision:

30:9aeca82187f5

Parent:

29:3284cda80b4a

Child:

31:19bb96d3113e

--- a/main.cpp	Thu Dec 19 13:37:34 2019 +0000
+++ b/main.cpp	Thu Dec 19 15:38:22 2019 +0000
@@ -1,256 +1,233 @@
 //Enhancement 3//
 
 #include "mbed.h"
-
 //Motor PWM (speed)
 PwmOut PWMA(PA_8);
 PwmOut PWMB(PB_4);
-
 //Motor Direction
 DigitalOut DIRA(PA_9);
 DigitalOut DIRB(PB_10);
-
 //Hall-Effect Sensor Inputs
 DigitalIn HEA1(PB_2);
 DigitalIn HEA2(PB_1);
 DigitalIn HEB1(PB_15);
 DigitalIn HEB2(PB_14);
-
 //On board switch
 DigitalIn SW1(USER_BUTTON);
-
 //Use the serial object so we can use higher speeds
 Serial terminal(USBTX, USBRX);
-
 //Timer used for measuring speeds
 Timer timerA;
 Timer timerB;
-
 //Enumerated types
 enum DIRECTION   {FORWARD=0, REVERSE};
 enum PULSE       {NOPULSE=0, PULSE};
 enum SWITCHSTATE {PRESSED=0, RELEASED};
-
 //Duty cycles
-float dutyA = 0.2f; //100%
-float dutyB = 0.2f; //100%
-float speedA[3];
-float speedB[3];
+float dutyA, dutyB = 0.0f;
+//Speed Calculations
 float fA, fB = 0.0f;
-float sumA, sumB = 0.0f;
-int durA, durB = 0;
+float speedA, speedB = 0.0f;
 float TA[2];
 float TB[2];
 float TAA, TBB = 0.0f;
+//Distance Calculations
 float pulseA, pulseB = 0.0f;
 float travA, travB = 0.0f;
+
 void timeA() // this funtion calulates the rotation speed and distance of wheel A
 {
     static int n=0;            //Number of pulse sets
     static int HallState = 0;   //the hall effect current state
     if (n==0) {
         //Reset timer and Start
-        timerA.reset(); //resets timerA
-        timerA.start(); // starts timerA
-        TA[0] = timerA.read_us(); //reads timer from the beginning of the beginning of the first pulse
+        timerA.reset(); //Resets timerA
+        timerA.start(); //Starts timerA
+        TA[0] = timerA.read_us(); //Reads timer from the beginning of the first pulse
     }
     switch(HallState) {
         case 0:
             if(HEA1 == PULSE) {
-                HallState = 1; //change state
+                HallState = 1; //Change state
             }
             break;
         case 1:
             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
-                travA = ((176/20.8)/3)*pulseA; // gives the distance travelled by wheel A in mm
+                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
+                travA = ((176/20.8)/3)*pulseA; //Gives the distance travelled by wheel A in mm
             }
             break;
     }
     if (n < 4) return;
-    TA[1] = timerA.read_us(); //reads timer at the end of one shaft rotation
+    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
+    TAA = (TA[1]-TA[0]); //Time taken to do one shaft rotation
 
-    fA = 1.0f/ (TAA *(float)1.0E-6); //frequency of shaft rotation
-    sumA = fA/20.8;
+    fA = 1.0f/ (TAA *(float)1.0E-6); //Frequency of shaft rotation
+    speedA = fA/20.8; //wheel RPS
     //Reset count
     n=0;
 }
-void timeB() // this funtion calulates the rotation speed and distance of wheel A
+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;   //the hall effect current state
+    static int HallStateB = 0;   //The hall effect current state
     if (nB==0) {
         //Reset timer and Start
-        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
+        timerB.reset(); //Resets timerB
+        timerB.start(); //Starts timerB
+        TB[0] = timerB.read_us(); //Reads time from the beginning of the first pulse
     }
     switch(HallStateB) {
         case 0:
             if(HEB1 == PULSE) {
-                HallStateB = 1; //change state
+                HallStateB = 1; //Change state
             }
             break;
         case 1:
             if(HEB1 == NOPULSE) {
-                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
+                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 < 4) return;
-    TB[1] = timerB.read_us(); //reads timer at the end of one shaft rotation
+    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;
+    TBB = (TB[1]-TB[0]); //Time taken to do one shaft rotation
+    fB = 1.0f/ (TBB *(float)1.0E-6); //Frequency of shaft rotation
+    speedB = fB/20.8;  //Wheel RPS
     //Reset count
-    nB=0; 
+    nB=0;
 }
 void oneRPS()
 {
-    float deltaA = 1.0f-sumA;         //Error
-    float deltaB = 1.0f-sumB;
+    float deltaA = 1.0f-speedA;      //Error
+    float deltaB = 1.0f-speedB;      //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
+    //Lock the max and min values of duty
     dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
     dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
     dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
     dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
-    //Update duty cycle to correct in the first direction
+    //Update duty cycle to new value
     PWMA.write(dutyA);
     PWMB.write(dutyB);
 }
 void cornerRPS()
 {
-    float deltaA = 1.2f-sumA;         //Error
-    float deltaB = 0.55f-sumB;
+    float deltaA = 1.2f-speedA;       //Error
+    float deltaB = 0.55f-speedB;      //Error
     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
+    //Lock the maximum and minimum values of duty
     dutyA = (dutyA>1.0f) ? 1.0f : dutyA;
     dutyA = (dutyA<0.05f) ? 0.05f : dutyA;
     dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
     dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
-    //Update duty cycle to correct in the first direction
+    //Update duty cycle to new value
     PWMA.write(dutyA);
     PWMB.write(dutyB);
 }
-void reset()  // this fuction restes distnace travelled and pulses to 0 allowing fo a new distance measurement
+void reset()  //This fuction resets the distnace travelled and pulses to 0 allowing fo a new distance measurement
 {
     travA = 0;
     travB = 0;
     pulseA = 0;
     pulseB = 0;
 }
-void straight()  // this sets the wheel speed to roughly 1rps so the program doesnt have make major adjustments from the beginning
+void straight()  //This sets the wheel speed to roughly 1rps so the program doesnt have make major adjustments from the beginning
 {
     dutyA = 0.463f;
     dutyB = 0.457f;
     PWMA.write(dutyA);          //Set duty cycle (%)
     PWMB.write(dutyB);
 }
-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
+void turn() //This sets the wheel speed to roughly what is needed to make the turn so that the program doesnt have to make any major adjustments
 {
     dutyA = 0.556f;
     dutyB = 0.3f;
     PWMA.write(dutyA);          //Set duty cycle (%)
     PWMB.write(dutyB);
 }
-
 int main()
 {
-
     //Configure the terminal to high speed
     terminal.baud(115200);
-
     //Set initial motor direction
     DIRA = FORWARD;
     DIRB = FORWARD;
-
     //Set motor period to 100Hz
     PWMA.period_ms(10);
     PWMB.period_ms(10);
-
     //Set initial motor speed to stop
     PWMA.write(0.0f);           //0% duty cycle
     PWMB.write(0.0f);           //0% duty cycle
-
     //Wait for USER button (blue pull-down switch) to start
     terminal.puts("Press USER button to start");
-
     while (SW1 == RELEASED);
-    wait(0.5);
-    //Wait - give time to start running
+    //Wait - so buggy doesn't move immediately
     wait(1.0);
-    //Main polling loop
-
+    //Main loop
     while(1) {
-        dutyA = 0.463f;
-        dutyB = 0.457f;
-        PWMA.write(dutyA);          //Set duty cycle (%)
-        PWMB.write(dutyB);
-        oneRPS();
-        timeA();
-        timeB();
-        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();
-            timeB();
-            oneRPS();
-            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
+        straight();   //starts the straight function
+        oneRPS();     //starts oneRPS function
+        timeA();      //TimeA function to get wheel speed and distance
+        timeB();      //TimeB function to get wheel speed and distance
+        terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
+        while(travA <= 1250) {      //starts moving from 0mm to 1250mm (distance of wheel travel)
+            timeA();    //Starts time A
+            timeB();    //starts time B
+            oneRPS();   //starts oneRPS function
+            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
         }
-        reset();
-        turn();
-        while(travA <= 597) {
-            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);
+        reset();       //activates reset function
+        turn();        //starts turn function
+        while(travA <= 597) {   //moves from 0mm to 597mm (distance of outside wheel travel)
+            timeA();   //starts time A
+            timeB();   //starts time B
+            cornerRPS();  //starts CornerRPS function
+            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
         }
-        reset();
-        straight();
-        while(travA <= 1457) {
-            timeB();
-            timeA();
-            oneRPS();
-            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
+        reset();        //activates reset function
+        straight();     //starts the straight function
+        while(travA <= 1457) { //starts moving from 0mm to 1457mm (distance of wheel travel)
+            timeB();    //starts time A
+            timeA();    //starts time B
+            oneRPS();   //starts oneRPS function
+            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
         }
-        reset();
-        turn();
-        while(travA <= 453.8) {
-            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);
+        reset();    //activates reset function
+        turn();        //starts turn function
+        while(travA <= 453.8) { //starts moving from 0mm to 453.8mm (distance of wheel travel)
+            timeA();        //starts time A
+            timeB();        //starts time B
+            cornerRPS(); ///starts CornerRPS function
+            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
         }
-        reset();
-        straight();
-        while(travA <= 750) {
-            timeB();
-            timeA();
-            oneRPS();
-            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", sumA, sumB, travA, travB);
+        reset();        //activates reset function
+        straight();     //starts the straight function
+        while(travA <= 750) {     //starts moving from 0mm to 750mm (distance of wheel travel)
+            timeB();        //starts time A
+            timeA();        //starts time B
+            oneRPS();       //starts oneRPS function
+            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
         }
-        reset();
-        turn();
-        while(travA <= 349) {
-            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);
+        reset();        //activates reset function
+        turn();         //starts turn function
+        while(travA <= 350.3) {   //starts moving from 0mm to 349mm (distance of wheel travel)
+            timeA();        //starts time A
+            timeB();        //starts time B
+            cornerRPS(); //starts CornerRPS function
+            terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distanceA: %6.2f \t distanceB: %6.2f\n\r", speedA, speedB, travA, travB);
         }
         break;
     }
-    PWMA.write(0.0f);
-    PWMB.write(0.0f);
-
+    PWMA.write(0.0f);       //tells wheel A to stop
+    PWMB.write(0.0f);       //tells wheel B to stop
 }
\ No newline at end of file