SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v3

with the tof code

Dependencies:   mbed Servo ros_lib_kinetic

Motors/Motor.cpp@8:262c6c40bff9, 2019-12-10 (annotated)

Committer:
Stumi
Date:
Tue Dec 10 11:52:53 2019 +0000
Revision:
8:262c6c40bff9
Parent:
7:8248af58df5a
Child:
9:326b8f261ef0
Communicates keyboard inputs to the robot using rosnodes;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stumi 2:6197e1cf2cd1 1 /*--------------------------------------------------------------------------------
Stumi 2:6197e1cf2cd1 2 Filename: main.cpp
Stumi 2:6197e1cf2cd1 3 Description: Main program loop
Stumi 2:6197e1cf2cd1 4 --------------------------------------------------------------------------------*/
Stumi 6:2cc2aac35868 5 #include "Buzzer.h"
Stumi 7:8248af58df5a 6 #include "Motor.h"
Stumi 6:2cc2aac35868 7 #include "LED.h"
Stumi 8:262c6c40bff9 8 #include <ros.h>
Stumi 8:262c6c40bff9 9 #include <std_msgs/UInt8.h>
Stumi 2:6197e1cf2cd1 10
Stumi 4:36a04230554d 11 cMotor MotorL(M1_PWM, M1_IN1, M1_IN2); //Left motor class and pin initialisation
Stumi 4:36a04230554d 12 cMotor MotorR(M2_PWM, M2_IN1, M2_IN2); //Right motor class and pin initialisation
Stumi 7:8248af58df5a 13
Stumi 7:8248af58df5a 14 //Class definitions
Stumi 7:8248af58df5a 15 cBuzzer cBuzzer(Buzz);
Stumi 6:2cc2aac35868 16 cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
Stumi 7:8248af58df5a 17
Stumi 7:8248af58df5a 18
Stumi 8:262c6c40bff9 19 void keySub(const std_msgs::UInt8 &keyPress)
Stumi 7:8248af58df5a 20 {
Stumi 8:262c6c40bff9 21 printf("%d", keyPress.data);
Stumi 8:262c6c40bff9 22
Stumi 8:262c6c40bff9 23 if (keyPress.data == 8)
Stumi 8:262c6c40bff9 24 {
Stumi 8:262c6c40bff9 25 MotorR.Forwards(1.0f);
Stumi 8:262c6c40bff9 26 MotorL.Forwards(1.0f);
Stumi 8:262c6c40bff9 27 wait(0.5);
Stumi 8:262c6c40bff9 28 }
Stumi 8:262c6c40bff9 29
Stumi 8:262c6c40bff9 30 else if (keyPress.data == 4)
Stumi 8:262c6c40bff9 31 {
Stumi 8:262c6c40bff9 32 MotorR.Forwards(1.0f);
Stumi 8:262c6c40bff9 33 MotorL.Backwards(1.0f);
Stumi 8:262c6c40bff9 34 wait(0.5);
Stumi 8:262c6c40bff9 35 }
Stumi 8:262c6c40bff9 36
Stumi 8:262c6c40bff9 37 else if (keyPress.data == 6)
Stumi 8:262c6c40bff9 38 {
Stumi 8:262c6c40bff9 39 MotorR.Backwards(1.0f);
Stumi 8:262c6c40bff9 40 MotorL.Forwards(1.0f);
Stumi 8:262c6c40bff9 41 wait(0.5);
Stumi 8:262c6c40bff9 42 }
Stumi 8:262c6c40bff9 43
Stumi 8:262c6c40bff9 44 else if (keyPress.data == 2)
Stumi 8:262c6c40bff9 45 {
Stumi 8:262c6c40bff9 46 MotorR.Backwards(1.0f);
Stumi 8:262c6c40bff9 47 MotorL.Backwards(1.0f);
Stumi 8:262c6c40bff9 48 wait(0.5);
Stumi 8:262c6c40bff9 49 }
Stumi 8:262c6c40bff9 50
Stumi 8:262c6c40bff9 51 else if (keyPress.data == 5)
Stumi 8:262c6c40bff9 52 {
Stumi 8:262c6c40bff9 53 MotorR.Stop();
Stumi 8:262c6c40bff9 54 MotorL.Stop();
Stumi 8:262c6c40bff9 55 wait(0.5);
Stumi 8:262c6c40bff9 56 }
Stumi 8:262c6c40bff9 57
Stumi 8:262c6c40bff9 58 else
Stumi 8:262c6c40bff9 59 {
Stumi 8:262c6c40bff9 60 MotorR.Stop();
Stumi 8:262c6c40bff9 61 MotorL.Stop();
Stumi 8:262c6c40bff9 62 }
Stumi 7:8248af58df5a 63 }
Stumi 4:36a04230554d 64
Stumi 8:262c6c40bff9 65 /*--------------------------------------------------------------------------------
Stumi 8:262c6c40bff9 66 Function name: update_power_levels
Stumi 8:262c6c40bff9 67 Input Parameters: vBatt
Stumi 8:262c6c40bff9 68 Output Parameters: N/A
Stumi 8:262c6c40bff9 69 Description: Function used to send the battery level over to the LED class locally from global source files
Stumi 8:262c6c40bff9 70 ----------------------------------------------------------------------------------*/
Stumi 8:262c6c40bff9 71 void update_power_levels(float vBatt) {
Stumi 8:262c6c40bff9 72 cRGB_LED.record_power(vBatt);
Stumi 8:262c6c40bff9 73 }
Stumi 8:262c6c40bff9 74
Stumi 8:262c6c40bff9 75 /*--------------------------------------------------------------------------------
Stumi 8:262c6c40bff9 76 Function name: Check_for_obstacles
Stumi 8:262c6c40bff9 77 Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
Stumi 8:262c6c40bff9 78 Output Parameters: N/A
Stumi 8:262c6c40bff9 79 Description: Simple obstacle avoidance functionality
Stumi 8:262c6c40bff9 80 ----------------------------------------------------------------------------------*/
Stumi 8:262c6c40bff9 81 void Check_for_obstacles(uint8_t TOFRange[4]) {
Stumi 8:262c6c40bff9 82 //If top right sensor(6) detects something
Stumi 8:262c6c40bff9 83 if (TOFRange[2]<200) {
Stumi 7:8248af58df5a 84
Stumi 8:262c6c40bff9 85 if(TOFRange[2]>150) { //Provided its 15cm away...
Stumi 8:262c6c40bff9 86 cBuzzer.Beep();
Stumi 8:262c6c40bff9 87 cRGB_LED.blue_led();
Stumi 8:262c6c40bff9 88 wait(0.02);
Stumi 8:262c6c40bff9 89 cRGB_LED.display_power();
Stumi 8:262c6c40bff9 90 if(TOFRange[3]<200) { //...and the back sensor detects something
Stumi 8:262c6c40bff9 91 //Smooth turn right
Stumi 8:262c6c40bff9 92 MotorR.Forwards(0.8f);
Stumi 8:262c6c40bff9 93 MotorL.Forwards(0.2f);
Stumi 8:262c6c40bff9 94
Stumi 8:262c6c40bff9 95 } else if(TOFRange[1]<200) { //...and the top left sensor detects something
Stumi 8:262c6c40bff9 96 //Smooth turn left
Stumi 8:262c6c40bff9 97 MotorR.Forwards(0.2f);
Stumi 8:262c6c40bff9 98 MotorL.Forwards(0.8f);
Stumi 8:262c6c40bff9 99
Stumi 8:262c6c40bff9 100 } else {
Stumi 8:262c6c40bff9 101 MotorR.Forwards(0.8f);
Stumi 8:262c6c40bff9 102 MotorL.Forwards(0.2f);
Stumi 8:262c6c40bff9 103 }
Stumi 8:262c6c40bff9 104
Stumi 8:262c6c40bff9 105 } else {
Stumi 8:262c6c40bff9 106
Stumi 8:262c6c40bff9 107 if(TOFRange[3]<200) {
Stumi 8:262c6c40bff9 108 MotorR.Backwards(0.1f);
Stumi 8:262c6c40bff9 109 MotorL.Backwards(0.9f);
Stumi 8:262c6c40bff9 110
Stumi 8:262c6c40bff9 111 } else if(TOFRange[1]<200) {
Stumi 8:262c6c40bff9 112 MotorR.Backwards(0.9f);
Stumi 8:262c6c40bff9 113 MotorL.Backwards(0.1f);
Stumi 8:262c6c40bff9 114
Stumi 8:262c6c40bff9 115 } else {
Stumi 8:262c6c40bff9 116 MotorR.Backwards(0.1f);
Stumi 8:262c6c40bff9 117 MotorL.Backwards(0.9f);
Stumi 8:262c6c40bff9 118 }
Stumi 8:262c6c40bff9 119 }
Stumi 8:262c6c40bff9 120
Stumi 8:262c6c40bff9 121 } else if(TOFRange[3]<200) {
Stumi 7:8248af58df5a 122 cBuzzer.Beep();
Stumi 7:8248af58df5a 123 cRGB_LED.blue_led();
Stumi 7:8248af58df5a 124 wait(0.02);
Stumi 7:8248af58df5a 125 cRGB_LED.display_power();
Stumi 7:8248af58df5a 126
Stumi 8:262c6c40bff9 127 if(TOFRange[1]<200) {
Stumi 8:262c6c40bff9 128 MotorR.Forwards(1.0f);
Stumi 8:262c6c40bff9 129 MotorL.Forwards(1.0f);
Stumi 8:262c6c40bff9 130 } else {
Stumi 7:8248af58df5a 131
Stumi 8:262c6c40bff9 132 MotorR.Forwards(0.9f);
Stumi 8:262c6c40bff9 133 MotorL.Forwards(0.1f);
Stumi 4:36a04230554d 134 }
Stumi 7:8248af58df5a 135
Stumi 8:262c6c40bff9 136 } else if(TOFRange[1]<200) {
Stumi 8:262c6c40bff9 137 cBuzzer.Beep();
Stumi 8:262c6c40bff9 138 cRGB_LED.blue_led();
Stumi 8:262c6c40bff9 139 wait(0.02);
Stumi 8:262c6c40bff9 140 cRGB_LED.display_power();
Stumi 8:262c6c40bff9 141 MotorR.Forwards(0.1f);
Stumi 8:262c6c40bff9 142 MotorL.Forwards(0.9f);
Stumi 7:8248af58df5a 143
Stumi 8:262c6c40bff9 144 } else if(TOFRange[0]<200) {
Stumi 8:262c6c40bff9 145 cBuzzer.Beep();
Stumi 8:262c6c40bff9 146 cRGB_LED.blue_led();
Stumi 8:262c6c40bff9 147 wait(0.02);
Stumi 8:262c6c40bff9 148 cRGB_LED.display_power();
Stumi 8:262c6c40bff9 149 MotorR.Forwards(1.0f);
Stumi 8:262c6c40bff9 150 MotorL.Forwards(1.0f);
Stumi 4:36a04230554d 151 }
Stumi 7:8248af58df5a 152
Stumi 8:262c6c40bff9 153 else {
Stumi 4:36a04230554d 154 MotorR.Forwards(1.0f);
Stumi 4:36a04230554d 155 MotorL.Forwards(1.0f);
Stumi 4:36a04230554d 156 }
Stumi 4:36a04230554d 157 }
Stumi 4:36a04230554d 158
Stumi 8:262c6c40bff9 159 /*--------------------------------------------------------------------------------
Stumi 8:262c6c40bff9 160 Function name: cMotor
Stumi 8:262c6c40bff9 161 Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
Stumi 8:262c6c40bff9 162 Output Parameters: N/A
Stumi 8:262c6c40bff9 163 Description: Class constructor (Initialisation upon creating class)
Stumi 8:262c6c40bff9 164 ----------------------------------------------------------------------------------*/
Stumi 8:262c6c40bff9 165 cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev) {
Stumi 4:36a04230554d 166
Stumi 8:262c6c40bff9 167 // Set initial condition of PWM
Stumi 8:262c6c40bff9 168 _pwm.period(0.001); //1KHz
Stumi 8:262c6c40bff9 169 _pwm = 0;
Stumi 4:36a04230554d 170
Stumi 8:262c6c40bff9 171 // Initial condition of output enables
Stumi 8:262c6c40bff9 172 _fwd = 0;
Stumi 8:262c6c40bff9 173 _rev = 0;
Stumi 8:262c6c40bff9 174 }
Stumi 2:6197e1cf2cd1 175
Stumi 8:262c6c40bff9 176 /*--------------------------------------------------------------------------------
Stumi 8:262c6c40bff9 177 Function name: Forwards
Stumi 8:262c6c40bff9 178 Input Parameters: float speed - PWM duty between 0-1
Stumi 8:262c6c40bff9 179 Output Parameters: N/A
Stumi 8:262c6c40bff9 180 Description: Drives the motor forwards at a designated speed
Stumi 8:262c6c40bff9 181 ----------------------------------------------------------------------------------*/
Stumi 8:262c6c40bff9 182 void cMotor::Forwards(float speed) {
Stumi 8:262c6c40bff9 183 _fwd = 1;
Stumi 8:262c6c40bff9 184 _rev = 0;
Stumi 8:262c6c40bff9 185 _pwm = speed;
Stumi 8:262c6c40bff9 186 }
Stumi 2:6197e1cf2cd1 187
Stumi 8:262c6c40bff9 188 /*--------------------------------------------------------------------------------
Stumi 8:262c6c40bff9 189 Function name: Backwards
Stumi 8:262c6c40bff9 190 Input Parameters: float speed - PWM duty between 0-1
Stumi 8:262c6c40bff9 191 Output Parameters: N/A
Stumi 8:262c6c40bff9 192 Description: Drives the motor backwards at a designated speed
Stumi 8:262c6c40bff9 193 ----------------------------------------------------------------------------------*/
Stumi 8:262c6c40bff9 194 void cMotor::Backwards(float speed) {
Stumi 8:262c6c40bff9 195 _fwd = 0;
Stumi 8:262c6c40bff9 196 _rev = 1;
Stumi 8:262c6c40bff9 197 _pwm = speed;
Stumi 8:262c6c40bff9 198 }
Stumi 3:df6160e2f6d9 199
Stumi 8:262c6c40bff9 200 /*--------------------------------------------------------------------------------
Stumi 8:262c6c40bff9 201 Function name: Stop
Stumi 8:262c6c40bff9 202 Input Parameters: N/A
Stumi 8:262c6c40bff9 203 Output Parameters: N/A
Stumi 8:262c6c40bff9 204 Description: Drives the motor backwards at a designated speed
Stumi 8:262c6c40bff9 205 ----------------------------------------------------------------------------------*/
Stumi 8:262c6c40bff9 206 void cMotor::Stop() {
Stumi 8:262c6c40bff9 207 _fwd = 0;
Stumi 8:262c6c40bff9 208 _rev = 0;
Stumi 8:262c6c40bff9 209 _pwm = 0;
Stumi 8:262c6c40bff9 210 }