SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3_final

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@11:12e73437dc9f, 2020-01-10 (annotated)

Committer:
hongyunAHN
Date:
Fri Jan 10 15:05:11 2020 +0000
Revision:
11:12e73437dc9f
Parent:
10:51dd168b5a96 
ii

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hongyunAHN 11:12e73437dc9f 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"
Stumi 8:262c6c40bff9 9 #include <ros.h>
Alexshawcroft 9:326b8f261ef0 10 #include <Servo.h>
Stumi 8:262c6c40bff9 11 #include <std_msgs/UInt8.h>
hongyunAHN 11:12e73437dc9f 12
Alexshawcroft 9:326b8f261ef0 13
Stumi 2:6197e1cf2cd1 14
Stumi 4:36a04230554d 15 cMotor MotorL(M1_PWM, M1_IN1, M1_IN2); //Left motor class and pin initialisation
Stumi 4:36a04230554d 16 cMotor MotorR(M2_PWM, M2_IN1, M2_IN2); //Right motor class and pin initialisation
Stumi 7:8248af58df5a 17
Stumi 10:51dd168b5a96 18 float *TOFrangePtr[4];
Stumi 10:51dd168b5a96 19
hongyunAHN 11:12e73437dc9f 20
Alexshawcroft 9:326b8f261ef0 21
Stumi 7:8248af58df5a 22 //Class definitions
Stumi 7:8248af58df5a 23 cBuzzer cBuzzer(Buzz);
Stumi 6:2cc2aac35868 24 cRGB_LED cRGB_LED(DIAG_RED, DIAG_BLU, DIAG_GRN);
Stumi 7:8248af58df5a 25
Alexshawcroft 9:326b8f261ef0 26 int Servo1Pos;
Alexshawcroft 9:326b8f261ef0 27 int Servo2Pos;
Stumi 7:8248af58df5a 28
Alexshawcroft 9:326b8f261ef0 29 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 30 Function name: KeySub
Alexshawcroft 9:326b8f261ef0 31 Input Parameters: std_msgs::UInt8 &KeyPress
Alexshawcroft 9:326b8f261ef0 32 Output Parameters:
Alexshawcroft 9:326b8f261ef0 33 Description:
Alexshawcroft 9:326b8f261ef0 34 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 35 void MotorKeySub(const std_msgs::UInt8 &keyPress)
Stumi 7:8248af58df5a 36 {
Stumi 8:262c6c40bff9 37 printf("%d", keyPress.data);
Alexshawcroft 9:326b8f261ef0 38
Alexshawcroft 9:326b8f261ef0 39 // Lowercase chars //
hongyunAHN 11:12e73437dc9f 40 // get keypress information at rasbarry pi to move the DC motor.
hongyunAHN 11:12e73437dc9f 41 //"w,W" move forward, "a,A" move left side, "d,D" move right side,"s,S" move backward
hongyunAHN 11:12e73437dc9f 42 // "q" stop the motor, "e" check the obstacles automatically
Alexshawcroft 9:326b8f261ef0 43 if (keyPress.data == 119) { // w
Alexshawcroft 9:326b8f261ef0 44 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 45 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 46 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 47 }
Stumi 8:262c6c40bff9 48 MotorR.Forwards(1.0f);
Stumi 8:262c6c40bff9 49 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 50 wait(0.05);
Stumi 8:262c6c40bff9 51 }
Alexshawcroft 9:326b8f261ef0 52
Alexshawcroft 9:326b8f261ef0 53 else if (keyPress.data == 97) { // a
Alexshawcroft 9:326b8f261ef0 54 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 55 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 56 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 57 }
Stumi 8:262c6c40bff9 58 MotorR.Forwards(1.0f);
Stumi 8:262c6c40bff9 59 MotorL.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 60 wait(0.05);
Alexshawcroft 9:326b8f261ef0 61 }
Alexshawcroft 9:326b8f261ef0 62
Alexshawcroft 9:326b8f261ef0 63 else if (keyPress.data == 100) { // d
Alexshawcroft 9:326b8f261ef0 64 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 65 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 66 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 67 }
Stumi 8:262c6c40bff9 68 MotorR.Backwards(1.0f);
Stumi 8:262c6c40bff9 69 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 70 wait(0.05);
Alexshawcroft 9:326b8f261ef0 71 }
Alexshawcroft 9:326b8f261ef0 72
Alexshawcroft 9:326b8f261ef0 73 else if (keyPress.data == 115) { // s
Alexshawcroft 9:326b8f261ef0 74 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 75 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 76 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 77 }
Stumi 8:262c6c40bff9 78 MotorR.Backwards(1.0f);
Stumi 8:262c6c40bff9 79 MotorL.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 80 wait(0.05);
Alexshawcroft 9:326b8f261ef0 81 }
Alexshawcroft 9:326b8f261ef0 82
Alexshawcroft 9:326b8f261ef0 83
Alexshawcroft 9:326b8f261ef0 84 // Upper Case Chars
Alexshawcroft 9:326b8f261ef0 85 else if (keyPress.data == 87) { // W
Alexshawcroft 9:326b8f261ef0 86 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 87 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 88 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 89 }
Alexshawcroft 9:326b8f261ef0 90 MotorR.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 91 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 92 wait(0.387);
Alexshawcroft 9:326b8f261ef0 93 }
Alexshawcroft 9:326b8f261ef0 94
Alexshawcroft 9:326b8f261ef0 95 else if (keyPress.data == 65) { // A
Alexshawcroft 9:326b8f261ef0 96 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 97 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 98 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 99 }
Alexshawcroft 9:326b8f261ef0 100 MotorR.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 101 MotorL.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 102 wait(0.387);
Alexshawcroft 9:326b8f261ef0 103 }
Alexshawcroft 9:326b8f261ef0 104
Alexshawcroft 9:326b8f261ef0 105 else if (keyPress.data == 68) { // D
Alexshawcroft 9:326b8f261ef0 106 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 107 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 108 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 109 }
Alexshawcroft 9:326b8f261ef0 110 MotorR.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 111 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 112 wait(0.387);
Alexshawcroft 9:326b8f261ef0 113 }
Alexshawcroft 9:326b8f261ef0 114
Alexshawcroft 9:326b8f261ef0 115 else if (keyPress.data == 83) { // S
Alexshawcroft 9:326b8f261ef0 116 if (keyPress.data == 88 || 120) { // X
Alexshawcroft 9:326b8f261ef0 117 MotorR.Stop();
Alexshawcroft 9:326b8f261ef0 118 MotorL.Stop();
Alexshawcroft 9:326b8f261ef0 119 }
Alexshawcroft 9:326b8f261ef0 120 MotorR.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 121 MotorL.Backwards(1.0f);
Alexshawcroft 9:326b8f261ef0 122 wait(0.387);
Alexshawcroft 9:326b8f261ef0 123 }
Stumi 10:51dd168b5a96 124
Stumi 10:51dd168b5a96 125 //Stop command
Stumi 10:51dd168b5a96 126 else if (keyPress.data == 'q') {
Stumi 10:51dd168b5a96 127 MotorR.Stop();
Stumi 10:51dd168b5a96 128 MotorL.Stop();
Stumi 10:51dd168b5a96 129 }
Stumi 10:51dd168b5a96 130 else if (keyPress.data == 'e') {
Stumi 10:51dd168b5a96 131 Check_for_obstacles(*TOFrangePtr);
Stumi 10:51dd168b5a96 132 }
Alexshawcroft 9:326b8f261ef0 133
Alexshawcroft 9:326b8f261ef0 134 else {
Stumi 8:262c6c40bff9 135 MotorR.Stop();
Stumi 8:262c6c40bff9 136 MotorL.Stop();
Stumi 8:262c6c40bff9 137 }
Stumi 7:8248af58df5a 138 }
Stumi 4:36a04230554d 139
Alexshawcroft 9:326b8f261ef0 140 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 141 Function name: update_power_levels
Alexshawcroft 9:326b8f261ef0 142 Input Parameters: vBatt
Alexshawcroft 9:326b8f261ef0 143 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 144 Description: Function used to send the battery level over to the LED class locally from global source files
Alexshawcroft 9:326b8f261ef0 145 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 146 void update_power_levels(float vBatt)
Alexshawcroft 9:326b8f261ef0 147 {
Alexshawcroft 9:326b8f261ef0 148 cRGB_LED.record_power(vBatt);
Alexshawcroft 9:326b8f261ef0 149 }
Stumi 7:8248af58df5a 150
Alexshawcroft 9:326b8f261ef0 151 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 152 Function name: Check_for_obstacles
Alexshawcroft 9:326b8f261ef0 153 Input Parameters: cMotor - MotorL (left motor) cMotor MotorR (right motor) TOFrange[4] - sensor measurements
Alexshawcroft 9:326b8f261ef0 154 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 155 Description: Simple obstacle avoidance functionality
Alexshawcroft 9:326b8f261ef0 156 ----------------------------------------------------------------------------------*/
hongyunAHN 11:12e73437dc9f 157 // using the 4 sensor to check the obstacles and escape the obstacles.
hongyunAHN 11:12e73437dc9f 158 // TOFRange[0]is back sensor, TOFRange[1] is leftside sensor, TOFRange[2] is front sensor and TOFRange[4] is rightside sensor
hongyunAHN 11:12e73437dc9f 159 // devide the front sensor range to 200~150 and 150~0.
hongyunAHN 11:12e73437dc9f 160 // when robot detect the obstacles the buzzer and blue led are ON.
Stumi 10:51dd168b5a96 161 void Check_for_obstacles(float TOFRange[4])
Alexshawcroft 9:326b8f261ef0 162 {
Alexshawcroft 9:326b8f261ef0 163 //If top right sensor(6) detects something
Alexshawcroft 9:326b8f261ef0 164 if (TOFRange[2]<200) {
Stumi 8:262c6c40bff9 165
Alexshawcroft 9:326b8f261ef0 166 if(TOFRange[2]>150) { //Provided its 15cm away...
Stumi 7:8248af58df5a 167 cBuzzer.Beep();
Stumi 7:8248af58df5a 168 cRGB_LED.blue_led();
Stumi 7:8248af58df5a 169 wait(0.02);
Stumi 7:8248af58df5a 170 cRGB_LED.display_power();
Alexshawcroft 9:326b8f261ef0 171 if(TOFRange[3]<200) { //...and the back sensor detects something
Alexshawcroft 9:326b8f261ef0 172 //Smooth turn right
Alexshawcroft 9:326b8f261ef0 173 MotorR.Forwards(0.8f);
Alexshawcroft 9:326b8f261ef0 174 MotorL.Forwards(0.2f);
Stumi 7:8248af58df5a 175
Alexshawcroft 9:326b8f261ef0 176 } else if(TOFRange[1]<200) { //...and the top left sensor detects something
Alexshawcroft 9:326b8f261ef0 177 //Smooth turn left
Alexshawcroft 9:326b8f261ef0 178 MotorR.Forwards(0.2f);
Alexshawcroft 9:326b8f261ef0 179 MotorL.Forwards(0.8f);
Alexshawcroft 9:326b8f261ef0 180
Stumi 8:262c6c40bff9 181 } else {
Alexshawcroft 9:326b8f261ef0 182 MotorR.Forwards(0.8f);
Alexshawcroft 9:326b8f261ef0 183 MotorL.Forwards(0.2f);
Stumi 4:36a04230554d 184 }
Stumi 7:8248af58df5a 185
Alexshawcroft 9:326b8f261ef0 186 } else {
Alexshawcroft 9:326b8f261ef0 187
Alexshawcroft 9:326b8f261ef0 188 if(TOFRange[3]<200) {
Alexshawcroft 9:326b8f261ef0 189 MotorR.Backwards(0.1f);
Alexshawcroft 9:326b8f261ef0 190 MotorL.Backwards(0.9f);
Alexshawcroft 9:326b8f261ef0 191
Alexshawcroft 9:326b8f261ef0 192 } else if(TOFRange[1]<200) {
Alexshawcroft 9:326b8f261ef0 193 MotorR.Backwards(0.9f);
Alexshawcroft 9:326b8f261ef0 194 MotorL.Backwards(0.1f);
Stumi 7:8248af58df5a 195
Alexshawcroft 9:326b8f261ef0 196 } else {
Alexshawcroft 9:326b8f261ef0 197 MotorR.Backwards(0.1f);
Alexshawcroft 9:326b8f261ef0 198 MotorL.Backwards(0.9f);
Alexshawcroft 9:326b8f261ef0 199 }
Alexshawcroft 9:326b8f261ef0 200 }
Alexshawcroft 9:326b8f261ef0 201
Alexshawcroft 9:326b8f261ef0 202 } else if(TOFRange[3]<200) {
Alexshawcroft 9:326b8f261ef0 203 cBuzzer.Beep();
Alexshawcroft 9:326b8f261ef0 204 cRGB_LED.blue_led();
Alexshawcroft 9:326b8f261ef0 205 wait(0.02);
Alexshawcroft 9:326b8f261ef0 206 cRGB_LED.display_power();
Alexshawcroft 9:326b8f261ef0 207
Alexshawcroft 9:326b8f261ef0 208 if(TOFRange[1]<200) {
Stumi 8:262c6c40bff9 209 MotorR.Forwards(1.0f);
Stumi 8:262c6c40bff9 210 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 211 } else {
Alexshawcroft 9:326b8f261ef0 212
Alexshawcroft 9:326b8f261ef0 213 MotorR.Forwards(0.9f);
Alexshawcroft 9:326b8f261ef0 214 MotorL.Forwards(0.1f);
Stumi 4:36a04230554d 215 }
Stumi 7:8248af58df5a 216
Alexshawcroft 9:326b8f261ef0 217 } else if(TOFRange[1]<200) {
Alexshawcroft 9:326b8f261ef0 218 cBuzzer.Beep();
Alexshawcroft 9:326b8f261ef0 219 cRGB_LED.blue_led();
Alexshawcroft 9:326b8f261ef0 220 wait(0.02);
Alexshawcroft 9:326b8f261ef0 221 cRGB_LED.display_power();
Alexshawcroft 9:326b8f261ef0 222 MotorR.Forwards(0.1f);
Alexshawcroft 9:326b8f261ef0 223 MotorL.Forwards(0.9f);
Alexshawcroft 9:326b8f261ef0 224
Alexshawcroft 9:326b8f261ef0 225 } else if(TOFRange[0]<200) {
Alexshawcroft 9:326b8f261ef0 226 cBuzzer.Beep();
Alexshawcroft 9:326b8f261ef0 227 cRGB_LED.blue_led();
Alexshawcroft 9:326b8f261ef0 228 wait(0.02);
Alexshawcroft 9:326b8f261ef0 229 cRGB_LED.display_power();
Alexshawcroft 9:326b8f261ef0 230 MotorR.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 231 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 232 }
Alexshawcroft 9:326b8f261ef0 233
Alexshawcroft 9:326b8f261ef0 234 else {
Alexshawcroft 9:326b8f261ef0 235 MotorR.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 236 MotorL.Forwards(1.0f);
Alexshawcroft 9:326b8f261ef0 237 }
Alexshawcroft 9:326b8f261ef0 238 }
Alexshawcroft 9:326b8f261ef0 239
Alexshawcroft 9:326b8f261ef0 240 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 241 Function name: cMotor
Alexshawcroft 9:326b8f261ef0 242 Input Parameters: PwmOut pwm - Motor PWM, DigitalOut fwd - Motor input1, DigitalOut rev - Motor Input2
Alexshawcroft 9:326b8f261ef0 243 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 244 Description: Class constructor (Initialisation upon creating class)
Alexshawcroft 9:326b8f261ef0 245 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 246 cMotor::cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev):_pwm(pwm), _fwd(fwd), _rev(rev)
Alexshawcroft 9:326b8f261ef0 247 {
Alexshawcroft 9:326b8f261ef0 248
Alexshawcroft 9:326b8f261ef0 249 // Set initial condition of PWM
Alexshawcroft 9:326b8f261ef0 250 _pwm.period(0.001); //1KHz
Alexshawcroft 9:326b8f261ef0 251 _pwm = 0;
Alexshawcroft 9:326b8f261ef0 252
Alexshawcroft 9:326b8f261ef0 253 // Initial condition of output enables
Alexshawcroft 9:326b8f261ef0 254 _fwd = 0;
Alexshawcroft 9:326b8f261ef0 255 _rev = 0;
Alexshawcroft 9:326b8f261ef0 256 }
Alexshawcroft 9:326b8f261ef0 257
Alexshawcroft 9:326b8f261ef0 258 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 259 Function name: Forwards
Alexshawcroft 9:326b8f261ef0 260 Input Parameters: float speed - PWM duty between 0-1
Alexshawcroft 9:326b8f261ef0 261 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 262 Description: Drives the motor forwards at a designated speed
Alexshawcroft 9:326b8f261ef0 263 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 264 void cMotor::Forwards(float speed)
Alexshawcroft 9:326b8f261ef0 265 {
Alexshawcroft 9:326b8f261ef0 266 _fwd = 1;
Alexshawcroft 9:326b8f261ef0 267 _rev = 0;
Alexshawcroft 9:326b8f261ef0 268 _pwm = speed;
Alexshawcroft 9:326b8f261ef0 269 }
Alexshawcroft 9:326b8f261ef0 270
Alexshawcroft 9:326b8f261ef0 271 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 272 Function name: Backwards
Alexshawcroft 9:326b8f261ef0 273 Input Parameters: float speed - PWM duty between 0-1
Alexshawcroft 9:326b8f261ef0 274 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 275 Description: Drives the motor backwards at a designated speed
Alexshawcroft 9:326b8f261ef0 276 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 277 void cMotor::Backwards(float speed)
Alexshawcroft 9:326b8f261ef0 278 {
Alexshawcroft 9:326b8f261ef0 279 _fwd = 0;
Alexshawcroft 9:326b8f261ef0 280 _rev = 1;
Alexshawcroft 9:326b8f261ef0 281 _pwm = speed;
Alexshawcroft 9:326b8f261ef0 282 }
Alexshawcroft 9:326b8f261ef0 283
Alexshawcroft 9:326b8f261ef0 284 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 285 Function name: Stop
Alexshawcroft 9:326b8f261ef0 286 Input Parameters: N/A
Alexshawcroft 9:326b8f261ef0 287 Output Parameters: N/A
Alexshawcroft 9:326b8f261ef0 288 Description: Drives the motor backwards at a designated speed
Alexshawcroft 9:326b8f261ef0 289 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 290 void cMotor::Stop()
Alexshawcroft 9:326b8f261ef0 291 {
Alexshawcroft 9:326b8f261ef0 292 _fwd = 0;
Alexshawcroft 9:326b8f261ef0 293 _rev = 0;
Alexshawcroft 9:326b8f261ef0 294 _pwm = 0;
Alexshawcroft 9:326b8f261ef0 295 }
Alexshawcroft 9:326b8f261ef0 296
Alexshawcroft 9:326b8f261ef0 297
Alexshawcroft 9:326b8f261ef0 298
Alexshawcroft 9:326b8f261ef0 299 /*--------------------------------------------------------------------------------
Alexshawcroft 9:326b8f261ef0 300 Function name: ServoKeySub
Alexshawcroft 9:326b8f261ef0 301 Input Parameters:
Alexshawcroft 9:326b8f261ef0 302 Output Parameters:
Alexshawcroft 9:326b8f261ef0 303 Description:
Alexshawcroft 9:326b8f261ef0 304 ----------------------------------------------------------------------------------*/
Alexshawcroft 9:326b8f261ef0 305 /* Ascii values for arrow Keys
Alexshawcroft 9:326b8f261ef0 306 37(left arrow)
Alexshawcroft 9:326b8f261ef0 307 38(up arrow)
Alexshawcroft 9:326b8f261ef0 308 39(right arrow)
Alexshawcroft 9:326b8f261ef0 309 40(down arrow)
Alexshawcroft 9:326b8f261ef0 310 */
hongyunAHN 11:12e73437dc9f 311 // get the keypress data in rasbarry pi and move the 2 servo motor.
hongyunAHN 11:12e73437dc9f 312 //"j" move servo1 to left arrow, "l" move servo1 to right arrow, "i" move servo2 to up arrow, "k" move servo2 to down arrow.
Alexshawcroft 9:326b8f261ef0 313 void servoKeySub(const std_msgs::UInt8 &ServoKeyPress)
Alexshawcroft 9:326b8f261ef0 314 {
Alexshawcroft 9:326b8f261ef0 315 printf("%d", ServoKeyPress.data);
Alexshawcroft 9:326b8f261ef0 316
Alexshawcroft 9:326b8f261ef0 317 if (ServoKeyPress.data == 106) { // j for Pan Left
Alexshawcroft 9:326b8f261ef0 318 if(Servo1Pos>-45) {
Alexshawcroft 9:326b8f261ef0 319 Servo1Pos = Servo1Pos-IncSize;
Alexshawcroft 9:326b8f261ef0 320 servo1.position(Servo1Pos);
Alexshawcroft 9:326b8f261ef0 321 wait(0.01);
Stumi 4:36a04230554d 322 }
Stumi 4:36a04230554d 323 }
Stumi 4:36a04230554d 324
Alexshawcroft 9:326b8f261ef0 325 else if (ServoKeyPress.data == 108) { // l for Pan Right
Alexshawcroft 9:326b8f261ef0 326 if(Servo1Pos<45) {
Alexshawcroft 9:326b8f261ef0 327 Servo1Pos = Servo1Pos+IncSize;
Alexshawcroft 9:326b8f261ef0 328 servo1.position(Servo1Pos);
Alexshawcroft 9:326b8f261ef0 329 wait(0.01);
Alexshawcroft 9:326b8f261ef0 330 }
Alexshawcroft 9:326b8f261ef0 331 }
Stumi 4:36a04230554d 332
Stumi 4:36a04230554d 333
Alexshawcroft 9:326b8f261ef0 334 else if (ServoKeyPress.data == 105) { // i for Tilt Up
Alexshawcroft 9:326b8f261ef0 335 if(Servo2Pos>-45){
Alexshawcroft 9:326b8f261ef0 336 Servo2Pos = Servo2Pos-IncSize;
Alexshawcroft 9:326b8f261ef0 337 servo2.position(Servo2Pos);
Alexshawcroft 9:326b8f261ef0 338 wait(0.01);
Alexshawcroft 9:326b8f261ef0 339 }
Stumi 8:262c6c40bff9 340 }
Stumi 2:6197e1cf2cd1 341
Alexshawcroft 9:326b8f261ef0 342 else if (ServoKeyPress.data == 107) { // K for Tilt Down
Alexshawcroft 9:326b8f261ef0 343 if(Servo2Pos<45){
Alexshawcroft 9:326b8f261ef0 344 Servo2Pos = Servo2Pos+IncSize;
Alexshawcroft 9:326b8f261ef0 345 servo2.position(Servo2Pos);
Alexshawcroft 9:326b8f261ef0 346 wait(0.01);
Alexshawcroft 9:326b8f261ef0 347 }
Stumi 8:262c6c40bff9 348 }
Stumi 2:6197e1cf2cd1 349
Alexshawcroft 9:326b8f261ef0 350 else if (ServoKeyPress.data == 44) { // for Up
Alexshawcroft 9:326b8f261ef0 351 if(IncSize<20) {
Alexshawcroft 9:326b8f261ef0 352 IncSize=(IncSize+1);
Alexshawcroft 9:326b8f261ef0 353 }
Alexshawcroft 9:326b8f261ef0 354 } else if (ServoKeyPress.data == 46) { // for Up
Alexshawcroft 9:326b8f261ef0 355 if(IncSize>1) {
Alexshawcroft 9:326b8f261ef0 356 IncSize=(IncSize-1);
Alexshawcroft 9:326b8f261ef0 357 }
Stumi 8:262c6c40bff9 358 }
Stumi 3:df6160e2f6d9 359
Alexshawcroft 9:326b8f261ef0 360
Alexshawcroft 9:326b8f261ef0 361
Alexshawcroft 9:326b8f261ef0 362 pc.printf("Servo 1 = %d Servo 2 = %d \n\r",Servo1Pos,Servo2Pos);
Alexshawcroft 9:326b8f261ef0 363 servo1.position(Servo1Pos);
Alexshawcroft 9:326b8f261ef0 364 servo2.position(Servo2Pos);
Alexshawcroft 9:326b8f261ef0 365
Alexshawcroft 9:326b8f261ef0 366 }
Alexshawcroft 9:326b8f261ef0 367
Alexshawcroft 9:326b8f261ef0 368