SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v46666

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Servo ros_lib_kinetic

Motors/Motor.cpp@10:276cc357015c, 2020-01-05 (annotated)

Committer:
hongyunAHN
Date:
Sun Jan 05 21:47:14 2020 +0000
Revision:
10:276cc357015c
Parent:
9:326b8f261ef0 
a

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 --------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 5 #include "main.h"
Stumi 6:2cc2aac35868 6 #include "Buzzer.h"
Stumi 7:8248af58df5a 7 #include "Motor.h"
Stumi 6:2cc2aac35868 8 #include "LED.h"
hongyunAHN 10:276cc357015c 9 #include "TOF.h"
Stumi 8:262c6c40bff9 10 #include <ros.h>
Alexshawcroft 9:326b8f261ef0 11 #include <Servo.h>
Stumi 8:262c6c40bff9 12 #include <std_msgs/UInt8.h>
hongyunAHN 10:276cc357015c 13 #include "mbed.h"
Alexshawcroft 9:326b8f261ef0 14 //#include <std_msgs/UInt16.h>
Alexshawcroft 9:326b8f261ef0 15
hongyunAHN 10:276cc357015c 16 DigitalOut TOF1(PC_8);
hongyunAHN 10:276cc357015c 17 DigitalOut TOF3(PC_11);
hongyunAHN 10:276cc357015c 18 DigitalOut TOF5(PC_12);
hongyunAHN 10:276cc357015c 19 DigitalOut TOF7(PD_2);
Stumi 2:6197e1cf2cd1 20
hongyunAHN 10:276cc357015c 21 cAdafruit_VL6180X VL6180X(TOF1,TOF3,TOF5,TOF7);
Stumi 4:36a04230554d 22 cMotor MotorL(M1_PWM, M1_IN1, M1_IN2); //Left motor class and pin initialisation
Stumi 4:36a04230554d 23 cMotor MotorR(M2_PWM, M2_IN1, M2_IN2); //Right motor class and pin initialisation
Stumi 7:8248af58df5a 24
Alexshawcroft 9:326b8f261ef0 25 //Servo servo1(srvoTilt);
Alexshawcroft 9:326b8f261ef0 26 //Servo servo2(srvoPan);
Alexshawcroft 9:326b8f261ef0 27
Stumi 7:8248af58df5a 28 //Class definitions
Stumi 7:8248af58df5a 29 cBuzzer cBuzzer(Buzz);
Stumi 6:2cc2aac35868 30 cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
Stumi 7:8248af58df5a 31
hongyunAHN 10:276cc357015c 32 uint8_t serviceTOF(uint8_t address){
hongyunAHN 10:276cc357015c 33
hongyunAHN 10:276cc357015c 34 uint8_t range = 0;
hongyunAHN 10:276cc357015c 35
hongyunAHN 10:276cc357015c 36 // poll the VL6180X till new sample ready
hongyunAHN 10:276cc357015c 37 VL6180X.Poll_Range(address);
hongyunAHN 10:276cc357015c 38
hongyunAHN 10:276cc357015c 39 // read range result
hongyunAHN 10:276cc357015c 40 range = VL6180X.Read_Range(address);
hongyunAHN 10:276cc357015c 41
hongyunAHN 10:276cc357015c 42 // clear the interrupt on VL6180X
hongyunAHN 10:276cc357015c 43 VL6180X.Clear_Interrupts(address);
hongyunAHN 10:276cc357015c 44
hongyunAHN 10:276cc357015c 45 return range;
hongyunAHN 10:276cc357015c 46 }
hongyunAHN 10:276cc357015c 47
Alexshawcroft 9:326b8f261ef0 48 int Servo1Pos;
Alexshawcroft 9:326b8f261ef0 49 int Servo2Pos;
Stumi 7:8248af58df5a 50
hongyunAHN 10:276cc357015c 51
hongyunAHN 10:276cc357015c 52
Alexshawcroft 9:326b8f261ef0 53 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 54 Function name: KeySub
Alexshawcroft 9:326b8f261ef0 55 Input Parameters: std_msgs::UInt8 &KeyPress
Alexshawcroft 9:326b8f261ef0 56 Output Parameters:
Alexshawcroft 9:326b8f261ef0 57 Description:
Alexshawcroft 9:326b8f261ef0 58 ----------------------------------------------------------------------------------*/
hongyunAHN 10:276cc357015c 59 int main(){
hongyunAHN 10:276cc357015c 60 uint8_t TOFRange[4];
hongyunAHN 10:276cc357015c 61
hongyunAHN 10:276cc357015c 62 while(1){
hongyunAHN 10:276cc357015c 63 TOFRange[0] = serviceTOF(ADDR1);
hongyunAHN 10:276cc357015c 64 TOFRange[1] = serviceTOF(ADDR2);
hongyunAHN 10:276cc357015c 65 TOFRange[2] = serviceTOF(ADDR3);
hongyunAHN 10:276cc357015c 66 TOFRange[3] = serviceTOF(ADDR4);
hongyunAHN 10:276cc357015c 67
hongyunAHN 10:276cc357015c 68 //Send range to pc by serial
hongyunAHN 10:276cc357015c 69 pc.printf("TOF1:%d TOF3: %d TOF5: %d TOF7: %d\r\n", TOFRange[0], TOFRange[1], TOFRange[2],TOFRange[3]);
hongyunAHN 10:276cc357015c 70
hongyunAHN 10:276cc357015c 71 //Short delay
hongyunAHN 10:276cc357015c 72 wait(0.1);
Alexshawcroft 9:326b8f261ef0 73 void MotorKeySub(const std_msgs::UInt8 &keyPress)
hongyunAHN 10:276cc357015c 74 {
hongyunAHN 10:276cc357015c 75
hongyunAHN 10:276cc357015c 76
hongyunAHN 10:276cc357015c 77
Stumi 8:262c6c40bff9 78 printf("%d", keyPress.data);
hongyunAHN 10:276cc357015c 79
Alexshawcroft 9:326b8f261ef0 80 // Lowercase chars //
Alexshawcroft 9:326b8f261ef0 81 if (keyPress.data == 119) { // w
Alexshawcroft 9:326b8f261ef0 82 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 83 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 84 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 85 }
hongyunAHN 10:276cc357015c 86 else if(TOFRange[2]<150){
hongyunAHN 10:276cc357015c 87 MotorR.Stop();
hongyunAHN 10:276cc357015c 88 MotorL.Stop();
hongyunAHN 10:276cc357015c 89 }
hongyunAHN 10:276cc357015c 90 else {
Stumi 8:262c6c40bff9 91 MotorR.Forwards(1.0f);
Stumi 8:262c6c40bff9 92 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 93 wait(0.05);
hongyunAHN 10:276cc357015c 94 }
Stumi 8:262c6c40bff9 95 }
Alexshawcroft 9:326b8f261ef0 96
Alexshawcroft 9:326b8f261ef0 97 else if (keyPress.data == 97) { // a
Alexshawcroft 9:326b8f261ef0 98 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 99 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 100 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 101 }
hongyunAHN 10:276cc357015c 102 else if (TOFRange[3]<150){
hongyunAHN 10:276cc357015c 103 MotorR.Stop();
hongyunAHN 10:276cc357015c 104 MotorL.Stop();
hongyunAHN 10:276cc357015c 105 }
hongyunAHN 10:276cc357015c 106 else{
Stumi 8:262c6c40bff9 107 MotorR.Forwards(1.0f);
Stumi 8:262c6c40bff9 108 MotorL.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 109 wait(0.05);
hongyunAHN 10:276cc357015c 110 }
Alexshawcroft 9:326b8f261ef0 111 }
Alexshawcroft 9:326b8f261ef0 112
Alexshawcroft 9:326b8f261ef0 113 else if (keyPress.data == 100) { // d
Alexshawcroft 9:326b8f261ef0 114 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 115 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 116 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 117 }
hongyunAHN 10:276cc357015c 118 else if(TOFRange[1]<150){
hongyunAHN 10:276cc357015c 119 MotorR.Stop();
hongyunAHN 10:276cc357015c 120 MotorL.Stop();
hongyunAHN 10:276cc357015c 121 }
hongyunAHN 10:276cc357015c 122 else{
hongyunAHN 10:276cc357015c 123 MotorR.Backwards(1.0f);
Stumi 8:262c6c40bff9 124 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 125 wait(0.05);
hongyunAHN 10:276cc357015c 126 }
Alexshawcroft 9:326b8f261ef0 127 }
Alexshawcroft 9:326b8f261ef0 128
Alexshawcroft 9:326b8f261ef0 129 else if (keyPress.data == 115) { // s
Alexshawcroft 9:326b8f261ef0 130 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 131 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 132 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 133 }
hongyunAHN 10:276cc357015c 134 else if(TOFRange[0]<150){
hongyunAHN 10:276cc357015c 135 MotorR.Stop();
hongyunAHN 10:276cc357015c 136 MotorL.Stop();
hongyunAHN 10:276cc357015c 137 }
hongyunAHN 10:276cc357015c 138 else{
Stumi 8:262c6c40bff9 139 MotorR.Backwards(1.0f);
Stumi 8:262c6c40bff9 140 MotorL.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 141 wait(0.05);
hongyunAHN 10:276cc357015c 142 }
Alexshawcroft 9:326b8f261ef0 143 }
Alexshawcroft 9:326b8f261ef0 144
Alexshawcroft 9:326b8f261ef0 145
Alexshawcroft 9:326b8f261ef0 146 // Upper Case Chars
Alexshawcroft 9:326b8f261ef0 147 else if (keyPress.data == 87) { // W
Alexshawcroft 9:326b8f261ef0 148 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 149 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 150 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 151 }
hongyunAHN 10:276cc357015c 152 else if(TOFRange[2]<150){
hongyunAHN 10:276cc357015c 153 MotorR.Stop();
hongyunAHN 10:276cc357015c 154 MotorL.Stop();
hongyunAHN 10:276cc357015c 155 }
hongyunAHN 10:276cc357015c 156 else{
hongyunAHN 10:276cc357015c 157 MotorR.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 158 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 159 wait(0.387);
hongyunAHN 10:276cc357015c 160 }
Alexshawcroft 9:326b8f261ef0 161 }
Alexshawcroft 9:326b8f261ef0 162
Alexshawcroft 9:326b8f261ef0 163 else if (keyPress.data == 65) { // A
Alexshawcroft 9:326b8f261ef0 164 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 165 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 166 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 167 }
hongyunAHN 10:276cc357015c 168 else if (TOFRange[3]<150){
hongyunAHN 10:276cc357015c 169 MotorR.Stop();
hongyunAHN 10:276cc357015c 170 MotorL.Stop();
hongyunAHN 10:276cc357015c 171 }
hongyunAHN 10:276cc357015c 172 else{
Alexshawcroft 9:326b8f261ef0 173 MotorR.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 174 MotorL.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 175 wait(0.387);
hongyunAHN 10:276cc357015c 176 }
Alexshawcroft 9:326b8f261ef0 177 }
Alexshawcroft 9:326b8f261ef0 178
Alexshawcroft 9:326b8f261ef0 179 else if (keyPress.data == 68) { // D
Alexshawcroft 9:326b8f261ef0 180 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 181 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 182 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 183 }
hongyunAHN 10:276cc357015c 184 else if(TOFRange[1]<150){
hongyunAHN 10:276cc357015c 185 MotorR.Stop();
hongyunAHN 10:276cc357015c 186 MotorL.Stop();
hongyunAHN 10:276cc357015c 187 }
hongyunAHN 10:276cc357015c 188 else{
Alexshawcroft 9:326b8f261ef0 189 MotorR.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 190 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 191 wait(0.387);
hongyunAHN 10:276cc357015c 192 }
Alexshawcroft 9:326b8f261ef0 193 }
Alexshawcroft 9:326b8f261ef0 194
Alexshawcroft 9:326b8f261ef0 195 else if (keyPress.data == 83) { // S
Alexshawcroft 9:326b8f261ef0 196 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 197 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 198 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 199 }
hongyunAHN 10:276cc357015c 200 else if(TOFRange[0]<150){
hongyunAHN 10:276cc357015c 201 MotorR.Stop();
hongyunAHN 10:276cc357015c 202 MotorL.Stop();
hongyunAHN 10:276cc357015c 203 }
hongyunAHN 10:276cc357015c 204 else{
Alexshawcroft 9:326b8f261ef0 205 MotorR.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 206 MotorL.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 207 wait(0.387);
hongyunAHN 10:276cc357015c 208 }
Alexshawcroft 9:326b8f261ef0 209 }
Alexshawcroft 9:326b8f261ef0 210
Alexshawcroft 9:326b8f261ef0 211
Alexshawcroft 9:326b8f261ef0 212 else {
Stumi 8:262c6c40bff9 213 MotorR.Stop();
Stumi 8:262c6c40bff9 214 MotorL.Stop();
Stumi 8:262c6c40bff9 215 }
hongyunAHN 10:276cc357015c 216 } } }
Stumi 4:36a04230554d 217
Alexshawcroft 9:326b8f261ef0 218 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 219 Function name: update_power_levels
Alexshawcroft 9:326b8f261ef0 220 Input Parameters: vBatt
Alexshawcroft 9:326b8f261ef0 221 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 222 Description: Function used to send the battery level over to the LED class locally from global source files
Alexshawcroft 9:326b8f261ef0 223 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 224 void update_power_levels(float vBatt)
Alexshawcroft 9:326b8f261ef0 225 {
Alexshawcroft 9:326b8f261ef0 226 cRGB_LED.record_power(vBatt);
Alexshawcroft 9:326b8f261ef0 227 }
Stumi 7:8248af58df5a 228
Alexshawcroft 9:326b8f261ef0 229 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 230 Function name: Check_for_obstacles
Alexshawcroft 9:326b8f261ef0 231 Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
Alexshawcroft 9:326b8f261ef0 232 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 233 Description: Simple obstacle avoidance functionality
Alexshawcroft 9:326b8f261ef0 234 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 235 void Check_for_obstacles(uint8_t TOFRange[4])
Alexshawcroft 9:326b8f261ef0 236 {
Alexshawcroft 9:326b8f261ef0 237 //If top right sensor(6) detects something
Alexshawcroft 9:326b8f261ef0 238 if (TOFRange[2]<200) {
Stumi 8:262c6c40bff9 239
Alexshawcroft 9:326b8f261ef0 240 if(TOFRange[2]>150) { //Provided its 15cm away...
Stumi 7:8248af58df5a 241 cBuzzer.Beep();
Stumi 7:8248af58df5a 242 cRGB_LED.blue_led();
Stumi 7:8248af58df5a 243 wait(0.02);
Stumi 7:8248af58df5a 244 cRGB_LED.display_power();
Alexshawcroft 9:326b8f261ef0 245 if(TOFRange[3]<200) { //...and the back sensor detects something
Alexshawcroft 9:326b8f261ef0 246 //Smooth turn right
Alexshawcroft 9:326b8f261ef0 247 MotorR.Forwards(0.8f);
Alexshawcroft 9:326b8f261ef0 248 MotorL.Forwards(0.2f);
Stumi 7:8248af58df5a 249
Alexshawcroft 9:326b8f261ef0 250 } else if(TOFRange[1]<200) { //...and the top left sensor detects something
Alexshawcroft 9:326b8f261ef0 251 //Smooth turn left
Alexshawcroft 9:326b8f261ef0 252 MotorR.Forwards(0.2f);
Alexshawcroft 9:326b8f261ef0 253 MotorL.Forwards(0.8f);
Alexshawcroft 9:326b8f261ef0 254
Stumi 8:262c6c40bff9 255 } else {
Alexshawcroft 9:326b8f261ef0 256 MotorR.Forwards(0.8f);
Alexshawcroft 9:326b8f261ef0 257 MotorL.Forwards(0.2f);
Stumi 4:36a04230554d 258 }
Stumi 7:8248af58df5a 259
Alexshawcroft 9:326b8f261ef0 260 } else {
Alexshawcroft 9:326b8f261ef0 261
Alexshawcroft 9:326b8f261ef0 262 if(TOFRange[3]<200) {
Alexshawcroft 9:326b8f261ef0 263 MotorR.Backwards(0.1f);
Alexshawcroft 9:326b8f261ef0 264 MotorL.Backwards(0.9f);
Alexshawcroft 9:326b8f261ef0 265
Alexshawcroft 9:326b8f261ef0 266 } else if(TOFRange[1]<200) {
Alexshawcroft 9:326b8f261ef0 267 MotorR.Backwards(0.9f);
Alexshawcroft 9:326b8f261ef0 268 MotorL.Backwards(0.1f);
Stumi 7:8248af58df5a 269
Alexshawcroft 9:326b8f261ef0 270 } else {
Alexshawcroft 9:326b8f261ef0 271 MotorR.Backwards(0.1f);
Alexshawcroft 9:326b8f261ef0 272 MotorL.Backwards(0.9f);
Alexshawcroft 9:326b8f261ef0 273 }
Alexshawcroft 9:326b8f261ef0 274 }
Alexshawcroft 9:326b8f261ef0 275
Alexshawcroft 9:326b8f261ef0 276 } else if(TOFRange[3]<200) {
Alexshawcroft 9:326b8f261ef0 277 cBuzzer.Beep();
Alexshawcroft 9:326b8f261ef0 278 cRGB_LED.blue_led();
Alexshawcroft 9:326b8f261ef0 279 wait(0.02);
Alexshawcroft 9:326b8f261ef0 280 cRGB_LED.display_power();
Alexshawcroft 9:326b8f261ef0 281
Alexshawcroft 9:326b8f261ef0 282 if(TOFRange[1]<200) {
Stumi 8:262c6c40bff9 283 MotorR.Forwards(1.0f);
Stumi 8:262c6c40bff9 284 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 285 } else {
Alexshawcroft 9:326b8f261ef0 286
Alexshawcroft 9:326b8f261ef0 287 MotorR.Forwards(0.9f);
Alexshawcroft 9:326b8f261ef0 288 MotorL.Forwards(0.1f);
Stumi 4:36a04230554d 289 }
Stumi 7:8248af58df5a 290
Alexshawcroft 9:326b8f261ef0 291 } else if(TOFRange[1]<200) {
Alexshawcroft 9:326b8f261ef0 292 cBuzzer.Beep();
Alexshawcroft 9:326b8f261ef0 293 cRGB_LED.blue_led();
Alexshawcroft 9:326b8f261ef0 294 wait(0.02);
Alexshawcroft 9:326b8f261ef0 295 cRGB_LED.display_power();
Alexshawcroft 9:326b8f261ef0 296 MotorR.Forwards(0.1f);
Alexshawcroft 9:326b8f261ef0 297 MotorL.Forwards(0.9f);
Alexshawcroft 9:326b8f261ef0 298
Alexshawcroft 9:326b8f261ef0 299 } else if(TOFRange[0]<200) {
Alexshawcroft 9:326b8f261ef0 300 cBuzzer.Beep();
Alexshawcroft 9:326b8f261ef0 301 cRGB_LED.blue_led();
Alexshawcroft 9:326b8f261ef0 302 wait(0.02);
Alexshawcroft 9:326b8f261ef0 303 cRGB_LED.display_power();
Alexshawcroft 9:326b8f261ef0 304 MotorR.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 305 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 306 }
Alexshawcroft 9:326b8f261ef0 307
Alexshawcroft 9:326b8f261ef0 308 else {
Alexshawcroft 9:326b8f261ef0 309 MotorR.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 310 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 311 }
Alexshawcroft 9:326b8f261ef0 312 }
Alexshawcroft 9:326b8f261ef0 313
Alexshawcroft 9:326b8f261ef0 314 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 315 Function name: cMotor
Alexshawcroft 9:326b8f261ef0 316 Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
Alexshawcroft 9:326b8f261ef0 317 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 318 Description: Class constructor (Initialisation upon creating class)
Alexshawcroft 9:326b8f261ef0 319 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 320 cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev)
Alexshawcroft 9:326b8f261ef0 321 {
Alexshawcroft 9:326b8f261ef0 322
Alexshawcroft 9:326b8f261ef0 323 // Set initial condition of PWM
Alexshawcroft 9:326b8f261ef0 324 _pwm.period(0.001); //1KHz
Alexshawcroft 9:326b8f261ef0 325 _pwm = 0;
Alexshawcroft 9:326b8f261ef0 326
Alexshawcroft 9:326b8f261ef0 327 // Initial condition of output enables
Alexshawcroft 9:326b8f261ef0 328 _fwd = 0;
Alexshawcroft 9:326b8f261ef0 329 _rev = 0;
Alexshawcroft 9:326b8f261ef0 330 }
Alexshawcroft 9:326b8f261ef0 331
Alexshawcroft 9:326b8f261ef0 332 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 333 Function name: Forwards
Alexshawcroft 9:326b8f261ef0 334 Input Parameters: float speed - PWM duty between 0-1
Alexshawcroft 9:326b8f261ef0 335 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 336 Description: Drives the motor forwards at a designated speed
Alexshawcroft 9:326b8f261ef0 337 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 338 void cMotor::Forwards(float speed)
Alexshawcroft 9:326b8f261ef0 339 {
Alexshawcroft 9:326b8f261ef0 340 _fwd = 1;
Alexshawcroft 9:326b8f261ef0 341 _rev = 0;
Alexshawcroft 9:326b8f261ef0 342 _pwm = speed;
Alexshawcroft 9:326b8f261ef0 343 }
Alexshawcroft 9:326b8f261ef0 344
Alexshawcroft 9:326b8f261ef0 345 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 346 Function name: Backwards
Alexshawcroft 9:326b8f261ef0 347 Input Parameters: float speed - PWM duty between 0-1
Alexshawcroft 9:326b8f261ef0 348 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 349 Description: Drives the motor backwards at a designated speed
Alexshawcroft 9:326b8f261ef0 350 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 351 void cMotor::Backwards(float speed)
Alexshawcroft 9:326b8f261ef0 352 {
Alexshawcroft 9:326b8f261ef0 353 _fwd = 0;
Alexshawcroft 9:326b8f261ef0 354 _rev = 1;
Alexshawcroft 9:326b8f261ef0 355 _pwm = speed;
Alexshawcroft 9:326b8f261ef0 356 }
Alexshawcroft 9:326b8f261ef0 357
Alexshawcroft 9:326b8f261ef0 358 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 359 Function name: Stop
Alexshawcroft 9:326b8f261ef0 360 Input Parameters: N/A
Alexshawcroft 9:326b8f261ef0 361 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 362 Description: Drives the motor backwards at a designated speed
Alexshawcroft 9:326b8f261ef0 363 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 364 void cMotor::Stop()
Alexshawcroft 9:326b8f261ef0 365 {
Alexshawcroft 9:326b8f261ef0 366 _fwd = 0;
Alexshawcroft 9:326b8f261ef0 367 _rev = 0;
Alexshawcroft 9:326b8f261ef0 368 _pwm = 0;
Alexshawcroft 9:326b8f261ef0 369 }
Alexshawcroft 9:326b8f261ef0 370
Alexshawcroft 9:326b8f261ef0 371
Alexshawcroft 9:326b8f261ef0 372
Alexshawcroft 9:326b8f261ef0 373 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 374 Function name: ServoKeySub
Alexshawcroft 9:326b8f261ef0 375 Input Parameters:
Alexshawcroft 9:326b8f261ef0 376 Output Parameters:
Alexshawcroft 9:326b8f261ef0 377 Description:
Alexshawcroft 9:326b8f261ef0 378 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 379 /* Ascii values for arrow Keys
Alexshawcroft 9:326b8f261ef0 380 37(left arrow)
Alexshawcroft 9:326b8f261ef0 381 38(up arrow)
Alexshawcroft 9:326b8f261ef0 382 39(right arrow)
Alexshawcroft 9:326b8f261ef0 383 40(down arrow)
Alexshawcroft 9:326b8f261ef0 384 */
Alexshawcroft 9:326b8f261ef0 385
Alexshawcroft 9:326b8f261ef0 386 void servoKeySub(const std_msgs::UInt8 &ServoKeyPress)
Alexshawcroft 9:326b8f261ef0 387 {
hongyunAHN 10:276cc357015c 388
Alexshawcroft 9:326b8f261ef0 389 printf("%d", ServoKeyPress.data);
Alexshawcroft 9:326b8f261ef0 390
Alexshawcroft 9:326b8f261ef0 391 if (ServoKeyPress.data == 106) { // j for Pan Left
Alexshawcroft 9:326b8f261ef0 392 if(Servo1Pos>-45) {
Alexshawcroft 9:326b8f261ef0 393 Servo1Pos = Servo1Pos-IncSize;
Alexshawcroft 9:326b8f261ef0 394 servo1.position(Servo1Pos);
Alexshawcroft 9:326b8f261ef0 395 wait(0.01);
Stumi 4:36a04230554d 396 }
Stumi 4:36a04230554d 397 }
Stumi 4:36a04230554d 398
Alexshawcroft 9:326b8f261ef0 399 else if (ServoKeyPress.data == 108) { // l for Pan Right
Alexshawcroft 9:326b8f261ef0 400 if(Servo1Pos<45) {
Alexshawcroft 9:326b8f261ef0 401 Servo1Pos = Servo1Pos+IncSize;
Alexshawcroft 9:326b8f261ef0 402 servo1.position(Servo1Pos);
Alexshawcroft 9:326b8f261ef0 403 wait(0.01);
Alexshawcroft 9:326b8f261ef0 404 }
Alexshawcroft 9:326b8f261ef0 405 }
Stumi 4:36a04230554d 406
Stumi 4:36a04230554d 407
Alexshawcroft 9:326b8f261ef0 408 else if (ServoKeyPress.data == 105) { // i for Tilt Up
Alexshawcroft 9:326b8f261ef0 409 if(Servo2Pos>-45){
Alexshawcroft 9:326b8f261ef0 410 Servo2Pos = Servo2Pos-IncSize;
Alexshawcroft 9:326b8f261ef0 411 servo2.position(Servo2Pos);
Alexshawcroft 9:326b8f261ef0 412 wait(0.01);
Alexshawcroft 9:326b8f261ef0 413 }
Stumi 8:262c6c40bff9 414 }
Stumi 2:6197e1cf2cd1 415
Alexshawcroft 9:326b8f261ef0 416 else if (ServoKeyPress.data == 107) { // K for Tilt Down
Alexshawcroft 9:326b8f261ef0 417 if(Servo2Pos<45){
Alexshawcroft 9:326b8f261ef0 418 Servo2Pos = Servo2Pos+IncSize;
Alexshawcroft 9:326b8f261ef0 419 servo2.position(Servo2Pos);
Alexshawcroft 9:326b8f261ef0 420 wait(0.01);
Alexshawcroft 9:326b8f261ef0 421 }
Stumi 8:262c6c40bff9 422 }
Stumi 2:6197e1cf2cd1 423
Alexshawcroft 9:326b8f261ef0 424 else if (ServoKeyPress.data == 44) { // for Up
Alexshawcroft 9:326b8f261ef0 425 if(IncSize<20) {
Alexshawcroft 9:326b8f261ef0 426 IncSize=(IncSize+1);
Alexshawcroft 9:326b8f261ef0 427 }
Alexshawcroft 9:326b8f261ef0 428 } else if (ServoKeyPress.data == 46) { // for Up
Alexshawcroft 9:326b8f261ef0 429 if(IncSize>1) {
Alexshawcroft 9:326b8f261ef0 430 IncSize=(IncSize-1);
Alexshawcroft 9:326b8f261ef0 431 }
Stumi 8:262c6c40bff9 432 }
Stumi 3:df6160e2f6d9 433
Alexshawcroft 9:326b8f261ef0 434
Alexshawcroft 9:326b8f261ef0 435
Alexshawcroft 9:326b8f261ef0 436 pc.printf("Servo 1 = %d Servo 2 = %d \n\r",Servo1Pos,Servo2Pos);
Alexshawcroft 9:326b8f261ef0 437 servo1.position(Servo1Pos);
Alexshawcroft 9:326b8f261ef0 438 servo2.position(Servo2Pos);
Alexshawcroft 9:326b8f261ef0 439
Alexshawcroft 9:326b8f261ef0 440 }
Alexshawcroft 9:326b8f261ef0 441
Alexshawcroft 9:326b8f261ef0 442