Diff: main.cpp

Revision:

0:81f6a19a5a44

Child:

1:2a6b6d2112ba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Nov 05 12:25:37 2019 +0000
@@ -0,0 +1,161 @@
+/*  
+ Version 2 – Mike's edit 
+*/  
+
+#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;  
+//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 = 0.99f; //100%  
+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;  
+    //Set initial motor speed to stop    {  
+    for(float ramp = 0.0f; ramp <= 1.0f ; ramp += 0.1)  
+        {  
+            PWMA.write(ramp);          //Set duty cycle y  
+            PWMB.write(ramp-0.011);          //Set duty cycle y  
+            wait(1);  
+        }  
+    PWMA.write(dutyA);          //Set duty cycle hyp  
+    PWMB.write(dutyB);          //Set duty cycle hyp  
+    wait(0.6);
+    PWMB.write(0.2f);          //turn 31deg 
+    wait(1.6);  
+    PWMA.write(dutyA);          //Set duty cycle hyp  
+    PWMB.write(dutyB);          //Set duty cycle hyp  
+    wait(4.4);  
+    PWMB.write(0.2f);          //turn 59deg 
+    wait(1.1);  
+    PWMA.write(dutyA);          //Set duty cycle x  
+    PWMB.write(dutyB);          //Set duty cycle x  
+    wait(2.3);  
+    PWMB.write(0.2f);          //turn 45deg 
+    wait(0.7);  
+    //Array of sensor data  
+    int tA1[2];  
+    int tA2[2];  
+    PWMA.write(0.0f);    
+    PWMB.write(0.0f);   
+    //Instructions to user  
+    terminal.puts("Press USER button repeatedly to adapt duty (to convernge on 1 rotation/s)");  
+    //Main polling loop  
+    while(1) {  
+    //Reset timer and Start  
+    timer.reset();  
+    timer.start();    
+
+        //*********************************************************************  
+
+        //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);  
+        //*********************** 
+        //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();  
+        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  
+        float TA1 = 2.0f * (tA1[1]-tA1[0]);  
+        float TA2 = 2.0f * (tA2[1]-tA2[0]);  
+        float TA = (TA1 + TA2) * 0.5f;  
+        float fA = 1.0f/ (TA *(float)3.0E-6);  
+        terminal.printf("Average A2 Shaft: %6.2fHz \t Wheel: %6.2f\n", fA, fA/20.2f);  
+        //Reset timers  
+        timer.stop();  
+        //Wait for button press  
+        while (SW1 == 1);  
+        wait(0.2);  
+        while (SW1 == 0);  
+        wait(0.1);  
+        //******************************************  
+
+        //Adapt duty to meet 1 revolution per second  
+
+        //******************************************  
+        float wA = fA/20.2f;            //Wheel speed  
+        float deltaA = 1.0f-wA;         //Error  
+        dutyA = dutyA + deltaA*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;  
+        //Update duty cycle to correct in the first direction  
+        PWMA.write(dutyA);                    
+        //Echo to the terminal  
+     terminal.printf("Adapting duty cycle to %6.2f\n", dutyA);  
+    }  
+}  
+
+  
+
+   
+
+  
+
+  
+
+ 
+
+ 
\ No newline at end of file