ECE4333 - 2018 - Ahmed & Brandon / Mbed OS MbedOS_Robot_Team

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot by ECE4333 - 2018 - Ahmed & Brandon

ui.cpp@17:1184df616383, 2018-03-11 (annotated)

Committer:
asobhy
Date:
Sun Mar 11 01:26:23 2018 +0000
Revision:
17:1184df616383
Parent:
15:cf67f83d5409
Child:
18:db6d9fc1ebd0
MANUAL CONTROL - TESTING AND TUNNING REQUIRED

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 14:5777377537a2 38 User interface 3
asobhy 14:5777377537a2 39 ******************************************************************************/
asobhy 14:5777377537a2 40 void consoleUI(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 9:fe56b888985c 189 if ( setpointR < 560 )
asobhy 1:3e9684e81312 190 {
asobhy 1:3e9684e81312 191 //setpoint = setpoint + SPEED_STEP;
asobhy 9:fe56b888985c 192 setpointR = 280;
asobhy 1:3e9684e81312 193 }
asobhy 9:fe56b888985c 194 setpointR_mutex.unlock();
asobhy 12:172c448a359e 195 bluetooth.printf("\r\n %5d", setpointR);
asobhy 1:3e9684e81312 196 }
asobhy 1:3e9684e81312 197
asobhy 1:3e9684e81312 198 // if s is pressed decrease the speed
asobhy 1:3e9684e81312 199 // by decrementing u
asobhy 1:3e9684e81312 200 else if(x == 's') {
asobhy 1:3e9684e81312 201
asobhy 9:fe56b888985c 202 setpointR_mutex.lock();
asobhy 9:fe56b888985c 203 if (setpointR > -560)
asobhy 1:3e9684e81312 204 {
asobhy 9:fe56b888985c 205 setpointR = -280;
asobhy 1:3e9684e81312 206 //setpoint = setpoint - SPEED_STEP;
asobhy 1:3e9684e81312 207 }
asobhy 1:3e9684e81312 208
asobhy 9:fe56b888985c 209 setpointR_mutex.unlock();
asobhy 1:3e9684e81312 210
asobhy 1:3e9684e81312 211 // display speed
asobhy 9:fe56b888985c 212 bluetooth.printf("\r\n %5d", setpointR);
asobhy 1:3e9684e81312 213 }
asobhy 9:fe56b888985c 214
asobhy 9:fe56b888985c 215 /******************************LEFT MOTOR**************************************/
asobhy 4:417e475239c7 216 else if (x=='i')
asobhy 1:3e9684e81312 217 {
asobhy 9:fe56b888985c 218 setpointL_mutex.lock();
asobhy 9:fe56b888985c 219 if ( setpointL < 560 )
asobhy 9:fe56b888985c 220 {
asobhy 9:fe56b888985c 221 //setpoint = setpoint + SPEED_STEP;
asobhy 9:fe56b888985c 222 setpointL = 280;
asobhy 9:fe56b888985c 223 }
asobhy 9:fe56b888985c 224 setpointL_mutex.unlock();
asobhy 12:172c448a359e 225 bluetooth.printf("\r\n %5d", setpointL);
asobhy 1:3e9684e81312 226 }
asobhy 4:417e475239c7 227 else if (x=='k')
asobhy 1:3e9684e81312 228 {
asobhy 9:fe56b888985c 229 setpointL_mutex.lock();
asobhy 9:fe56b888985c 230 if (setpointL > -560)
asobhy 9:fe56b888985c 231 {
asobhy 9:fe56b888985c 232 setpointL = -280;
asobhy 9:fe56b888985c 233 //setpoint = setpoint - SPEED_STEP;
asobhy 9:fe56b888985c 234 }
asobhy 9:fe56b888985c 235
asobhy 9:fe56b888985c 236 setpointL_mutex.unlock();
asobhy 9:fe56b888985c 237
asobhy 9:fe56b888985c 238 // display speed
asobhy 9:fe56b888985c 239 bluetooth.printf("\r\n %5d", setpointL);
asobhy 1:3e9684e81312 240 }
asobhy 9:fe56b888985c 241 /******************************END MOTOR SETPOINT******************************/
asobhy 1:3e9684e81312 242
asobhy 1:3e9684e81312 243 // error wrong input
asobhy 1:3e9684e81312 244 else {
asobhy 1:3e9684e81312 245 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 246 }
asobhy 1:3e9684e81312 247 }
asobhy 1:3e9684e81312 248
asobhy 8:a0890fa79084 249
asobhy 8:a0890fa79084 250
asobhy 1:3e9684e81312 251 //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 252
asobhy 8:a0890fa79084 253 //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 254
asobhy 9:fe56b888985c 255 /*
asobhy 8:a0890fa79084 256 bluetooth.printf("\r\n %d, ", e);
asobhy 8:a0890fa79084 257 bluetooth.printf("%d, ", dPosition);
asobhy 8:a0890fa79084 258 bluetooth.printf("%d, ", xState);
asobhy 8:a0890fa79084 259 bluetooth.printf("%d, ", u);
asobhy 9:fe56b888985c 260 */
asobhy 2:ca2a7430739b 261
asobhy 2:ca2a7430739b 262 //bluetooth.printf("\r\ne: %d, Pos: %d, dP: %d, xState: %d, u: %d, dT: %d", e, position, dPosition, xState, u, dTime);
asobhy 1:3e9684e81312 263
asobhy 1:3e9684e81312 264 }
asobhy 14:5777377537a2 265
asobhy 17:1184df616383 266 /******************************************************************************
asobhy 17:1184df616383 267 User interface 3 - manual control
asobhy 17:1184df616383 268 ******************************************************************************/
asobhy 14:5777377537a2 269
asobhy 17:1184df616383 270 void consoleUIManualControl(void)
asobhy 17:1184df616383 271 {
asobhy 17:1184df616383 272
asobhy 17:1184df616383 273 if (bluetooth.readable()) {
asobhy 17:1184df616383 274 x = bluetooth.getc();
asobhy 17:1184df616383 275
asobhy 17:1184df616383 276 // if input from console is the letter 'r'
asobhy 17:1184df616383 277 if(x == 'r') {
asobhy 17:1184df616383 278 // reset watchdog timer
asobhy 17:1184df616383 279 WatchdogReset();
asobhy 17:1184df616383 280 setpointR = 0;
asobhy 17:1184df616383 281 setpointL = 0;
asobhy 17:1184df616383 282 bluetooth.printf("\r\nWatchdog has been reset");
asobhy 17:1184df616383 283 }
asobhy 17:1184df616383 284
asobhy 17:1184df616383 285 /******************************ROBOT FWD RVS***********************************/
asobhy 17:1184df616383 286 // if w is pressed increase the speed
asobhy 17:1184df616383 287 // by incrementing u
asobhy 17:1184df616383 288 else if(x == 'w') {
asobhy 17:1184df616383 289 mutexSetpoint.lock();
asobhy 17:1184df616383 290 Setpoint = Setpoint + 100;
asobhy 17:1184df616383 291 mutexSetpoint.unlock();
asobhy 17:1184df616383 292
asobhy 17:1184df616383 293 }
asobhy 17:1184df616383 294
asobhy 17:1184df616383 295 // if s is pressed decrease the speed
asobhy 17:1184df616383 296 // by decrementing u
asobhy 17:1184df616383 297 else if(x == 's') {
asobhy 17:1184df616383 298 mutexSetpoint.lock();
asobhy 17:1184df616383 299 Setpoint = Setpoint - 100;
asobhy 17:1184df616383 300 mutexSetpoint.unlock();
asobhy 17:1184df616383 301
asobhy 17:1184df616383 302 }
asobhy 17:1184df616383 303
asobhy 17:1184df616383 304 /******************************ROBOT STEERING**********************************/
asobhy 17:1184df616383 305 else if (x=='d')
asobhy 17:1184df616383 306 {
asobhy 17:1184df616383 307 mutexSetpoint.lock();
asobhy 17:1184df616383 308 SteeringError = SteeringError + 10;
asobhy 17:1184df616383 309 mutexSetpoint.unlock();
asobhy 17:1184df616383 310 }
asobhy 17:1184df616383 311 else if (x=='a')
asobhy 17:1184df616383 312 {
asobhy 17:1184df616383 313 mutexSetpoint.lock();
asobhy 17:1184df616383 314 SteeringError = SteeringError - 10;
asobhy 17:1184df616383 315 mutexSetpoint.unlock();
asobhy 17:1184df616383 316 }
asobhy 17:1184df616383 317 // error wrong input
asobhy 17:1184df616383 318 else {
asobhy 17:1184df616383 319 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 320 }
asobhy 17:1184df616383 321 }
asobhy 17:1184df616383 322
asobhy 17:1184df616383 323 // If no key is pressed stop the robot.
asobhy 17:1184df616383 324 Setpoint = 0;
asobhy 17:1184df616383 325 SteeringError = 0;
asobhy 17:1184df616383 326
asobhy 17:1184df616383 327 }