with the tof code
Dependencies: mbed Servo ros_lib_kinetic
Diff: Motors/Motor.cpp
- Revision:
- 7:8248af58df5a
- Parent:
- 6:2cc2aac35868
- Child:
- 8:262c6c40bff9
--- a/Motors/Motor.cpp Tue Nov 12 12:56:02 2019 +0000 +++ b/Motors/Motor.cpp Tue Nov 19 12:55:44 2019 +0000 @@ -3,13 +3,27 @@ Description: Main program loop --------------------------------------------------------------------------------*/ #include "Buzzer.h" +#include "Motor.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 + +//Class definitions +cBuzzer cBuzzer(Buzz); cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN); -cBuzzer cBuzzer(Buzz); + + +/*-------------------------------------------------------------------------------- +Function name: update_power_levels +Input Parameters: vBatt +Output Parameters: N/A +Description: Function used to send the battery level over to the LED class locally from global source files +----------------------------------------------------------------------------------*/ +void update_power_levels(float vBatt) +{ + cRGB_LED.record_power(vBatt); +} /*-------------------------------------------------------------------------------- Function name: Check_for_obstacles @@ -21,68 +35,76 @@ { //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... - + cBuzzer.Beep(); + cRGB_LED.blue_led(); + wait(0.02); + cRGB_LED.display_power(); if(TOFRange[3]<200) { //...and the back sensor detects something //Smooth turn right MotorR.Forwards(0.8f); MotorL.Forwards(0.2f); - + } else if(TOFRange[1]<200) { //...and the top left sensor detects something //Smooth turn left MotorR.Forwards(0.2f); MotorL.Forwards(0.8f); - + } else { MotorR.Forwards(0.8f); MotorL.Forwards(0.2f); } - + } else { if(TOFRange[3]<200) { MotorR.Backwards(0.1f); MotorL.Backwards(0.9f); - + } else if(TOFRange[1]<200) { MotorR.Backwards(0.9f); MotorL.Backwards(0.1f); - + } else { MotorR.Backwards(0.1f); MotorL.Backwards(0.9f); } } - + } else if(TOFRange[3]<200) { - cBuzzer.Beep(); - cRGB_LED.red_led(); + cBuzzer.Beep(); + cRGB_LED.blue_led(); + wait(0.02); + cRGB_LED.display_power(); + if(TOFRange[1]<200) { MotorR.Forwards(1.0f); MotorL.Forwards(1.0f); } else { - + MotorR.Forwards(0.9f); MotorL.Forwards(0.1f); } - + } else if(TOFRange[1]<200) { - cBuzzer.Beep(); - cRGB_LED.red_led(); + cBuzzer.Beep(); + cRGB_LED.blue_led(); + wait(0.02); + cRGB_LED.display_power(); MotorR.Forwards(0.1f); MotorL.Forwards(0.9f); - + } else if(TOFRange[0]<200) { - cBuzzer.Beep(); - cRGB_LED.red_led(); + cBuzzer.Beep(); + cRGB_LED.blue_led(); + wait(0.02); + cRGB_LED.display_power(); MotorR.Forwards(1.0f); MotorL.Forwards(1.0f); } else { - cRGB_LED.green_led(); MotorR.Forwards(1.0f); MotorL.Forwards(1.0f); }