Diff: main.cpp

Revision:

14:e66cf781f5b9

Parent:

13:29a994566dc5

Child:

15:b7ca53716b05

--- a/main.cpp	Tue Nov 19 15:24:59 2019 +0000
+++ b/main.cpp	Tue Nov 19 16:44:00 2019 +0000
@@ -1,1 +1,202 @@
-//Enhancement 2//
\ No newline at end of file
+//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 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 dis;
+float trav =0;
+float speedA, speedB = 1.0f;
+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]);
+    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 TB = (TB1 + TB2) * 0.5f;
+
+    dis = timer1.read_us();
+    float mm = ((TA*3)*20.8)/175.9;
+    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);
+    speedA = fA/20.8f;
+    speedB = fB/20.8f;
+    }
+         
+void reset()
+{
+    timer1.reset();
+    time();
+}
+         
+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");
+    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);
+    timer1.reset();
+    timer1.start();
+    //Main polling loop
+    
+    float paceA = 1.0f;
+    float paceB = 1.0f;
+    PWMA.write(paceA);          //Set duty cycle (%)
+    PWMB.write(paceB);
+    while(1)
+    {
+        time();
+        if (speedA <= 1.0f)
+        {
+            paceA +=0.01f;
+            wait_ms(25);
+        }
+        if (speedA >= 1.0f)
+        {
+            paceA -=0.01f;
+            wait_ms(25);
+        }
+        if (speedB <= 1.0f)
+        {
+            paceB +=0.01f;
+            wait_ms(25);
+        }
+        if (speedB >= 1.0f)
+        {
+            paceB -=0.01f;
+            wait_ms(25);
+        }
+        PWMA.write(paceA);
+        PWMB.write(paceA);
+    }
+}
\ No newline at end of file