File content as of revision 6:dfc31e7e2c23:

/*
Simple Routine for Nucleo Board for ROCO104 Buggy Motor Control and Microswitches
Heavy edit from previous ROCO103PP code
Motor Class can now be instansiated with all four pins needed to control the H Bridge
with a member functions as follows

Motor::Speed(float A, Float B) range -0.1 to +1.0 to give full reverse to full forward for A/B motors
Motor::Stop()       STOP
Motor::Fwd(float)   Forward but floating point number (range 0.0 to 1.0)
Motor::Rev(float)   Reverse but floating point number (range 0.0 to 1.0)

Plymouth University
M.Simpson 31st October 2016
Edited 03/02/2017
Edited 06/12/2018
*/
#include "mbed.h"
#include "motor.h"
#include "UltraSonic.h"
#include "spc.h"

#define TIME_PERIOD 2             //Constant compiler Values here 2 equates to 2ms or 500Hz base Frequency
#define DUTY 0.9                  //DUTY of 1.0=100%, 0.4=40% etc.,

Motor Wheel(D13,D11,D9,D10);      //Instance of the Motor Class called 'Wheel' see motor.h and motor.cpp

DigitalIn myButton(USER_BUTTON);  //USER_BUTTON is the Blue Button on the NUCLEO Board

DigitalOut led(LED3);             //LED1 is the Green LED on the NUCLEO board
                                  //N.B. The RED LED is the POWER Indicator
                                  //and the Multicolored LED indicates status of the ST-LINK Programming cycle

Serial pc(USBTX,USBRX);           //Instance of the Serial class to enable much faster BAUD rates then standard 9600 i.e. 115200
                                  //This is Pseudo RS232 over USB the NUCLEO will appear as a COMx Port see device Manager on PC used
                                  //Use PuTTY to monitor check COMx and BAUD rate (115200)
                                  
DigitalOut red(D6);               //Three colored LEDs for the single Pixel Camera
DigitalOut green(D12);
DigitalOut blue(D8);

AnalogIn ldr(A1);                 //Instance of the AnalogIn class called 'ldr'

DigitalOut Trigger(D7);           //Instance of the DigitalInOut class called 'TriggerEcho' WAS D6
DigitalIn  Echo(D2);              //Instance of the DigitalInOut class called 'TriggerEcho'
Timer pulse;                      //Instance of the Timer class called 'pulse' so we can measure timed events

void SPCoff(void){  //Function to turn off SPC
    red=0;
    green=0;
    blue=0;
    }

void SPCtest(void){
    float rlight,glight,blight;
        
    red=1;                   //ILLUMINATE WITH RED ONLY!
    green=0;
    blue=0;
    wait_ms(50);            //Wait for LDR to respond leave this
    rlight=ldr/RCOR;        //Read the Analogue input MBED Normalised 0.0 to 1.0f Multiply by 3.3f to Calibrate to a Voltage read
    printf("RED %4.2fV ",rlight);  //Send LDR response in Volts to PC via PuTTY
    wait(0.1f);
    
    red=0;
    green=1;                //ILLUMINATE WITH GREEN ONLY!
    blue=0;
    wait_ms(50);            //Wait for LDR to respond leave this
    glight=ldr/GCOR;        //Read the Analogue input MBED Normalised 0.0 to 1.0f Multiply by 3.3f to Calibrate to a Voltage read
    printf("GRN %4.2fV ",glight);  //Send LDR response in Volts to PC via PuTTY
    wait(0.1f);
    
    red=0;
    green=0;
    blue=1;                 //ILLUMINATE WITH BLUE ONLY!
    wait_ms(50);            //Wait for LDR to respond leave this
    blight=ldr/BCOR;        //Read the Analogue input MBED Normalised 0.0 to 1.0f Multiply by 3.3f to Calibrate to a Voltage read
    printf("BLU %4.2fV\n\r",blight);  //Send LDR response in Volts to PC via PuTTY
    wait(0.1f);
    
    red=0;
    green=0;
    blue=0;
    if(rlight<glight && rlight<blight){printf("\n\rRED!\n\n\r");  SPCflash(RED,5);}
    if(glight<rlight && glight<blight){printf("\n\rGREEN!\n\n\r");SPCflash(GRN,5);}
    if(blight<glight && blight<rlight){printf("\n\rBLUE!\n\n\r"); SPCflash(BLU,5);}
    wait(1.0f);
}

