Revision 6:dfc31e7e2c23, committed 2019-05-07

Comitter:

Pabs44

Date:

Tue May 07 14:47:34 2019 +0000

Parent:

5:2621fc2ed6da

Commit message:

code

Changed in this revision

diff -r 2621fc2ed6da -r dfc31e7e2c23 main.cpp
--- a/main.cpp	Mon May 06 10:25:13 2019 +0000
+++ b/main.cpp	Tue May 07 14:47:34 2019 +0000
@@ -22,86 +22,90 @@
 #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.,
 
-DigitalIn microswitch1(D4);       //Instance of the DigitalIn class called 'microswitch1'
-DigitalIn microswitch2(D3);       //Instance of the DigitalIn class called 'microswitch2'
-
 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 Multicoloured LED indicates status of the ST-LINK Programming cycle
+                                  //and the Multicolored LED indicates status of the ST-LINK Programming cycle
 
-Serial terminal(USBTX,USBRX);           //Instance of the Serial class to enable much faster BAUD rates then standard 9600 i.e. 115200
+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 coloured LEDs for the single Pixel Camera
+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'
 
-void SPCoff(void){red=0;green=0;blue=0;}
+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 SPCtest(void)
-{
+void SPCoff(void){  //Function to turn off SPC
+    red=0;
+    green=0;
+    blue=0;
+    }
+
+void SPCtest(void){
     float rlight,glight,blight;
         
-    red=1;green=0;blue=0;   //ILLUMINATE WITH RED ONLY!
+    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;blue=0;   //ILLUMINATE WITH GREEN ONLY!
+    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!
+    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;
+    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 colour,int numberOfFlashes)
-{
-    switch (colour)
-    {
-        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;
+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;
     }
 }
-                                  
-
-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
-
-
-// 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());   
-}
   
-// Function GetDistance() will return a float value of the Distance from the Ultrasonic Sensor in millimetres typically use as: ‘(float)myDistance=GetDistance();’
-float GetDistance()
-{                                                       //Function Name to be called
+// 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
@@ -119,26 +123,88 @@
     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(){
-   terminal.baud(115200);
-   while(1){
-       SPCoff();
-       SPCtest();
-       wait(0.5);
+   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);
+    }
 }
-} 
-
-
-
-/*
-//Consider these lines of code to Accelerate the motors
-      for (float i=0.5f; i<=1.0f; i+=0.01f) //Accelerate  from 50% to 100%
-      { 
-        Wheel.Speed(i,i);
-        wait(0.1f);
-      }
-*/