Diff: main.cpp

Revision:

17:a92d96b65cbc

Parent:

16:afe936377d66

Child:

18:11937e78239c

--- a/main.cpp	Wed Nov 20 14:40:26 2019 +0000
+++ b/main.cpp	Tue Nov 26 16:34:50 2019 +0000
@@ -1,119 +1,63 @@
 //Enhancement 2//
-//Enhancement 2//
-
 #include "mbed.h"
-
-//Status LED
-DigitalOut led(LED1);
-
 //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 timer;
+Timer timerA1;
+Timer timerA2;
+Timer timerB1;
+Timer timerB2;
 Timer timer1;
-
 //Enumerated types
 enum DIRECTION   {FORWARD=0, REVERSE};
 enum PULSE       {NOPULSE=0, PULSE};
 enum SWITCHSTATE {PRESSED=0, RELEASED};
-
 //Duty cycles
-float dutyA = 1.0f; //100%
-float dutyB = 1.0f; //100%
-//Array of sensor data
-int tA1[2];
-int tA2[2];
-int tB1[2];
-int tB2[2];
+float dutyA;
+float dutyB;
 float dis;
 float trav =0;
 float speedA, speedB = 1.0f;
-float paceA = 1.0f;
-float paceB = 1.0f;
+float paceA, paceB = 1.0f;
+int tmrA1[2];
+int tmrA2[2];
+int tmrB1[2];
+int tmrB2[2];
 void time()
