Diff: main.cpp

Revision:

21:d553c43a5a26

Parent:

20:c89685cd0b02

Child:

22:568a6d19b98a

--- a/main.cpp	Tue Dec 10 10:51:17 2019 +0000
+++ b/main.cpp	Tue Dec 10 12:12:19 2019 +0000
@@ -1,11 +1,8 @@
-/*
-Our version
-*/
+//Enhancement 2//
+//Enhancement 2//
+
 #include "mbed.h"
 
-//Status LED
-DigitalOut led(LED1);
-
 //Motor PWM (speed)
 PwmOut PWMA(PA_8);
 PwmOut PWMB(PB_4);
@@ -20,7 +17,6 @@
 DigitalIn HEB1(PB_15);
 DigitalIn HEB2(PB_14);
 
-
 //On board switch
 DigitalIn SW1(USER_BUTTON);
 
@@ -36,9 +32,6 @@
 enum PULSE       {NOPULSE=0, PULSE};
 enum SWITCHSTATE {PRESSED=0, RELEASED};
 
-//Debug GPIO
-DigitalOut probe(D10);
-
 //Duty cycles
 float dutyA = 1.0f; //100%
 float dutyB = 1.0f; //100%
@@ -49,130 +42,102 @@
 int tB2[2];
 float dis;
 float trav =0;
