SOFT564Z Group 3
with the tof code
Dependencies: mbed Servo ros_lib_kinetic
Diff: Motors/Motor.cpp
- Revision:
- 9:326b8f261ef0
- Parent:
- 8:262c6c40bff9
--- a/Motors/Motor.cpp Tue Dec 10 11:52:53 2019 +0000
+++ b/Motors/Motor.cpp Sat Jan 04 21:35:25 2020 +0000
@@ -2,209 +2,350 @@
Filename: main.cpp
Description: Main program loop
--------------------------------------------------------------------------------*/
+#include "main.h"
#include "Buzzer.h"
#include "Motor.h"
#include "LED.h"
#include <ros.h>
+#include <Servo.h>
#include <std_msgs/UInt8.h>
+//#include <std_msgs/UInt16.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
+//Servo servo1(srvoTilt);
+//Servo servo2(srvoPan);
+
//Class definitions
cBuzzer cBuzzer(Buzz);
cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
+int Servo1Pos;
+int Servo2Pos;
-void keySub(const std_msgs::UInt8 &keyPress)
+/*--------------------------------------------------------------------------------
+Function name: KeySub
+Input Parameters: std_msgs::UInt8 &KeyPress
+Output Parameters:
+Description:
+----------------------------------------------------------------------------------*/
+void MotorKeySub(const std_msgs::UInt8 &keyPress)
{
printf("%d", keyPress.data);
-
- if (keyPress.data == 8)
- {
+
+ // Lowercase chars //
+ if (keyPress.data == 119) { // w
+ if (keyPress.data == 88 || 120) { // X
+ MotorR.Stop();
+ MotorL.Stop();
+ }
MotorR.Forwards(1.0f);
MotorL.Forwards(1.0f);
- wait(0.5);
+ wait(0.05);
}
-
- else if (keyPress.data == 4)
- {
+
+ else if (keyPress.data == 97) { // a
+ if (keyPress.data == 88 || 120) { // X
+ MotorR.Stop();
+ MotorL.Stop();
+ }
MotorR.Forwards(1.0f);
MotorL.Backwards(1.0f);
- wait(0.5);
- }
-
- else if (keyPress.data == 6)
- {
+ wait(0.05);
+ }
+
+ else if (keyPress.data == 100) { // d
+ if (keyPress.data == 88 || 120) { // X
+ MotorR.Stop();
+ MotorL.Stop();
+ }
MotorR.Backwards(1.0f);
MotorL.Forwards(1.0f);
- wait(0.5);
- }
-
- else if (keyPress.data == 2)
- {
+ wait(0.05);
+ }
+
+ else if (keyPress.data == 115) { // s
+ if (keyPress.data == 88 || 120) { // X
+ MotorR.Stop();
+ MotorL.Stop();
+ }
MotorR.Backwards(1.0f);
MotorL.Backwards(1.0f);
- wait(0.5);
- }
-
- else if (keyPress.data == 5)
- {
+ wait(0.05);
+ }
+
+
+ // Upper Case Chars
+ else if (keyPress.data == 87) { // W
+ if (keyPress.data == 88 || 120) { // X
+ MotorR.Stop();
+ MotorL.Stop();
+ }
+ MotorR.Forwards(1.0f);
+ MotorL.Forwards(1.0f);
+ wait(0.387);
+ }
+
+ else if (keyPress.data == 65) { // A
+ if (keyPress.data == 88 || 120) { // X
+ MotorR.Stop();
+ MotorL.Stop();
+ }
+ MotorR.Forwards(1.0f);
+ MotorL.Backwards(1.0f);
+ wait(0.387);
+ }
+
+ else if (keyPress.data == 68) { // D
+ if (keyPress.data == 88 || 120) { // X
+ MotorR.Stop();
+ MotorL.Stop();
+ }
+ MotorR.Backwards(1.0f);
+ MotorL.Forwards(1.0f);
+ wait(0.387);
+ }
+
+ else if (keyPress.data == 83) { // S
+ if (keyPress.data == 88 || 120) { // X
+ MotorR.Stop();
+ MotorL.Stop();
+ }
+ MotorR.Backwards(1.0f);
+ MotorL.Backwards(1.0f);
+ wait(0.387);
+ }
+
+
+ else {
MotorR.Stop();
MotorL.Stop();
- wait(0.5);
- }
-
- else
- {
- MotorR.Stop();
- MotorL.Stop();
}
}
- /*--------------------------------------------------------------------------------
- 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) {
+/*--------------------------------------------------------------------------------
+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);
+}
- 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);
+/*--------------------------------------------------------------------------------
+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) {
- } 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) {
+ 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);
- if(TOFRange[1]<200) {
- MotorR.Forwards(1.0f);
- MotorL.Forwards(1.0f);
+ } 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.9f);
- MotorL.Forwards(0.1f);
+ MotorR.Forwards(0.8f);
+ MotorL.Forwards(0.2f);
}
- } 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[3]<200) {
+ MotorR.Backwards(0.1f);
+ MotorL.Backwards(0.9f);
+
+ } else if(TOFRange[1]<200) {
+ MotorR.Backwards(0.9f);
+ MotorL.Backwards(0.1f);
- } else if(TOFRange[0]<200) {
- cBuzzer.Beep();
- cRGB_LED.blue_led();
- wait(0.02);
- cRGB_LED.display_power();
+ } 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[1]<200) {
MotorR.Forwards(1.0f);
MotorL.Forwards(1.0f);
+ } else {
+
+ MotorR.Forwards(0.9f);
+ MotorL.Forwards(0.1f);
}
- else {
- MotorR.Forwards(1.0f);
- MotorL.Forwards(1.0f);
+ } 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)
+{
+
+ // Set initial condition of PWM
+ _pwm.period(0.001); //1KHz
+ _pwm = 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: 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;
+}
+
+
+
+/*--------------------------------------------------------------------------------
+Function name: ServoKeySub
+Input Parameters:
+Output Parameters:
+Description:
+----------------------------------------------------------------------------------*/
+/* Ascii values for arrow Keys
+37(left arrow)
+38(up arrow)
+39(right arrow)
+40(down arrow)
+*/
+
+void servoKeySub(const std_msgs::UInt8 &ServoKeyPress)
+{
+ printf("%d", ServoKeyPress.data);
+
+ if (ServoKeyPress.data == 106) { // j for Pan Left
+ if(Servo1Pos>-45) {
+ Servo1Pos = Servo1Pos-IncSize;
+ servo1.position(Servo1Pos);
+ wait(0.01);
}
}
- /*--------------------------------------------------------------------------------
- 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) {
+ else if (ServoKeyPress.data == 108) { // l for Pan Right
+ if(Servo1Pos<45) {
+ Servo1Pos = Servo1Pos+IncSize;
+ servo1.position(Servo1Pos);
+ wait(0.01);
+ }
+ }
- // Set initial condition of PWM
- _pwm.period(0.001); //1KHz
- _pwm = 0;
- // Initial condition of output enables
- _fwd = 0;
- _rev = 0;
+ else if (ServoKeyPress.data == 105) { // i for Tilt Up
+ if(Servo2Pos>-45){
+ Servo2Pos = Servo2Pos-IncSize;
+ servo2.position(Servo2Pos);
+ wait(0.01);
+ }
}
- /*--------------------------------------------------------------------------------
- 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;
+ else if (ServoKeyPress.data == 107) { // K for Tilt Down
+ if(Servo2Pos<45){
+ Servo2Pos = Servo2Pos+IncSize;
+ servo2.position(Servo2Pos);
+ wait(0.01);
+ }
}
- /*--------------------------------------------------------------------------------
- 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;
+ else if (ServoKeyPress.data == 44) { // for Up
+ if(IncSize<20) {
+ IncSize=(IncSize+1);
+ }
+ } else if (ServoKeyPress.data == 46) { // for Up
+ if(IncSize>1) {
+ IncSize=(IncSize-1);
+ }
}
- /*--------------------------------------------------------------------------------
- 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
+
+
+ pc.printf("Servo 1 = %d Servo 2 = %d \n\r",Servo1Pos,Servo2Pos);
+ servo1.position(Servo1Pos);
+ servo2.position(Servo2Pos);
+
+}
+
+