ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ObjectFollower

ObjectFollower

Dependencies:   TPixy-Interface

Fork of PlayBack by ECE4333 - 2018 - Ahmed & Brandon

ui.cpp@20:9118203f7c9c, 2018-03-23 (annotated)

Committer:
asobhy
Date:
Fri Mar 23 22:10:38 2018 +0000
Revision:
20:9118203f7c9c
Parent:
19:4fbffc755ed7
Child:
22:c09acff62e6a
Manual Control - After Increasing Samp. Freq. to 10ms

Who changed what in which revision?

UserRevisionLine numberNew contents of line
asobhy 8:a0890fa79084 1 /******************************************************************************/
asobhy 8:a0890fa79084 2 // ECE4333
asobhy 9:fe56b888985c 3 // LAB Partner 1: Ahmed Sobhy - ID: 3594449
asobhy 9:fe56b888985c 4 // LAB Partner 2: Brandon Kingman - ID: 3470444
asobhy 9:fe56b888985c 5 // Project: Autonomous Robot Design
asobhy 9:fe56b888985c 6 // Instructor: Prof. Chris Diduch
asobhy 8:a0890fa79084 7 /******************************************************************************/
asobhy 8:a0890fa79084 8 // filename: ui.cpp
asobhy 8:a0890fa79084 9 // file content description:
asobhy 8:a0890fa79084 10 // * Functions to display and manage the user interface on the PC terminal.
asobhy 8:a0890fa79084 11 /******************************************************************************/
asobhy 8:a0890fa79084 12
asobhy 0:a355e511bc5d 13 #include "mbed.h"
asobhy 0:a355e511bc5d 14 #include "WatchdogThread.h"
asobhy 0:a355e511bc5d 15 #include "ui.h"
asobhy 15:cf67f83d5409 16 #include "CameraThread.h"
asobhy 17:1184df616383 17 #include "PiControlThread.h"
asobhy 0:a355e511bc5d 18
asobhy 0:a355e511bc5d 19 Serial bluetooth(p9,p10); // Define the bluetooth channel and IO pins
asobhy 0:a355e511bc5d 20 Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel
asobhy 0:a355e511bc5d 21
asobhy 9:fe56b888985c 22 Mutex setpointR_mutex;
asobhy 9:fe56b888985c 23 Mutex setpointL_mutex;
asobhy 0:a355e511bc5d 24
asobhy 20:9118203f7c9c 25
asobhy 0:a355e511bc5d 26 // speed
asobhy 9:fe56b888985c 27 int setpointR = 0;
asobhy 9:fe56b888985c 28 int setpointL = 0;
asobhy 9:fe56b888985c 29
asobhy 17:1184df616383 30
asobhy 15:cf67f83d5409 31 bool killRobot = false;
asobhy 15:cf67f83d5409 32
asobhy 0:a355e511bc5d 33 // variable to store character recieved from terminal
asobhy 0:a355e511bc5d 34 char x;
asobhy 17:1184df616383 35 int16_t position;
asobhy 0:a355e511bc5d 36
asobhy 1:3e9684e81312 37
asobhy 14:5777377537a2 38 /******************************************************************************
asobhy 20:9118203f7c9c 39 User interface 3 Object Follower
asobhy 14:5777377537a2 40 ******************************************************************************/
asobhy 18:db6d9fc1ebd0 41 void consoleUIcamera(void){
asobhy 14:5777377537a2 42
asobhy 15:cf67f83d5409 43 cameraData_mutex.lock();
asobhy 15:cf67f83d5409 44 bluetooth.printf("\r\nsetpointR: %d setpointL: %d ObjectWidth: %d ObjectXcoordinate: %d SteeringError: %d DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError);
asobhy 15:cf67f83d5409 45 cameraData_mutex.unlock();
asobhy 15:cf67f83d5409 46 //bluetooth.printf("\r\nsetpointR: %d setpointL: %d ObjectWidth: %d ObjectXcoordinate: %d SteeringError: %d DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError);
asobhy 15:cf67f83d5409 47
asobhy 15:cf67f83d5409 48 // safety mechanism
asobhy 15:cf67f83d5409 49 if (bluetooth.readable()){
asobhy 15:cf67f83d5409 50 x = bluetooth.getc();
asobhy 17:1184df616383 51 if (x == 't')
asobhy 15:cf67f83d5409 52 {
asobhy 15:cf67f83d5409 53 killRobot = true;
asobhy 15:cf67f83d5409 54 }
asobhy 17:1184df616383 55 else if (x == 'r')
asobhy 15:cf67f83d5409 56 {
asobhy 15:cf67f83d5409 57 killRobot = false;
asobhy 15:cf67f83d5409 58 }
asobhy 17:1184df616383 59
asobhy 17:1184df616383 60
asobhy 15:cf67f83d5409 61 }
asobhy 15:cf67f83d5409 62
asobhy 14:5777377537a2 63 }
asobhy 14:5777377537a2 64
asobhy 2:ca2a7430739b 65
asobhy 0:a355e511bc5d 66
asobhy 0:a355e511bc5d 67 /******************************************************************************
asobhy 0:a355e511bc5d 68 A function to test blutooth communication
asobhy 0:a355e511bc5d 69 ******************************************************************************/
asobhy 0:a355e511bc5d 70 void twoTerminalsTest()
asobhy 0:a355e511bc5d 71 {
asobhy 0:a355e511bc5d 72 if (pc.readable()) { // If a key is pressed on the pc channel
asobhy 0:a355e511bc5d 73 x=pc.getc(); // read the character and send it to both the
asobhy 0:a355e511bc5d 74 bluetooth.putc(x); // bluetooth channel and the pc channel for
asobhy 0:a355e511bc5d 75 pc.putc(x); // display.
asobhy 0:a355e511bc5d 76 }
asobhy 0:a355e511bc5d 77 if (bluetooth.readable()) {
asobhy 0:a355e511bc5d 78 x=bluetooth.getc(); // If there’s a keypress on the bluetooth
asobhy 0:a355e511bc5d 79 pc.putc(x); // channel, read the character and send it to
asobhy 0:a355e511bc5d 80 bluetooth.putc(x); // both the pc channel and the bluetooth
asobhy 0:a355e511bc5d 81 } // channel for display
asobhy 0:a355e511bc5d 82 }
asobhy 0:a355e511bc5d 83
asobhy 0:a355e511bc5d 84
asobhy 0:a355e511bc5d 85 /******************************************************************************
asobhy 0:a355e511bc5d 86 A function to Display startup Messsage
asobhy 0:a355e511bc5d 87 ******************************************************************************/
asobhy 15:cf67f83d5409 88 void displayStartupMsg_old()
asobhy 0:a355e511bc5d 89 {
asobhy 1:3e9684e81312 90 bluetooth.printf("\r\n************************************");
asobhy 1:3e9684e81312 91 bluetooth.printf("\r\n**** DC Motor Control using PWM ****");
asobhy 1:3e9684e81312 92 bluetooth.printf("\r\n************************************");
asobhy 1:3e9684e81312 93 bluetooth.printf("\r\n-Enter r to reset the watchdog timer");
asobhy 9:fe56b888985c 94 bluetooth.printf("\r\n-press w to increase motor speedR");
asobhy 9:fe56b888985c 95 bluetooth.printf("\r\n-press s to decrease motor speedR");
asobhy 9:fe56b888985c 96 bluetooth.printf("\r\n-press i to increase motor speedL");
asobhy 9:fe56b888985c 97 bluetooth.printf("\r\n-press k to decrease motor speedL");
asobhy 0:a355e511bc5d 98 }
asobhy 0:a355e511bc5d 99
asobhy 15:cf67f83d5409 100 void displayStartupMsg()
asobhy 15:cf67f83d5409 101 {
asobhy 15:cf67f83d5409 102 bluetooth.printf("\r\n************************************");
asobhy 15:cf67f83d5409 103 bluetooth.printf("\r\n**** AUTONOMOUS FOLLOWING ROBOT ****");
asobhy 15:cf67f83d5409 104 bluetooth.printf("\r\n************************************");
asobhy 15:cf67f83d5409 105 bluetooth.printf("\r\n-PRESS 'r' TO KILL ROBOT");
asobhy 15:cf67f83d5409 106 }
asobhy 0:a355e511bc5d 107
asobhy 0:a355e511bc5d 108 /******************************************************************************
asobhy 20:9118203f7c9c 109 User interface 1 - PI tunning
asobhy 0:a355e511bc5d 110 ******************************************************************************/
asobhy 4:417e475239c7 111 /*
asobhy 0:a355e511bc5d 112 void consoleUI(void)
asobhy 0:a355e511bc5d 113 {
asobhy 1:3e9684e81312 114 if (bluetooth.readable()) {
asobhy 1:3e9684e81312 115 x = bluetooth.getc();
asobhy 1:3e9684e81312 116
asobhy 0:a355e511bc5d 117 // if input from console is the letter 'r'
asobhy 0:a355e511bc5d 118 if(x == 'r') {
asobhy 0:a355e511bc5d 119 // reset watchdog timer
asobhy 0:a355e511bc5d 120 WatchdogReset();
asobhy 1:3e9684e81312 121 bluetooth.printf("\r\nWatchdog has been reset");
asobhy 0:a355e511bc5d 122 }
asobhy 0:a355e511bc5d 123
asobhy 0:a355e511bc5d 124 // if w is pressed increase the speed
asobhy 0:a355e511bc5d 125 // by incrementing u
asobhy 0:a355e511bc5d 126 else if(x == 'w') {
asobhy 1:3e9684e81312 127 setpoint_mutex.lock();
asobhy 1:3e9684e81312 128 if ( setpoint < 560 )
asobhy 1:3e9684e81312 129 {
asobhy 1:3e9684e81312 130 //setpoint = setpoint + SPEED_STEP;
asobhy 1:3e9684e81312 131 setpoint = 100;
asobhy 0:a355e511bc5d 132 }
asobhy 1:3e9684e81312 133 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 134
asobhy 0:a355e511bc5d 135 // display speed
asobhy 1:3e9684e81312 136 bluetooth.printf("\r\n %5d", setpoint);
asobhy 0:a355e511bc5d 137 }
asobhy 0:a355e511bc5d 138
asobhy 0:a355e511bc5d 139 // if s is pressed decrease the speed
asobhy 0:a355e511bc5d 140 // by decrementing u
asobhy 0:a355e511bc5d 141 else if(x == 's') {
asobhy 1:3e9684e81312 142
asobhy 1:3e9684e81312 143 setpoint_mutex.lock();
asobhy 1:3e9684e81312 144 if (setpoint > -560)
asobhy 1:3e9684e81312 145 {
asobhy 1:3e9684e81312 146 setpoint = -100;
asobhy 1:3e9684e81312 147 //setpoint = setpoint - SPEED_STEP;
asobhy 0:a355e511bc5d 148 }
asobhy 1:3e9684e81312 149
asobhy 1:3e9684e81312 150 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 151
asobhy 0:a355e511bc5d 152 // display speed
asobhy 1:3e9684e81312 153 bluetooth.printf("\r\n %5d", setpoint);
asobhy 0:a355e511bc5d 154 }
asobhy 0:a355e511bc5d 155
asobhy 0:a355e511bc5d 156 // error wrong input
asobhy 0:a355e511bc5d 157 else {
asobhy 1:3e9684e81312 158 bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog");
asobhy 0:a355e511bc5d 159 }
asobhy 0:a355e511bc5d 160 }
asobhy 0:a355e511bc5d 161 position += dPosition;
asobhy 2:ca2a7430739b 162 bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, xState, dTime, Kp, Ki, vel, e);
asobhy 0:a355e511bc5d 163
asobhy 0:a355e511bc5d 164 }
asobhy 4:417e475239c7 165 */
asobhy 1:3e9684e81312 166 /******************************************************************************
asobhy 20:9118203f7c9c 167 User interface 2 - old
asobhy 1:3e9684e81312 168 ******************************************************************************/
asobhy 1:3e9684e81312 169
asobhy 14:5777377537a2 170 void consoleUIold(void)
asobhy 1:3e9684e81312 171 {
asobhy 1:3e9684e81312 172
asobhy 1:3e9684e81312 173 if (bluetooth.readable()) {
asobhy 1:3e9684e81312 174 x = bluetooth.getc();
asobhy 1:3e9684e81312 175
asobhy 1:3e9684e81312 176 // if input from console is the letter 'r'
asobhy 1:3e9684e81312 177 if(x == 'r') {
asobhy 1:3e9684e81312 178 // reset watchdog timer
asobhy 1:3e9684e81312 179 WatchdogReset();
asobhy 9:fe56b888985c 180 setpointR = 0;
asobhy 9:fe56b888985c 181 setpointL = 0;
asobhy 1:3e9684e81312 182 bluetooth.printf("\r\nWatchdog has been reset");
asobhy 1:3e9684e81312 183 }
asobhy 9:fe56b888985c 184
asobhy 9:fe56b888985c 185 /******************************RIGHT MOTOR*************************************/
asobhy 1:3e9684e81312 186 // if w is pressed increase the speed
asobhy 1:3e9684e81312 187 // by incrementing u
asobhy 1:3e9684e81312 188 else if(x == 'w') {
asobhy 9:fe56b888985c 189 setpointR_mutex.lock();
asobhy 19:4fbffc755ed7 190 //setpoint = setpoint + SPEED_STEP;
asobhy 19:4fbffc755ed7 191 setpointR = 25;
asobhy 9:fe56b888985c 192 setpointR_mutex.unlock();
asobhy 19:4fbffc755ed7 193
asobhy 12:172c448a359e 194 bluetooth.printf("\r\n %5d", setpointR);
asobhy 1:3e9684e81312 195 }
asobhy 1:3e9684e81312 196
asobhy 1:3e9684e81312 197 // if s is pressed decrease the speed
asobhy 1:3e9684e81312 198 // by decrementing u
asobhy 1:3e9684e81312 199 else if(x == 's') {
asobhy 1:3e9684e81312 200
asobhy 9:fe56b888985c 201 setpointR_mutex.lock();
asobhy 19:4fbffc755ed7 202
asobhy 19:4fbffc755ed7 203 setpointR = -25;
asobhy 19:4fbffc755ed7 204 //setpoint = setpoint - SPEED_STEP;
asobhy 19:4fbffc755ed7 205
asobhy 9:fe56b888985c 206 setpointR_mutex.unlock();
asobhy 1:3e9684e81312 207
asobhy 1:3e9684e81312 208 // display speed
asobhy 19:4fbffc755ed7 209 //bluetooth.printf("\r\n %5d", setpointR);
asobhy 1:3e9684e81312 210 }
asobhy 9:fe56b888985c 211
asobhy 9:fe56b888985c 212 /******************************LEFT MOTOR**************************************/
asobhy 4:417e475239c7 213 else if (x=='i')
asobhy 1:3e9684e81312 214 {
asobhy 9:fe56b888985c 215 setpointL_mutex.lock();
asobhy 19:4fbffc755ed7 216
asobhy 19:4fbffc755ed7 217
asobhy 9:fe56b888985c 218 //setpoint = setpoint + SPEED_STEP;
asobhy 19:4fbffc755ed7 219 setpointL = 25;
asobhy 19:4fbffc755ed7 220
asobhy 9:fe56b888985c 221 setpointL_mutex.unlock();
asobhy 19:4fbffc755ed7 222 //bluetooth.printf("\r\n %5d", setpointL);
asobhy 1:3e9684e81312 223 }
asobhy 4:417e475239c7 224 else if (x=='k')
asobhy 1:3e9684e81312 225 {
asobhy 9:fe56b888985c 226 setpointL_mutex.lock();
asobhy 19:4fbffc755ed7 227
asobhy 19:4fbffc755ed7 228 setpointL = -25;
asobhy 19:4fbffc755ed7 229 //setpoint = setpoint - SPEED_STEP;
asobhy 19:4fbffc755ed7 230
asobhy 9:fe56b888985c 231 setpointL_mutex.unlock();
asobhy 9:fe56b888985c 232
asobhy 9:fe56b888985c 233 // display speed
asobhy 19:4fbffc755ed7 234 //bluetooth.printf("\r\n %5d", setpointL);
asobhy 1:3e9684e81312 235 }
asobhy 9:fe56b888985c 236 /******************************END MOTOR SETPOINT******************************/
asobhy 1:3e9684e81312 237
asobhy 1:3e9684e81312 238 // error wrong input
asobhy 1:3e9684e81312 239 else {
asobhy 1:3e9684e81312 240 bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog");
asobhy 1:3e9684e81312 241 }
asobhy 1:3e9684e81312 242 }
asobhy 1:3e9684e81312 243
asobhy 8:a0890fa79084 244
asobhy 8:a0890fa79084 245
asobhy 1:3e9684e81312 246 //bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e);
asobhy 2:ca2a7430739b 247
asobhy 8:a0890fa79084 248 //bluetooth.printf("\r\ndP: %d, vel: %d, (Kp*e) Kp: %f, (Ki*xState) Ki: %f, e: %d, xState: %d, u: %d", dPosition, vel, Kp, Ki, e, xState , u);
asobhy 8:a0890fa79084 249
asobhy 9:fe56b888985c 250 /*
asobhy 8:a0890fa79084 251 bluetooth.printf("\r\n %d, ", e);
asobhy 8:a0890fa79084 252 bluetooth.printf("%d, ", dPosition);
asobhy 8:a0890fa79084 253 bluetooth.printf("%d, ", xState);
asobhy 8:a0890fa79084 254 bluetooth.printf("%d, ", u);
asobhy 9:fe56b888985c 255 */
asobhy 2:ca2a7430739b 256
asobhy 2:ca2a7430739b 257 //bluetooth.printf("\r\ne: %d, Pos: %d, dP: %d, xState: %d, u: %d, dT: %d", e, position, dPosition, xState, u, dTime);
asobhy 1:3e9684e81312 258
asobhy 1:3e9684e81312 259 }
asobhy 14:5777377537a2 260
asobhy 17:1184df616383 261 /******************************************************************************
asobhy 20:9118203f7c9c 262 User interface 3 - Manual Control
asobhy 17:1184df616383 263 ******************************************************************************/
asobhy 14:5777377537a2 264
asobhy 18:db6d9fc1ebd0 265 void consoleUI(void)
asobhy 17:1184df616383 266 {
asobhy 17:1184df616383 267 if (bluetooth.readable()) {
asobhy 17:1184df616383 268 x = bluetooth.getc();
asobhy 17:1184df616383 269
asobhy 17:1184df616383 270 // if input from console is the letter 'r'
asobhy 17:1184df616383 271 if(x == 'r') {
asobhy 17:1184df616383 272 // reset watchdog timer
asobhy 17:1184df616383 273 WatchdogReset();
asobhy 17:1184df616383 274 setpointR = 0;
asobhy 17:1184df616383 275 setpointL = 0;
asobhy 17:1184df616383 276 bluetooth.printf("\r\nWatchdog has been reset");
asobhy 17:1184df616383 277 }
asobhy 17:1184df616383 278
asobhy 17:1184df616383 279 /******************************ROBOT FWD RVS***********************************/
asobhy 17:1184df616383 280 // if w is pressed increase the speed
asobhy 17:1184df616383 281 // by incrementing u
asobhy 17:1184df616383 282 else if(x == 'w') {
asobhy 17:1184df616383 283 mutexSetpoint.lock();
asobhy 20:9118203f7c9c 284 Setpoint = -40;
asobhy 17:1184df616383 285 mutexSetpoint.unlock();
asobhy 17:1184df616383 286
asobhy 17:1184df616383 287 }
asobhy 17:1184df616383 288
asobhy 17:1184df616383 289 // if s is pressed decrease the speed
asobhy 17:1184df616383 290 // by decrementing u
asobhy 17:1184df616383 291 else if(x == 's') {
asobhy 17:1184df616383 292 mutexSetpoint.lock();
asobhy 20:9118203f7c9c 293 Setpoint = 40;
asobhy 17:1184df616383 294 mutexSetpoint.unlock();
asobhy 17:1184df616383 295
asobhy 17:1184df616383 296 }
asobhy 17:1184df616383 297
asobhy 17:1184df616383 298 /******************************ROBOT STEERING**********************************/
asobhy 18:db6d9fc1ebd0 299 else if (x=='a')
asobhy 18:db6d9fc1ebd0 300 {
asobhy 18:db6d9fc1ebd0 301 mutexSetpoint.lock();
asobhy 20:9118203f7c9c 302 SteeringError = 80;
asobhy 18:db6d9fc1ebd0 303 mutexSetpoint.unlock();
asobhy 18:db6d9fc1ebd0 304 }
asobhy 17:1184df616383 305 else if (x=='d')
asobhy 17:1184df616383 306 {
asobhy 17:1184df616383 307 mutexSetpoint.lock();
asobhy 20:9118203f7c9c 308 SteeringError = -80;
asobhy 17:1184df616383 309 mutexSetpoint.unlock();
asobhy 17:1184df616383 310 }
asobhy 17:1184df616383 311 // error wrong input
asobhy 17:1184df616383 312 else {
asobhy 17:1184df616383 313 bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog");
asobhy 17:1184df616383 314 }
asobhy 17:1184df616383 315 }
asobhy 18:db6d9fc1ebd0 316 else{
asobhy 18:db6d9fc1ebd0 317 // If no key is pressed stop the robot.
asobhy 18:db6d9fc1ebd0 318 Setpoint = 0;
asobhy 18:db6d9fc1ebd0 319 SteeringError = 0;
asobhy 18:db6d9fc1ebd0 320 }
asobhy 17:1184df616383 321
asobhy 17:1184df616383 322 }