-float speedA, speedB = 0;
-int countA, countB = 0;
-void HallA()
-        {
-            //Reset timer and Start
+float speedA, speedB = 1.0f;
+
+int pulse = 0;
+
+void timeA()
+{
+    static int n=0;            //Number of pulse sets
+    static int HallState = 0;
+    static float T = 0.0f;
+    //**********************
+    //TIME THE FULL SEQUENCE
+    //**********************
+    if (n==0) {
+        //Reset timer and Start
         timerA.reset();
-        timerA.start();
-        bool all = true;
-        //**********************
-        //TIME THE FULL SEQUENCE
-        //**********************
-        int HallStateA = 0;
-        while(all)
-        {
-            switch(HallStateA)
+        timerA.start();  
+        T = timerA.read_us();      
+    }
+    switch(HallState)
             {
                 case 0:
-                if(HEA1 == NOPULSE){
-                    HallStateA = 1;
-                    tA1[0] = timerA.read_us();
+                if(HEA1 == PULSE){
+                    HallState = 1;
                 }break;
                 case 1:
-                if(HEA2 == NOPULSE){
-                    HallStateA = 2;
-                    tA2[0] = timerA.read_us();
+                if(HEA2 == PULSE){
+                    HallState = 2;
                 }break;
                 case 2:
-                if(HEA1 == PULSE){
-                    HallStateA = 3;
-                    tA1[1] = timerA.read_us();
+                if(HEA1 == NOPULSE){
+                    HallState = 3;
                 }break;
                 case 3:
-                if(HEA2 == PULSE){
-                    HallStateA = 0;
-                    all = false;
-                    tA2[1] = timerA.read_us();
+                if(HEA2 == NOPULSE){
+                    HallState = 0;
+                    n++;
                 }break;
             }
-        }
-        countA++;
-        //Calculate the frequency of rotation
-        float TA1 = 2.0f * (tA1[1]-tA1[0]);
-        float TA2 = 2.0f * (tA2[1]-tA2[0]);
-        float TA = (TA1 + TA2) * 0.5f;
-        trav = (20.8*countA);
-        float fA = 1.0f/ (TA *(float)3.0E-6);
-        speedA = fA/20.8;
-        terminal.printf("WheelA: %6.2f \t WheelB: %6.2f \t pulses: %d \t trav: %6.2f\n", speedA, speedB, countA, trav);
-        if(countA >=48){
-            PWMA.write(0.0f);
-            PWMB.write(0.0f);
-            wait(10000);
-        }
-        }
+    if (n < 9) return;
+    T = timerA.read_us();
+    T = T;    
+    // Calculate speeed and adapt
+    speedA = ((175.9/20.8)/3) / T;
+    //Reset count
+    n=0;
+    }
 
-void HallB()
-        {
-            //Reset timer and Start
+void timeB()
+{
+    static int n=0;            //Number of pulse sets
+    static int HallState = 0;
+    static float TB = 0.0f;
+    //**********************
+    //TIME THE FULL SEQUENCE
+    //**********************
+    if (n==0) {
+        //Reset timer and Start
         timerB.reset();
-        timerB.start();
-        bool allB = true;
-        //**********************
-        //TIME THE FULL SEQUENCE
-        //**********************
-        int HallStateB = 0;
-        while(allB)
-        {
-            switch(HallStateB)
+        timerB.start();  
+        TB = timerB.read_us();      
+    }
+    switch(HallState)
             {
                 case 0:
-                if(HEB1 == NOPULSE){
-                    HallStateB = 1;
-                    tB1[0] = timerB.read_us();
+                if(HEB1 == PULSE){
+                    HallState = 1;
                 }break;
                 case 1:
-                if(HEB2 == NOPULSE){
-                    HallStateB = 2;
-                    tB2[0] = timerB.read_us();
+                if(HEB2 == PULSE){
+                    HallState = 2;
                 }break;
                 case 2:
-                if(HEB1 == PULSE){
-                    HallStateB = 3;
-                    tB1[1] = timerB.read_us();
+                if(HEB1 == NOPULSE){
+                    HallState = 3;
                 }break;
                 case 3:
-                if(HEB2 == PULSE){
-                    HallStateB = 0;
-                    allB = false;
-                    countB++;
-                    tB2[1] = timerB.read_us();
+                if(HEB2 == NOPULSE){
+                    HallState = 0;
+                    n++;
                 }break;
             }
-        }
-        //Calculate the frequency of rotation
-        float TB1 = 2.0f * (tB1[1]-tB1[0]);
-        float TB2 = 2.0f * (tB2[1]-tB2[0]);
-        float TB = (TB1 + TB2) * 0.5f;
-        float fB = 1.0f/ (TB *(float)3.0E-6);
-        speedB = fB/20.8;
-        }
+    if (n < 9) return;
+    TB = timerB.read_us();   
+    TB = TB; 
+    // Calculate speeed and adapt
+    speedB = ((175.9/20.8)/3) / TB;
+    //Reset count
+    n=0;
+    }
 
-void oneRPS(){   
-        float deltaA = 1.0f-speedA;         //Error
-        float deltaB = 1.0f-speedB;
-        dutyA = dutyA + deltaA*0.1f;    //Increase duty in proportion to the error
-        dutyB = dutyB + deltaB*0.1f;    //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;
-        dutyB = (dutyB>1.0f) ? 1.0f : dutyB;
-        dutyB = (dutyB<0.05f) ? 0.05f : dutyB;
-        //Update duty cycle to correct in the first direction
-        PWMA.write(dutyA);                  
-        PWMB.write(dutyB); 
-}
+
 int main()
 {
 
     //Configure the terminal to high speed
-    terminal.baud(9600);
-    
-    terminal.printf("Hello\n\r");
-
-    
+    terminal.baud(115200);
 
     //Set initial motor direction
     DIRA = FORWARD;
@@ -188,22 +153,67 @@
 
     //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 - give time to start running
-    wait(1.0);
-    //Set initial motor speed to stop
+    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);
     //Main polling loop
+
+    float paceA = 1.0f;
+    float paceB = 1.0f;
+    PWMA.write(paceA);          //Set duty cycle (%)
+    PWMB.write(paceB);
     while(1)
     {
-        HallA();
-        HallB();
-        oneRPS();
+        timeA();
+        timeB();
+        terminal.printf("wheelA: %4.2f \t wheelB: %4.2f \t \n\r", speedA, speedB);
+        if (speedA <= 1.0f)
+        {
+            paceA +=0.005f;
+        }
+        if (speedA >= 1.0f)
+        {
+            paceA -=0.005f;
+        }
+        if (speedB <= 1.0f)
+        {
+            paceB +=0.005f;
+        }
+        if (speedB >= 1.0f)
+        {
+            paceB -=0.005f;
+        }
+        PWMA.write(paceA);
+        PWMB.write(paceB);
     }
-    PWMA.write(0.0f);
-    PWMB.write(0.0f);
-}
+}
\ No newline at end of file