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@10:51dd168b5a96, 2020-01-06 (annotated)

Committer:
Stumi
Date:
Mon Jan 06 20:38:07 2020 +0000
Revision:
10:51dd168b5a96
Parent:
9:326b8f261ef0
Child:
11:12e73437dc9f
added autonomous control to ros commands

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