-    {
-        //Reset timer and Start
-    timer.reset();
-    timer.start();
-
-    //**********************
-    //TIME THE FULL SEQUENCE
-    //**********************
-
-    //Wait for rising edge of A1 and log time
-    while (HEA1 == NOPULSE);
-    tA1[0] = timer.read_us();
-    //Wait for rising edge of A2 and log time (30 degrees?)
-    while (HEA2 == NOPULSE);
-    tA2[0] = timer.read_us();
-    //Wait for falling edge of A1
-    while (HEA1 == PULSE);
-    tA1[1] = timer.read_us();
-    //Wait for falling edge of A2
-    while (HEA2 == PULSE);
-    tA2[1] = timer.read_us();
-    //Wait for rising edge of B1 and log time
-    while (HEB1 == NOPULSE);
-    tB1[0] = timer.read_us();
-    //Wait for rising edge of B2 and log time (30 degrees?)
-    while (HEB2 == NOPULSE);
-    tB2[0] = timer.read_us();
-    //Wait for falling edge of B1
-    while (HEB1 == PULSE);
-    tB1[1] = timer.read_us();
-    //Wait for falling edge of B2
-    while (HEB2 == PULSE);
-    tB2[1] = timer.read_us();
-
-    terminal.printf("tA1(0) = %d\n", tA1[0]);
-    terminal.printf("tA1(1) = %d\n", tA1[1]);
-    terminal.printf("tA2(0) = %d\n", tA2[0]);
-    terminal.printf("tA2(1) = %d\n", tA2[1]);
-
-    //Calculate the frequency of rotation for A
-    float TA1 = 2.0f * (tA1[1]-tA1[0]);
-    float TA2 = 2.0f * (tA2[1]-tA2[0]);
+{
+    terminal.printf("tmrA1(0) = %d\n", tmrA1[0]);
+    terminal.printf("tmrA1(1) = %d\n", tmrA1[1]);
+    terminal.printf("tmrB1(0) = %d\n", tmrA2[0]);
+    terminal.printf("tmrB1(1) = %d\n", tmrA2[1]);
+    float TA1 = 2.0f * (tmrA1[1]-tmrA1[0]);
+    float TA2 = 2.0f * (tmrA2[1]-tmrA2[0]);
     float TA = (TA1 + TA2) * 0.5f;
-
-    //Calculate the frequency of rotation for B
-    float TB1 = 2.0f * (tB1[1]-tB1[0]);
-    float TB2 = 2.0f * (tB2[1]-tB2[0]);
+    float TB1 = 2.0f * (tmrB1[1]-tmrB1[0]);
+    float TB2 = 2.0f * (tmrB2[1]-tmrB2[0]);
     float TB = (TB1 + TB2) * 0.5f;
-
     dis = timer1.read_us();
-    float mm = ((TA*3)*20.8)/175.9;
+    float mm = (((((TA*0.3)*20.8)/175.9)+(((TB*0.3)*20.8)/175.9)))*0.5f;
     trav = dis/mm;
-    float fA = 1.0f/ (TA *(float)3.0E-6); // FOR A
-    float fB = 1.0f/ (TB *(float)3.0E-6); // FOR B
-    terminal.printf("Wheel A: %6.2f \t Wheel B: %6.2f \t Distance travelled: %6.2f\n", fA/20.8f, fB/20.8f, trav);
+    float fA = 1.0f/ (TA *(float)3.0E-7); // FOR A
+    float fB = 1.0f/ (TB *(float)3.0E-7); // FOR B
+    terminal.printf("Wheel A: %6.2f \t Wheel B: %6.2f \t Distance travelled: %6.2f\n",fA/20.8f, fB/20.8f, trav);
     speedA = fA/20.8f;
     speedB = fB/20.8f;
-    }
-
-void reset()
-{
-    timer1.reset();
-    time();
 }
-
+//Rotational speed towards 1rps
 void oneRPS()
 {
     time();
@@ -143,74 +87,110 @@
 
 int main()
 {
-
+    int hallStateA = 0;
+    int hallStateB = 0;
     //Configure the terminal to high speed
-    terminal.baud(115200);
-
+    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");
-    led = 0;
     while (SW1 == RELEASED);
-    led = 1;
-    wait(0.5);
-     //Set align wheels
-    PWMA.write(1.0f);          //Set duty cycle (%)
-    PWMB.write(1.0f);          //Set duty cycle (%)
-    //*********************************************************************
-    //FIRST TIME - SYNCHRONISE (YOU SHOULD NOT NEED THIS ONCE IT's RUNNING)
-    //*********************************************************************
-
-    //Wait for rising edge of A1 and log time
-    while (HEA1 == NOPULSE);
-    //Wait for rising edge of A2 and log time (30 degrees?)
-    while (HEA2 == NOPULSE);
-    //Wait for falling edge of A1
-    while (HEA1 == PULSE);
-    //Wait for falling edge of A2
-    while (HEA2 == PULSE);
-    //Wait for rising edge of B1 and log time
-    while (HEB1 == NOPULSE);
-    //Wait for rising edge of B2 and log time (30 degrees?)
-    while (HEB2 == NOPULSE);
-    //Wait for falling edge of B1
-    while (HEB1 == PULSE);
-    //Wait for falling edge of B2
-    while (HEB2 == PULSE);
-
-    //Set initial motor speed to stop
-    PWMA.write(0.0f);          //Set duty cycle (%)
-    PWMB.write(0.0f);          //Set duty cycle (%)
-
     //Wait - give time to start running
     wait(1.0);
+    PWMA.write(1.0f);           //100% duty cycle
+    PWMB.write(1.0f);
     timer1.reset();
     timer1.start();
-    //Main polling loop
-
-
-    PWMA.write(paceA);          //Set duty cycle (%)
-    PWMB.write(paceB);
     while(1)
     {
-        
-        while (trav <= 1250)
+        int passA1 = HEA1;
+        int passA2 = HEA2;
+        int passB1 = HEB1;
+        int passB2 = HEB2;
+        time();
+        switch (hallStateA)
         {
-            oneRPS();
+            case 0:
+                if (passA1 ==1)
+                {
+                    hallStateA = 1;
+                    timerA1.reset();
+                    timerA1.start();
+                    tmrA1[0] = timerA1.read_us();
+                }
+                break;
+            case 1:
+                if (passA2 ==1)
+                {
+                    hallStateA = 2;
+                    timerA2.reset();
+                    timerA2.start();
+                    tmrA2[0] = timerA2.read_us();
+                }
+                break;
+            case 2:
+                if (passA1 ==0)
+                {
+                    hallStateA = 3;
+                    tmrA1[1] = timerA1.read_us();
+                    timerA1.stop();
+                }
+                break;
+            case 3:
+                if (passA2 ==0)
+                {
+                    hallStateA = 0;
+                    tmrA2[1] = timerA2.read_us();
+                    timerA2.stop();
+                }
+                break;
         }
-    PWMA.write(0.0f);          //Set duty cycle (%)
-    PWMB.write(0.0f);
-    break;
+        
+        
+        switch (hallStateB)
+        {
+            case 0:
+                if (passB1 ==1)
+                {
+                    hallStateB = 1;
+                    timerB1.reset();
+                    timerB1.start();
+                    tmrB1[0] = timerB1.read_us();
+                }
+                break;
+            case 1:
+                if (passB2 ==1)
+                {
+                    hallStateB = 2;
+                    timerB2.reset();
+                    timerB2.start();
+                    tmrB2[0] = timerB2.read_us();
+                }
+                break;
+            case 2:
+                if (passB1 ==0)
+                {
+                    hallStateB = 3;
+                    tmrB1[1] = timerB1.read_us();
+                    timerB1.stop();
+                }
+                break;
+            case 3:
+                if (passB2 ==0)
+                {
+                    hallStateB = 0;
+                    tmrB2[1] = timerB2.read_us();
+                    timerB2.stop();
+                }
+                break;
+        }
     }
 }
\ No newline at end of file