Diff: main.cpp

Revision:

19:d3b82416df50

Parent:

18:11937e78239c

Child:

20:c89685cd0b02

--- a/main.cpp	Tue Dec 03 16:39:25 2019 +0000
+++ b/main.cpp	Mon Dec 09 16:41:10 2019 +0000
@@ -2,44 +2,43 @@
 Our version
 */
 #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 timerA;
 Timer timerB;
-Timer timer1;
- 
+
 //Enumerated types
 enum DIRECTION   {FORWARD=0, REVERSE};
 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%
@@ -50,7 +49,8 @@
 int tB2[2];
 float dis;
 float trav =0;
-
+float speedA, speedB = 0;
+int countA, countB = 0;
 void HallA()
         {
             //Reset timer and Start
@@ -88,23 +88,20 @@
                 }break;
             }
         }
- 
- 
-        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]);
- 
+        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;
-
-        dis = timer1.read_us();
-        float mm = ((TA*3)*20.8)/175.9;
-        trav = dis/mm;
+        trav = (175.9/20.8);
         float fA = 1.0f/ (TA *(float)3.0E-6);
-        terminal.printf("Average A2 Shaft: %6.2fHz \t Wheel: %6.2f \t trav: %6.2f\n", fA, fA/20.8f, trav);
+        speedA = fA/20.8;
+        terminal.printf("WheelA: %6.2f \t WheelB: %6.2f \t trav: %d\n", speedA, speedB, countA);
+        if(countA >=21){
+            PWMA.write(0.0f);
+            PWMB.write(0.0f);
+            wait(10000);
+        }
         }
 
 void HallB()
@@ -140,117 +137,73 @@
                 if(HEB2 == PULSE){
                     HallStateB = 0;
                     allB = false;
+                    countB++;
                     tB2[1] = timerB.read_us();
                 }break;
             }
         }
- 
- 
-        terminal.printf("tB1(0) = %d\n", tB1[0]);
-        terminal.printf("tB1(1) = %d\n", tB1[1]);
-        terminal.printf("tB2(0) = %d\n", tB2[0]);
-        terminal.printf("tB2(1) = %d\n", tB2[1]);
- 
         //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;
         }
-         
-void reset()
-{
-    timer1.reset();
-    HallA();
+
+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(115200);
- 
+    terminal.baud(9600);
+    
+    terminal.printf("Hello\n\r");
+
+    
+
     //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;
- 
-    //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();
+    //Set initial motor speed to stop
+    PWMA.write(0.5f);          //Set duty cycle (%)
+    PWMB.write(0.5f);          //Set duty cycle (%)
+
     //Main polling loop
     while(1)
     {
-        while(trav <= 1250)  
-            {  
-               PWMA.write(dutyA);          //Set duty cycle y  
-               PWMB.write(dutyB);
-               HallA();
-               HallB();
-            } 
-        reset();
-        while(trav <= 330)  
-            {  
-               PWMA.write(dutyA);           
-               PWMB.write(0.0f);
-               HallA();
-               HallB();
-            }
-        reset();
-        while(trav <= 1457)  
-            {  
-               PWMA.write(dutyA);           
-               PWMB.write(dutyB);
-               HallA();
-               HallB();
-            }
-        reset();
-        while(trav <= 268)  
-            {  
-               PWMA.write(dutyA);           
-               PWMB.write(0.0f);
-               HallA();
-               HallB();
-            }
-        reset();
-        while(trav <= 750)  
-            {  
-               PWMA.write(dutyA);           
-               PWMB.write(dutyB);
-               HallA();
-               HallB();
-            }
-        reset();
-        while(trav <= 200)  
-            {  
-               PWMA.write(dutyA);           
-               PWMB.write(0.0f);
-               HallA();
-               HallB();
-            }  
-        timerA.stop();   
-        timerB.stop();
-        break;
+        HallA();
+        HallB();
+        oneRPS();
     }
-    PWMA.write(0.0f);    
+    PWMA.write(0.0f);
     PWMB.write(0.0f);
 }
- 
\ No newline at end of file