Rachel Ireland-Jones / Mbed OS FinalYear0

main.cpp

Committer:
Mikebob
Date:
2019-11-19
Revision:
15:b7ca53716b05
Parent:
14:e66cf781f5b9
Child:
16:afe936377d66

File content as of revision 15:b7ca53716b05:

//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;
float paceA, paceB = 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();
}

void oneRPS ()
{
    time();
    if (speedA <= 1.0f)
    {
        paceA +=0.01f;
        wait_ms(25);
    }
    else if (speedA >= 1.0f)
    {
        paceA -=0.01f;
        wait_ms(25);
    }
    else if (speedB <= 1.0f)
    {
        paceB +=0.01f;
        wait_ms(25);
    }
    else if (speedB >= 1.0f)
    {
        paceB -=0.01f;
        wait_ms(25);
    }
}
      
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)
    {
        oneRPS();
        while(trav <= 1250)  
            {  
               PWMA.write(paceA);          //Set duty cycle y  
               PWMB.write(paceB);
               time();
               oneRPS();
            } 
        reset();
        while(trav <= 330)  
            {  
               PWMA.write(paceA);           
               PWMB.write(0.0f);
               time();
               oneRPS();
            }
        reset();
        while(trav <= 1457)  
            {  
               PWMA.write(paceA);           
               PWMB.write(paceB);
               time();
               oneRPS();
            }
        reset();
        while(trav <= 268)  
            {  
               PWMA.write(paceA);           
               PWMB.write(0.0f);
               time();
               oneRPS();
            }
        reset();
        while(trav <= 750)  
            {  
               PWMA.write(paceA);           
               PWMB.write(paceB);
               time();
               oneRPS();
            }
        reset();
        while(trav <= 200)  
            {  
               PWMA.write(paceA);           
               PWMB.write(0.0f);
               time();
               oneRPS();
            }  
        timer.stop();   
        break;
    }
    PWMA.write(0.0f);           
    PWMB.write(0.0f);
}