SOFT564Z Group 3
with the tof code
Dependencies: mbed Servo ros_lib_kinetic
Diff: Motors/Motor.cpp
- Revision:
- 8:262c6c40bff9
- Parent:
- 7:8248af58df5a
- Child:
- 9:326b8f261ef0
--- a/Motors/Motor.cpp Tue Nov 19 12:55:44 2019 +0000
+++ b/Motors/Motor.cpp Tue Dec 10 11:52:53 2019 +0000
@@ -5,6 +5,8 @@
#include "Buzzer.h"
#include "Motor.h"
#include "LED.h"
+#include <ros.h>
+#include <std_msgs/UInt8.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
@@ -14,155 +16,195 @@
cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
-/*--------------------------------------------------------------------------------
-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)
+void keySub(const std_msgs::UInt8 &keyPress)
{
- cRGB_LED.record_power(vBatt);
+ printf("%d", keyPress.data);
+
+ if (keyPress.data == 8)
+ {
+ MotorR.Forwards(1.0f);
+ MotorL.Forwards(1.0f);
+ wait(0.5);
+ }
+
+ else if (keyPress.data == 4)
+ {
+ MotorR.Forwards(1.0f);
+ MotorL.Backwards(1.0f);
+ wait(0.5);
+ }
+
+ else if (keyPress.data == 6)
+ {
+ MotorR.Backwards(1.0f);
+ MotorL.Forwards(1.0f);
+ wait(0.5);
+ }
+
+ else if (keyPress.data == 2)
+ {
+ MotorR.Backwards(1.0f);
+ MotorL.Backwards(1.0f);
+ wait(0.5);
+ }
+
+ else if (keyPress.data == 5)
+ {
+ MotorR.Stop();
+ MotorL.Stop();
+ wait(0.5);
+ }
+
+ else
+ {
+ MotorR.Stop();
+ MotorL.Stop();
+ }
}
-/*--------------------------------------------------------------------------------
-Function name: Check_for_obstacles
-Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
-Output Parameters: N/A
-Description: Simple obstacle avoidance functionality
-----------------------------------------------------------------------------------*/
-void Check_for_obstacles(uint8_t TOFRange[4])
-{
- //If top right sensor(6) detects something
- if (TOFRange[2]<200) {
+ /*--------------------------------------------------------------------------------
+ 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
+ Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
+ Output Parameters: N/A
+ Description: Simple obstacle avoidance functionality
+ ----------------------------------------------------------------------------------*/
+ void Check_for_obstacles(uint8_t TOFRange[4]) {
+ //If top right sensor(6) detects something
+ if (TOFRange[2]<200) {
- if(TOFRange[2]>150) { //Provided its 15cm away...
+ 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.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);
+ if(TOFRange[1]<200) {
+ MotorR.Forwards(1.0f);
+ MotorL.Forwards(1.0f);
+ } else {
- } else {
- MotorR.Forwards(0.8f);
- MotorL.Forwards(0.2f);
+ MotorR.Forwards(0.9f);
+ MotorL.Forwards(0.1f);
}
- } else {
-
- if(TOFRange[3]<200) {
- MotorR.Backwards(0.1f);
- MotorL.Backwards(0.9f);
+ } else if(TOFRange[1]<200) {
+ cBuzzer.Beep();
+ cRGB_LED.blue_led();
+ wait(0.02);
+ cRGB_LED.display_power();
+ MotorR.Forwards(0.1f);
+ MotorL.Forwards(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[0]<200) {
+ cBuzzer.Beep();
+ cRGB_LED.blue_led();
+ wait(0.02);
+ cRGB_LED.display_power();
+ MotorR.Forwards(1.0f);
+ MotorL.Forwards(1.0f);
}
- } else if(TOFRange[3]<200) {
- cBuzzer.Beep();
- cRGB_LED.blue_led();
- wait(0.02);
- cRGB_LED.display_power();
-
- if(TOFRange[1]<200) {
+ else {
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.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.blue_led();
- wait(0.02);
- cRGB_LED.display_power();
- MotorR.Forwards(1.0f);
- MotorL.Forwards(1.0f);
}
- else {
- MotorR.Forwards(1.0f);
- MotorL.Forwards(1.0f);
- }
-}
+ /*--------------------------------------------------------------------------------
+ Function name: cMotor
+ Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
+ Output Parameters: N/A
+ Description: Class constructor (Initialisation upon creating class)
+ ----------------------------------------------------------------------------------*/
+ cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev) {
-/*--------------------------------------------------------------------------------
-Function name: cMotor
-Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
-Output Parameters: N/A
-Description: Class constructor (Initialisation upon creating class)
-----------------------------------------------------------------------------------*/
-cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev)
-{
+ // Set initial condition of PWM
+ _pwm.period(0.001); //1KHz
+ _pwm = 0;
- // Set initial condition of PWM
- _pwm.period(0.001); //1KHz
- _pwm = 0;
-
- // Initial condition of output enables
- _fwd = 0;
- _rev = 0;
-}
+ // Initial condition of output enables
+ _fwd = 0;
+ _rev = 0;
+ }
-/*--------------------------------------------------------------------------------
-Function name: Forwards
-Input Parameters: float speed - PWM duty between 0-1
-Output Parameters: N/A
-Description: Drives the motor forwards at a designated speed
-----------------------------------------------------------------------------------*/
-void cMotor::Forwards(float speed)
-{
- _fwd = 1;
- _rev = 0;
- _pwm = speed;
-}
+ /*--------------------------------------------------------------------------------
+ Function name: Forwards
+ Input Parameters: float speed - PWM duty between 0-1
+ Output Parameters: N/A
+ Description: Drives the motor forwards at a designated speed
+ ----------------------------------------------------------------------------------*/
+ void cMotor::Forwards(float speed) {
+ _fwd = 1;
+ _rev = 0;
+ _pwm = speed;
+ }
-/*--------------------------------------------------------------------------------
-Function name: Backwards
-Input Parameters: float speed - PWM duty between 0-1
-Output Parameters: N/A
-Description: Drives the motor backwards at a designated speed
-----------------------------------------------------------------------------------*/
-void cMotor::Backwards(float speed)
-{
- _fwd = 0;
- _rev = 1;
- _pwm = speed;
-}
+ /*--------------------------------------------------------------------------------
+ Function name: Backwards
+ Input Parameters: float speed - PWM duty between 0-1
+ Output Parameters: N/A
+ Description: Drives the motor backwards at a designated speed
+ ----------------------------------------------------------------------------------*/
+ void cMotor::Backwards(float speed) {
+ _fwd = 0;
+ _rev = 1;
+ _pwm = speed;
+ }
-/*--------------------------------------------------------------------------------
-Function name: Stop
-Input Parameters: N/A
-Output Parameters: N/A
-Description: Drives the motor backwards at a designated speed
-----------------------------------------------------------------------------------*/
-void cMotor::Stop()
-{
- _fwd = 0;
- _rev = 0;
- _pwm = 0;
-}
\ No newline at end of file
+ /*--------------------------------------------------------------------------------
+ Function name: Stop
+ Input Parameters: N/A
+ Output Parameters: N/A
+ Description: Drives the motor backwards at a designated speed
+ ----------------------------------------------------------------------------------*/
+ void cMotor::Stop() {
+ _fwd = 0;
+ _rev = 0;
+ _pwm = 0;
+ }
\ No newline at end of file