Diff: main.cpp

Revision:

26:072ab2309eec

Parent:

25:50d3f80cb763

Child:

27:44ab9ebf07eb

--- a/main.cpp	Wed Dec 11 16:32:28 2019 +0000
+++ b/main.cpp	Wed Dec 18 16:11:53 2019 +0000
@@ -1,5 +1,4 @@
-//Enhancement 2//
-//Enhancement 2//
+//Enhancement 3//
 
 #include "mbed.h"
 
@@ -35,11 +34,6 @@
 //Duty cycles
 float dutyA = 0.2f; //100%
 float dutyB = 0.2f; //100%
-//Array of sensor data
-int tA1[2];
-int tA2[2];
-int tB1[2];
-int tB2[2];
 float speedA[3];
 float speedB[3];
 float fA, fB = 0.0f;
@@ -48,62 +42,57 @@
 float TA[2];
 float TB[2];
 float TAA, TBB = 0.0f;
-float pulse = 0.0f;
-float trav = 0.0f;
-float rpsA, rpsB = 0.0f;
+float pulseA, pulseB = 0.0f;
+float travA, travB = 0.0f;
 void timeA()
 {
     static int n=0;            //Number of pulse sets
     static int HallState = 0;
-    //**********************
-    //TIME THE FULL SEQUENCE
-    //**********************
     if (n==0) {
         //Reset timer and Start
         timerA.reset();
-        timerA.start();  
-        TA[0] = timerA.read_us();      
+        timerA.start();
+        TA[0] = timerA.read_us();
     }
-    switch(HallState)
-            {
-                case 0:
-                if(HEA1 == PULSE){
-                    HallState = 1;
-                }break;
-                case 1:
-                if(HEA1 == NOPULSE){
-                    HallState = 0;
-                    n++;
-                    pulse++;
-                    trav = ((176/20.8)/3)*pulse;
-                }break;
+    switch(HallState) {
+        case 0:
+            if(HEA1 == NOPULSE) {
+                HallState = 1;
             }
+            break;
+        case 1:
+            if(HEA1 == PULSE) {
+                HallState = 0;
+                n++;
+                pulseA++;
+                travA = ((176/20.8)/3)*pulseA;
+            }
+            break;
+    }
     if (n < 9) return;
-    TA[1] = timerA.read_us(); 
+    TA[1] = timerA.read_us();
     TAA = (TA[1]-TA[0]);
     // Calculate speeed
     fA = 2.0f/ (TAA *(float)1.0E-6);
-    if(durA == 0){
+    if(durA == 0) {
         speedA[durA] = fA/20.8;
         durA++;
         return;
-    }
-    else if(durA == 1){
+    } else if(durA == 1) {
         speedA[durA] = fA/20.8;
         durA++;
         return;
-    }
-    else if(durA == 2){
+    } else if(durA == 2) {
         speedA[durA] = fA/20.8;
         durA = 0;
     }
-    for(int xA=0;xA<3;xA++){
-      sumA+=speedA[xA];
+    for(int xA=0; xA<3; xA++) {
+        sumA+=speedA[xA];
     }
     sumA = sumA/3;
     //Reset count
     n=0;
-    }
+}
 
 void timeB()
 {
@@ -115,53 +104,55 @@
     if (nB==0) {
         //Reset timer and Start
         timerB.reset();
-        timerB.start();  
-        TB[0] = timerB.read_us();      
+        timerB.start();
+        TB[0] = timerB.read_us();
     }
-    switch(HallStateB)
-            {
-                case 0:
-                if(HEB1 == PULSE){
-                    HallStateB = 1;
-                }break;
-                case 1:
-                if(HEB1 == NOPULSE){
-                    HallStateB = 0;
-                    nB++;
-                }break;
+    switch(HallStateB) {
+        case 0:
+            if(HEB1 == PULSE) {
+                HallStateB = 1;
             }
+            break;
+        case 1:
+            if(HEB1 == NOPULSE) {
+                HallStateB = 0;
+                nB++;
+                pulseB++;
+                travB = ((176/20.8)/3)*pulseB;
+            }
+            break;
+    }
     if (nB < 9) return;
-    TB[1] = timerB.read_us();   
+    TB[1] = timerB.read_us();
     TBB = (TB[1]-TB[0]);
     // Calculate speeed
     fB = 2.0f/ (TBB *(float)1.0E-6);
-    if(durB == 0){
+    if(durB == 0) {
         speedB[durB] = fB/20.8;
         durB++;
         return;
-    }
-    else if(durB == 1){
+    } else if(durB == 1) {
         speedB[durB] = fB/20.8;
         durB++;
         return;
-    }
-    else if(durB == 2){
+    } else if(durB == 2) {
         speedB[durB] = fB/20.8;
         durB = 0;
     }
-    for(int xB=0;xB<3;xB++){
-      sumB+=speedB[xB];
+    for(int xB=0; xB<3; xB++) {
+        sumB+=speedB[xB];
     }
     sumB = sumB/3;
     //Reset count
     nB=0;
-    }
+}
 
-void oneRPS(){
+void oneRPS()
+{
     float deltaA = 1.0f-sumA;         //Error
     float deltaB = 1.0f-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
+    dutyA = dutyA + deltaA*0.0001f;    //Increase duty in proportion to the error
+    dutyB = dutyB + deltaB*0.0001f;    //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;
@@ -170,28 +161,42 @@
     //Update duty cycle to correct in the first direction
     PWMA.write(dutyA);
     PWMB.write(dutyB);
-    rpsA = dutyA;
-    rpsB = dutyB;
 }
-void cornerRPS(){
-    float deltaA = 1.0f-sumA;         //Error
-    float deltaB = 1.0f-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
+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
     //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+0.2);
-    PWMB.write(dutyB-0.35);
+    PWMA.write(dutyA);
+    PWMB.write(dutyB);
+}
+void reset()
+{
+    travA = 0;
+    travB = 0;
+    pulseA = 0;
+    pulseB = 0;
 }
-void reset(){
-    trav = 0;
-    pulse = 0;
-    dutyA = rpsA;
-    dutyB = rpsB;
+void straight()
+{
+    dutyA = 0.463f;
+    dutyB = 0.457f;
+    PWMA.write(dutyA);          //Set duty cycle (%)
+    PWMB.write(dutyB);
+}
+void turn()
+{
+    dutyA = 0.5556f;
+    dutyB = 0.28f;
+    PWMA.write(dutyA);          //Set duty cycle (%)
+    PWMB.write(dutyB);
 }
 int main()
 {
@@ -216,95 +221,67 @@
 
     while (SW1 == RELEASED);
     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
-    PWMA.write(0.2f);          //Set duty cycle (%)
-    PWMB.write(0.2f);
-    while(1)
-    {
+    dutyA = 0.463f;
+    dutyB = 0.457f;
+    PWMA.write(dutyA);          //Set duty cycle (%)
+    PWMB.write(dutyB);
+    while(1) {
         timeA();
         timeB();
-        oneRPS();
-        terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
-        while(trav <= 1400)  
-            {  
-                timeA();
-                timeB();
-                oneRPS();  
-                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);      
-            } 
+        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();
+            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);
+        }
         reset();
-        while(trav <= 640)  
-            {  
-                timeA();
-                timeB();
-                cornerRPS();
-                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
-            }
+        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();
-        while(trav <= 1550)  
-            {  
-                timeB();
-                timeA();
-                oneRPS();
-                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
-            }
+        straight();
+        while(travA <= 1118) {
+            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();
+        turn();
+        while(travA <= 453.7) {
+            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();
-        while(trav <= 455)  
-            {  
-                timeA();
-                timeB();
-                cornerRPS();
-                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
-            }
-        reset();     
-        while(trav <= 800)  
-            {  
-                timeB();
-                timeA();
-                oneRPS();
-                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
-            }
+        straight();
+        while(travA <= 500) {
+            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();
-        while(trav <= 330)  
-            {  
-                timeA();
-                timeB();
-                cornerRPS();
-                terminal.printf("wheelA: %6.3f \t wheelB: %6.3f \t distance: %6.2f \n\r", sumA, sumB, trav);
-            }  
+        turn();
+        while(travA <= 350.29) {
+            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);
+        }
         break;
     }
-    PWMA.write(0.0f);    
+    PWMA.write(0.0f);
     PWMB.write(0.0f);
-        
-}
+
+}
\ No newline at end of file