void SPCflash(int color,int numberOfFlashes){ //Flash color in case of finding each respective one
    switch (color){
        case RED:for(int i=0;i<numberOfFlashes*2;i++){
            red=!red;
            wait(0.15f);
            }break;
        case GRN:for(int i=0;i<numberOfFlashes*2;i++){
            green=!green;
            wait(0.15f);
            }break;
        case BLU:for(int i=0;i<numberOfFlashes*2;i++){
            blue=!blue;
            wait(0.15f);
            }break;
        default:break;
    }
}
  
// Function GetDistance() will return a float value of the Distance from the Ultrasonic Sensor in millimeters typically use as: ‘(float)myDistance=GetDistance();’
float GetDistance(){                                    //Function Name to be called
    int EchoPulseWidth=0,EchoStart=0,EchoEnd=0;         //Assign and set to zero the local variables for this function
    Trigger = 1;                                        //Signal goes High i.e. 3V3
    wait_us(100);                                       //Wait 100us to give a pulse width Triggering the Ultrasonic Module
    Trigger = 0;                                        //Signal goes Low i.e. 0V
    pulse.start();                        //Start the instance of the class timer
    pulse.reset();                                      //Reset the instance of the Timer Class
    while(Echo == 0 && EchoStart < 25000){              //wait for Echo to go high
        EchoStart=pulse.read_us();                      //Conditional 'AND' with timeout to prevent blocking!      
    }
    while(Echo == 1 && ((EchoEnd - EchoStart) < 25000)){//wait for echo to return low
        EchoEnd=pulse.read_us();                        //Conditional 'OR' with timeout to prevent blocking!   
    }
    EchoPulseWidth = EchoEnd - EchoStart;               //time period in us
    pulse.stop();                                       //Stop the instance of the class timer
    return (float)EchoPulseWidth/5.8f;                  //calculate distance in mm and return the value as a float
}

// Function ultra_sonic_distance() will load the global variable distance with Ultra Sonic Sensor value in mm
// and then send the value to the stdio ouput i.e serial over USB
void ultra_sonic_distance(void){
   printf("%dmm \n\r",(int)GetDistance());   
}

//Variable 'duty' for programmer to use to vary speed as required set here to #define compiler constant see above
float duty=DUTY;

int main(){
   pc.baud(115200);
   
   while(myButton==0){
        led=0;
        wait(0.1);
        led=1; 
        wait(0.1);
        }
    printf("\n\rCode Start\n\r");
    SPCoff();
    Wheel.Period_in_ms(2);//Set frequency of the PWMs 500Hz
    
    //CODE START --------------------------------------------
double Distance = GetDistance();
wait(2.0);
while(1){
   do{
        Wheel.Speed(0.7,0.7);//Turn left 70%
        wait(0.3);
        Wheel.Stop();
        wait(0.1);
        ultra_sonic_distance();
        Distance = GetDistance();
        wait(0.1);
        }while(Distance > 800);
        
    Wheel.Stop();
    wait(0.1);
    ultra_sonic_distance();
    Distance = GetDistance();
    wait(0.1);
    
    while(Distance > 90){
        Wheel.Speed(0.8,-0.8);//Forward 80%
        ultra_sonic_distance();
        Distance = GetDistance();
        if(Distance > 950){
            while(Distance > 950){
                Wheel.Speed(-0.7,-0.7);//Turn left 70%
                wait(0.2);
                Wheel.Stop();
                wait(0.1);
                ultra_sonic_distance();
                Distance = GetDistance();
                wait(0.1);
                Wheel.Speed(0.7,0.7);//Turn left 70%
                wait(0.4);
                Wheel.Stop();
                wait(0.1);
                ultra_sonic_distance();
                Distance = GetDistance();
                wait(0.1);
                }
            }
        wait(0.2);
        }
    
    Wheel.Stop();
    wait(0.5);
    SPCtest();
    wait(0.5);
    ultra_sonic_distance();
    Distance = GetDistance();
    wait(0.5);
    
    Wheel.Speed(-0.8,0.8);
    wait(2.5);
    Wheel.Speed(0.7,0.7);
    wait(2.0);
    Wheel.Stop();
    ultra_sonic_distance();
    Distance = GetDistance();
    wait(0.2);
    }
}