main.cpp

00001 /*
00002 Simple Routine for Nucleo Board for ROCO104 Buggy Motor Control and Microswitches
00003 Heavy edit from previous ROCO103PP code
00004 Motor Class can now be instansiated with all four pins needed to control the H Bridge
00005 with a member functions as follows
00006 
00007 Motor::Speed(float A, Float B) range -0.1 to +1.0 to give full reverse to full forward for A/B motors
00008 Motor::Stop()       STOP
00009 Motor::Fwd(float)   Forward but floating point number (range 0.0 to 1.0)
00010 Motor::Rev(float)   Reverse but floating point number (range 0.0 to 1.0)
00011 
00012 Plymouth University
00013 M.Simpson 31st October 2016
00014 Edited 03/02/2017
00015 Edited 06/12/2018
00016 */
00017 #include "mbed.h"
00018 #include "motor.h"
00019 #include "UltraSonic.h"
00020 #include "spc.h"
00021 
00022 #define TIME_PERIOD 2             //Constant compiler Values here 2 equates to 2ms or 500Hz base Frequency
00023 #define DUTY 0.9                  //DUTY of 1.0=100%, 0.4=40% etc.,
00024 
00025 Motor Wheel(D13,D11,D9,D10);      //Instance of the Motor Class called 'Wheel' see motor.h and motor.cpp
00026 
00027 DigitalIn myButton(USER_BUTTON);  //USER_BUTTON is the Blue Button on the NUCLEO Board
00028 
00029 DigitalOut led(LED3);             //LED1 is the Green LED on the NUCLEO board
00030                                   //N.B. The RED LED is the POWER Indicator
00031                                   //and the Multicolored LED indicates status of the ST-LINK Programming cycle
00032 
00033 Serial pc(USBTX,USBRX);           //Instance of the Serial class to enable much faster BAUD rates then standard 9600 i.e. 115200
00034                                   //This is Pseudo RS232 over USB the NUCLEO will appear as a COMx Port see device Manager on PC used
00035                                   //Use PuTTY to monitor check COMx and BAUD rate (115200)
00036                                   
00037 DigitalOut red(D6);               //Three colored LEDs for the single Pixel Camera
00038 DigitalOut green(D12);
00039 DigitalOut blue(D8);
00040 
00041 AnalogIn ldr(A1);                 //Instance of the AnalogIn class called 'ldr'
00042 
00043 DigitalOut Trigger(D7);           //Instance of the DigitalInOut class called 'TriggerEcho' WAS D6
00044 DigitalIn  Echo(D2);              //Instance of the DigitalInOut class called 'TriggerEcho'
00045 Timer pulse;                      //Instance of the Timer class called 'pulse' so we can measure timed events
00046 
00047 void SPCoff(void){  //Function to turn off SPC
00048     red=0;
00049     green=0;
00050     blue=0;
00051     }
00052 
00053 void SPCtest(void){
00054     float rlight,glight,blight;
00055         
00056     red=1;                   //ILLUMINATE WITH RED ONLY!
00057     green=0;
00058     blue=0;
00059     wait_ms(50);            //Wait for LDR to respond leave this
00060     rlight=ldr/RCOR;        //Read the Analogue input MBED Normalised 0.0 to 1.0f Multiply by 3.3f to Calibrate to a Voltage read
00061     printf("RED %4.2fV ",rlight);  //Send LDR response in Volts to PC via PuTTY
00062     wait(0.1f);
00063     
00064     red=0;
00065     green=1;                //ILLUMINATE WITH GREEN ONLY!
00066     blue=0;
00067     wait_ms(50);            //Wait for LDR to respond leave this
00068     glight=ldr/GCOR;        //Read the Analogue input MBED Normalised 0.0 to 1.0f Multiply by 3.3f to Calibrate to a Voltage read
00069     printf("GRN %4.2fV ",glight);  //Send LDR response in Volts to PC via PuTTY
00070     wait(0.1f);
00071     
00072     red=0;
00073     green=0;
00074     blue=1;                 //ILLUMINATE WITH BLUE ONLY!
00075     wait_ms(50);            //Wait for LDR to respond leave this
00076     blight=ldr/BCOR;        //Read the Analogue input MBED Normalised 0.0 to 1.0f Multiply by 3.3f to Calibrate to a Voltage read
00077     printf("BLU %4.2fV\n\r",blight);  //Send LDR response in Volts to PC via PuTTY
00078     wait(0.1f);
00079     
00080     red=0;
00081     green=0;
00082     blue=0;
00083     if(rlight<glight && rlight<blight){printf("\n\rRED!\n\n\r");  SPCflash(RED,5);}
00084     if(glight<rlight && glight<blight){printf("\n\rGREEN!\n\n\r");SPCflash(GRN,5);}
00085     if(blight<glight && blight<rlight){printf("\n\rBLUE!\n\n\r"); SPCflash(BLU,5);}
00086     wait(1.0f);
00087 }
00088 
00089 void SPCflash(int color,int numberOfFlashes){ //Flash color in case of finding each respective one
00090     switch (color){
00091         case RED:for(int i=0;i<numberOfFlashes*2;i++){
00092             red=!red;
00093             wait(0.15f);
00094             }break;
00095         case GRN:for(int i=0;i<numberOfFlashes*2;i++){
00096             green=!green;
00097             wait(0.15f);
00098             }break;
00099         case BLU:for(int i=0;i<numberOfFlashes*2;i++){
00100             blue=!blue;
00101             wait(0.15f);
00102             }break;
00103         default:break;
00104     }
00105 }
00106   
00107 // Function GetDistance() will return a float value of the Distance from the Ultrasonic Sensor in millimeters typically use as: ‘(float)myDistance=GetDistance();’
00108 float GetDistance(){                                    //Function Name to be called
00109     int EchoPulseWidth=0,EchoStart=0,EchoEnd=0;         //Assign and set to zero the local variables for this function
00110     Trigger = 1;                                        //Signal goes High i.e. 3V3
00111     wait_us(100);                                       //Wait 100us to give a pulse width Triggering the Ultrasonic Module
00112     Trigger = 0;                                        //Signal goes Low i.e. 0V
00113     pulse.start();                        //Start the instance of the class timer
00114     pulse.reset();                                      //Reset the instance of the Timer Class
00115     while(Echo == 0 && EchoStart < 25000){              //wait for Echo to go high
00116         EchoStart=pulse.read_us();                      //Conditional 'AND' with timeout to prevent blocking!      
00117     }
00118     while(Echo == 1 && ((EchoEnd - EchoStart) < 25000)){//wait for echo to return low
00119         EchoEnd=pulse.read_us();                        //Conditional 'OR' with timeout to prevent blocking!   
00120     }
00121     EchoPulseWidth = EchoEnd - EchoStart;               //time period in us
00122     pulse.stop();                                       //Stop the instance of the class timer
00123     return (float)EchoPulseWidth/5.8f;                  //calculate distance in mm and return the value as a float
00124 }
00125 
00126 // Function ultra_sonic_distance() will load the global variable distance with Ultra Sonic Sensor value in mm
00127 // and then send the value to the stdio ouput i.e serial over USB
00128 void ultra_sonic_distance(void){
00129    printf("%dmm \n\r",(int)GetDistance());   
00130 }
00131 
00132 //Variable 'duty' for programmer to use to vary speed as required set here to #define compiler constant see above
00133 float duty=DUTY;
00134 
00135 int main(){
00136    pc.baud(115200);
00137    
00138    while(myButton==0){
00139         led=0;
00140         wait(0.1);
00141         led=1; 
00142         wait(0.1);
00143         }
00144     printf("\n\rCode Start\n\r");
00145     SPCoff();
00146     Wheel.Period_in_ms(2);//Set frequency of the PWMs 500Hz
00147     
00148     //CODE START --------------------------------------------
00149 double Distance = GetDistance();
00150 wait(2.0);
00151 while(1){
00152    do{
00153         Wheel.Speed(0.7,0.7);//Turn left 70%
00154         wait(0.3);
00155         Wheel.Stop();
00156         wait(0.1);
00157         ultra_sonic_distance();
00158         Distance = GetDistance();
00159         wait(0.1);
00160         }while(Distance > 800);
00161         
00162     Wheel.Stop();
00163     wait(0.1);
00164     ultra_sonic_distance();
00165     Distance = GetDistance();
00166     wait(0.1);
00167     
00168     while(Distance > 90){
00169         Wheel.Speed(0.8,-0.8);//Forward 80%
00170         ultra_sonic_distance();
00171         Distance = GetDistance();
00172         if(Distance > 950){
00173             while(Distance > 950){
00174                 Wheel.Speed(-0.7,-0.7);//Turn left 70%
00175                 wait(0.2);
00176                 Wheel.Stop();
00177                 wait(0.1);
00178                 ultra_sonic_distance();
00179                 Distance = GetDistance();
00180                 wait(0.1);
00181                 Wheel.Speed(0.7,0.7);//Turn left 70%
00182                 wait(0.4);
00183                 Wheel.Stop();
00184                 wait(0.1);
00185                 ultra_sonic_distance();
00186                 Distance = GetDistance();
00187                 wait(0.1);
00188                 }
00189             }
00190         wait(0.2);
00191         }
00192     
00193     Wheel.Stop();
00194     wait(0.5);
00195     SPCtest();
00196     wait(0.5);
00197     ultra_sonic_distance();
00198     Distance = GetDistance();
00199     wait(0.5);
00200     
00201     Wheel.Speed(-0.8,0.8);
00202     wait(2.5);
00203     Wheel.Speed(0.7,0.7);
00204     wait(2.0);
00205     Wheel.Stop();
00206     ultra_sonic_distance();
00207     Distance = GetDistance();
00208     wait(0.2);
00209     }
00210 }