Diff: Motors/Motor.cpp

Revision:

6:2cc2aac35868

Parent:

4:36a04230554d

Child:

7:8248af58df5a

--- a/Motors/Motor.cpp	Tue Nov 12 10:36:59 2019 +0000
+++ b/Motors/Motor.cpp	Tue Nov 12 12:56:02 2019 +0000
@@ -2,11 +2,14 @@
 Filename: main.cpp
 Description: Main program loop
 --------------------------------------------------------------------------------*/
-
+#include "Buzzer.h"
+#include "LED.h"
 #include "Motor.h"
 
 cMotor MotorL(M1_PWM, M1_IN1, M1_IN2);  //Left motor class and pin initialisation
 cMotor MotorR(M2_PWM, M2_IN1, M2_IN2);  //Right motor class and pin initialisation
+cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
+cBuzzer cBuzzer(Buzz);
 
 /*--------------------------------------------------------------------------------
 Function name: Check_for_obstacles
@@ -18,7 +21,8 @@
 {
     //If top right sensor(6) detects something
     if (TOFRange[2]<200) {
-        
+       cBuzzer.Beep();
+       cRGB_LED.red_led();
         if(TOFRange[2]>150) {   //Provided its 15cm away...
             
             if(TOFRange[3]<200) {   //...and the back sensor detects something
@@ -53,7 +57,8 @@
         }
         
     } else if(TOFRange[3]<200) {
-        
+       cBuzzer.Beep();
+       cRGB_LED.red_led();
         if(TOFRange[1]<200) {
             MotorR.Forwards(1.0f);
             MotorL.Forwards(1.0f);
@@ -64,15 +69,20 @@
         }
         
     } else if(TOFRange[1]<200) {
+       cBuzzer.Beep();
+       cRGB_LED.red_led();
         MotorR.Forwards(0.1f);
         MotorL.Forwards(0.9f);
         
     } else if(TOFRange[0]<200) {
+       cBuzzer.Beep();
+       cRGB_LED.red_led();
         MotorR.Forwards(1.0f);
         MotorL.Forwards(1.0f);
     }
 
     else {
+       cRGB_LED.green_led();
         MotorR.Forwards(1.0f);
         MotorL.Forwards(1.0f);
     }