SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v3

with the tof code

Dependencies:   mbed Servo ros_lib_kinetic

Motors/Motor.cpp@9:326b8f261ef0, 2020-01-04 (annotated)

Committer:
Alexshawcroft
Date:
Sat Jan 04 21:35:25 2020 +0000
Revision:
9:326b8f261ef0
Parent:
8:262c6c40bff9 
oh

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