ECE4333 - 2018 - Ahmed & Brandon / Mbed OS PlayBack

PlayBack

Dependencies:   TPixy-Interface

Fork of ManualControlFinal by ECE4333 - 2018 - Ahmed & Brandon

ui.cpp@19:4fbffc755ed7, 2018-03-23 (annotated)

Committer:
asobhy
Date:
Fri Mar 23 19:03:35 2018 +0000
Revision:
19:4fbffc755ed7
Parent:
18:db6d9fc1ebd0
Child:
20:9118203f7c9c
increasing sampling rate manual control

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