ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ObjectFollower

ObjectFollower

Dependencies:   TPixy-Interface

Fork of PlayBack by ECE4333 - 2018 - Ahmed & Brandon

ui.cpp@15:cf67f83d5409, 2018-03-03 (annotated)

Committer:
asobhy
Date:
Sat Mar 03 04:47:26 2018 +0000
Revision:
15:cf67f83d5409
Parent:
14:5777377537a2
Child:
17:1184df616383
Child:
24:e88753f090b8
following robot almost done. Need to get a better object to follow. Need to use a torch to light up the object. Need to adjust width of object in camera settings to match new object then test.

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 0:a355e511bc5d 17
asobhy 0:a355e511bc5d 18 Serial bluetooth(p9,p10); // Define the bluetooth channel and IO pins
asobhy 0:a355e511bc5d 19 Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel
asobhy 0:a355e511bc5d 20
asobhy 9:fe56b888985c 21 Mutex setpointR_mutex;
asobhy 9:fe56b888985c 22 Mutex setpointL_mutex;
asobhy 0:a355e511bc5d 23
asobhy 0:a355e511bc5d 24 // speed
asobhy 9:fe56b888985c 25 int setpointR = 0;
asobhy 9:fe56b888985c 26 int setpointL = 0;
asobhy 9:fe56b888985c 27
asobhy 15:cf67f83d5409 28 bool killRobot = false;
asobhy 15:cf67f83d5409 29
asobhy 0:a355e511bc5d 30 // variable to store character recieved from terminal
asobhy 0:a355e511bc5d 31 char x;
asobhy 0:a355e511bc5d 32
asobhy 1:3e9684e81312 33 int16_t position;
asobhy 1:3e9684e81312 34
asobhy 14:5777377537a2 35 /******************************************************************************
asobhy 14:5777377537a2 36 User interface 3
asobhy 14:5777377537a2 37 ******************************************************************************/
asobhy 14:5777377537a2 38 void consoleUI(void){
asobhy 14:5777377537a2 39
asobhy 15:cf67f83d5409 40 cameraData_mutex.lock();
asobhy 15:cf67f83d5409 41 bluetooth.printf("\r\nsetpointR: %d setpointL: %d ObjectWidth: %d ObjectXcoordinate: %d SteeringError: %d DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError);
asobhy 15:cf67f83d5409 42 cameraData_mutex.unlock();
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
asobhy 15:cf67f83d5409 45 // safety mechanism
asobhy 15:cf67f83d5409 46 if (bluetooth.readable()){
asobhy 15:cf67f83d5409 47 x = bluetooth.getc();
asobhy 15:cf67f83d5409 48 if (x == 'r')
asobhy 15:cf67f83d5409 49 {
asobhy 15:cf67f83d5409 50 killRobot = true;
asobhy 15:cf67f83d5409 51 }
asobhy 15:cf67f83d5409 52 else if (x == 's')
asobhy 15:cf67f83d5409 53 {
asobhy 15:cf67f83d5409 54 killRobot = false;
asobhy 15:cf67f83d5409 55 }
asobhy 15:cf67f83d5409 56 }
asobhy 15:cf67f83d5409 57
asobhy 14:5777377537a2 58 }
asobhy 14:5777377537a2 59
asobhy 2:ca2a7430739b 60
asobhy 0:a355e511bc5d 61
asobhy 0:a355e511bc5d 62 /******************************************************************************
asobhy 0:a355e511bc5d 63 A function to test blutooth communication
asobhy 0:a355e511bc5d 64 ******************************************************************************/
asobhy 0:a355e511bc5d 65 void twoTerminalsTest()
asobhy 0:a355e511bc5d 66 {
asobhy 0:a355e511bc5d 67 if (pc.readable()) { // If a key is pressed on the pc channel
asobhy 0:a355e511bc5d 68 x=pc.getc(); // read the character and send it to both the
asobhy 0:a355e511bc5d 69 bluetooth.putc(x); // bluetooth channel and the pc channel for
asobhy 0:a355e511bc5d 70 pc.putc(x); // display.
asobhy 0:a355e511bc5d 71 }
asobhy 0:a355e511bc5d 72 if (bluetooth.readable()) {
asobhy 0:a355e511bc5d 73 x=bluetooth.getc(); // If there’s a keypress on the bluetooth
asobhy 0:a355e511bc5d 74 pc.putc(x); // channel, read the character and send it to
asobhy 0:a355e511bc5d 75 bluetooth.putc(x); // both the pc channel and the bluetooth
asobhy 0:a355e511bc5d 76 } // channel for display
asobhy 0:a355e511bc5d 77 }
asobhy 0:a355e511bc5d 78
asobhy 0:a355e511bc5d 79
asobhy 0:a355e511bc5d 80 /******************************************************************************
asobhy 0:a355e511bc5d 81 A function to Display startup Messsage
asobhy 0:a355e511bc5d 82 ******************************************************************************/
asobhy 15:cf67f83d5409 83 void displayStartupMsg_old()
asobhy 0:a355e511bc5d 84 {
asobhy 1:3e9684e81312 85 bluetooth.printf("\r\n************************************");
asobhy 1:3e9684e81312 86 bluetooth.printf("\r\n**** DC Motor Control using PWM ****");
asobhy 1:3e9684e81312 87 bluetooth.printf("\r\n************************************");
asobhy 1:3e9684e81312 88 bluetooth.printf("\r\n-Enter r to reset the watchdog timer");
asobhy 9:fe56b888985c 89 bluetooth.printf("\r\n-press w to increase motor speedR");
asobhy 9:fe56b888985c 90 bluetooth.printf("\r\n-press s to decrease motor speedR");
asobhy 9:fe56b888985c 91 bluetooth.printf("\r\n-press i to increase motor speedL");
asobhy 9:fe56b888985c 92 bluetooth.printf("\r\n-press k to decrease motor speedL");
asobhy 0:a355e511bc5d 93 }
asobhy 0:a355e511bc5d 94
asobhy 15:cf67f83d5409 95 void displayStartupMsg()
asobhy 15:cf67f83d5409 96 {
asobhy 15:cf67f83d5409 97 bluetooth.printf("\r\n************************************");
asobhy 15:cf67f83d5409 98 bluetooth.printf("\r\n**** AUTONOMOUS FOLLOWING ROBOT ****");
asobhy 15:cf67f83d5409 99 bluetooth.printf("\r\n************************************");
asobhy 15:cf67f83d5409 100 bluetooth.printf("\r\n-PRESS 'r' TO KILL ROBOT");
asobhy 15:cf67f83d5409 101 }
asobhy 0:a355e511bc5d 102
asobhy 0:a355e511bc5d 103 /******************************************************************************
asobhy 8:a0890fa79084 104 User interface 1
asobhy 0:a355e511bc5d 105 ******************************************************************************/
asobhy 4:417e475239c7 106 /*
asobhy 0:a355e511bc5d 107 void consoleUI(void)
asobhy 0:a355e511bc5d 108 {
asobhy 1:3e9684e81312 109 if (bluetooth.readable()) {
asobhy 1:3e9684e81312 110 x = bluetooth.getc();
asobhy 1:3e9684e81312 111
asobhy 0:a355e511bc5d 112 // if input from console is the letter 'r'
asobhy 0:a355e511bc5d 113 if(x == 'r') {
asobhy 0:a355e511bc5d 114 // reset watchdog timer
asobhy 0:a355e511bc5d 115 WatchdogReset();
asobhy 1:3e9684e81312 116 bluetooth.printf("\r\nWatchdog has been reset");
asobhy 0:a355e511bc5d 117 }
asobhy 0:a355e511bc5d 118
asobhy 0:a355e511bc5d 119 // if w is pressed increase the speed
asobhy 0:a355e511bc5d 120 // by incrementing u
asobhy 0:a355e511bc5d 121 else if(x == 'w') {
asobhy 1:3e9684e81312 122 setpoint_mutex.lock();
asobhy 1:3e9684e81312 123 if ( setpoint < 560 )
asobhy 1:3e9684e81312 124 {
asobhy 1:3e9684e81312 125 //setpoint = setpoint + SPEED_STEP;
asobhy 1:3e9684e81312 126 setpoint = 100;
asobhy 0:a355e511bc5d 127 }
asobhy 1:3e9684e81312 128 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 129
asobhy 0:a355e511bc5d 130 // display speed
asobhy 1:3e9684e81312 131 bluetooth.printf("\r\n %5d", setpoint);
asobhy 0:a355e511bc5d 132 }
asobhy 0:a355e511bc5d 133
asobhy 0:a355e511bc5d 134 // if s is pressed decrease the speed
asobhy 0:a355e511bc5d 135 // by decrementing u
asobhy 0:a355e511bc5d 136 else if(x == 's') {
asobhy 1:3e9684e81312 137
asobhy 1:3e9684e81312 138 setpoint_mutex.lock();
asobhy 1:3e9684e81312 139 if (setpoint > -560)
asobhy 1:3e9684e81312 140 {
asobhy 1:3e9684e81312 141 setpoint = -100;
asobhy 1:3e9684e81312 142 //setpoint = setpoint - SPEED_STEP;
asobhy 0:a355e511bc5d 143 }
asobhy 1:3e9684e81312 144
asobhy 1:3e9684e81312 145 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 146
asobhy 0:a355e511bc5d 147 // display speed
asobhy 1:3e9684e81312 148 bluetooth.printf("\r\n %5d", setpoint);
asobhy 0:a355e511bc5d 149 }
asobhy 0:a355e511bc5d 150
asobhy 0:a355e511bc5d 151 // error wrong input
asobhy 0:a355e511bc5d 152 else {
asobhy 1:3e9684e81312 153 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 154 }
asobhy 0:a355e511bc5d 155 }
asobhy 0:a355e511bc5d 156 position += dPosition;
asobhy 2:ca2a7430739b 157 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 158
asobhy 0:a355e511bc5d 159 }
asobhy 4:417e475239c7 160 */
asobhy 1:3e9684e81312 161 /******************************************************************************
asobhy 1:3e9684e81312 162 User interface 2
asobhy 1:3e9684e81312 163 ******************************************************************************/
asobhy 1:3e9684e81312 164
asobhy 14:5777377537a2 165 void consoleUIold(void)
asobhy 1:3e9684e81312 166 {
asobhy 1:3e9684e81312 167
asobhy 1:3e9684e81312 168 if (bluetooth.readable()) {
asobhy 1:3e9684e81312 169 x = bluetooth.getc();
asobhy 1:3e9684e81312 170
asobhy 1:3e9684e81312 171 // if input from console is the letter 'r'
asobhy 1:3e9684e81312 172 if(x == 'r') {
asobhy 1:3e9684e81312 173 // reset watchdog timer
asobhy 1:3e9684e81312 174 WatchdogReset();
asobhy 9:fe56b888985c 175 setpointR = 0;
asobhy 9:fe56b888985c 176 setpointL = 0;
asobhy 1:3e9684e81312 177 bluetooth.printf("\r\nWatchdog has been reset");
asobhy 1:3e9684e81312 178 }
asobhy 9:fe56b888985c 179
asobhy 9:fe56b888985c 180 /******************************RIGHT MOTOR*************************************/
asobhy 1:3e9684e81312 181 // if w is pressed increase the speed
asobhy 1:3e9684e81312 182 // by incrementing u
asobhy 1:3e9684e81312 183 else if(x == 'w') {
asobhy 9:fe56b888985c 184 setpointR_mutex.lock();
asobhy 9:fe56b888985c 185 if ( setpointR < 560 )
asobhy 1:3e9684e81312 186 {
asobhy 1:3e9684e81312 187 //setpoint = setpoint + SPEED_STEP;
asobhy 9:fe56b888985c 188 setpointR = 280;
asobhy 1:3e9684e81312 189 }
asobhy 9:fe56b888985c 190 setpointR_mutex.unlock();
asobhy 12:172c448a359e 191 bluetooth.printf("\r\n %5d", setpointR);
asobhy 1:3e9684e81312 192 }
asobhy 1:3e9684e81312 193
asobhy 1:3e9684e81312 194 // if s is pressed decrease the speed
asobhy 1:3e9684e81312 195 // by decrementing u
asobhy 1:3e9684e81312 196 else if(x == 's') {
asobhy 1:3e9684e81312 197
asobhy 9:fe56b888985c 198 setpointR_mutex.lock();
asobhy 9:fe56b888985c 199 if (setpointR > -560)
asobhy 1:3e9684e81312 200 {
asobhy 9:fe56b888985c 201 setpointR = -280;
asobhy 1:3e9684e81312 202 //setpoint = setpoint - SPEED_STEP;
asobhy 1:3e9684e81312 203 }
asobhy 1:3e9684e81312 204
asobhy 9:fe56b888985c 205 setpointR_mutex.unlock();
asobhy 1:3e9684e81312 206
asobhy 1:3e9684e81312 207 // display speed
asobhy 9:fe56b888985c 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 9:fe56b888985c 215 if ( setpointL < 560 )
asobhy 9:fe56b888985c 216 {
asobhy 9:fe56b888985c 217 //setpoint = setpoint + SPEED_STEP;
asobhy 9:fe56b888985c 218 setpointL = 280;
asobhy 9:fe56b888985c 219 }
asobhy 9:fe56b888985c 220 setpointL_mutex.unlock();
asobhy 12:172c448a359e 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 9:fe56b888985c 226 if (setpointL > -560)
asobhy 9:fe56b888985c 227 {
asobhy 9:fe56b888985c 228 setpointL = -280;
asobhy 9:fe56b888985c 229 //setpoint = setpoint - SPEED_STEP;
asobhy 9:fe56b888985c 230 }
asobhy 9:fe56b888985c 231
asobhy 9:fe56b888985c 232 setpointL_mutex.unlock();
asobhy 9:fe56b888985c 233
asobhy 9:fe56b888985c 234 // display speed
asobhy 9:fe56b888985c 235 bluetooth.printf("\r\n %5d", setpointL);
asobhy 1:3e9684e81312 236 }
asobhy 9:fe56b888985c 237 /******************************END MOTOR SETPOINT******************************/
asobhy 1:3e9684e81312 238
asobhy 1:3e9684e81312 239 // error wrong input
asobhy 1:3e9684e81312 240 else {
asobhy 1:3e9684e81312 241 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 242 }
asobhy 1:3e9684e81312 243 }
asobhy 1:3e9684e81312 244
asobhy 8:a0890fa79084 245
asobhy 8:a0890fa79084 246
asobhy 1:3e9684e81312 247 //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 248
asobhy 8:a0890fa79084 249 //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 250
asobhy 9:fe56b888985c 251 /*
asobhy 8:a0890fa79084 252 bluetooth.printf("\r\n %d, ", e);
asobhy 8:a0890fa79084 253 bluetooth.printf("%d, ", dPosition);
asobhy 8:a0890fa79084 254 bluetooth.printf("%d, ", xState);
asobhy 8:a0890fa79084 255 bluetooth.printf("%d, ", u);
asobhy 9:fe56b888985c 256 */
asobhy 2:ca2a7430739b 257
asobhy 2:ca2a7430739b 258 //bluetooth.printf("\r\ne: %d, Pos: %d, dP: %d, xState: %d, u: %d, dT: %d", e, position, dPosition, xState, u, dTime);
asobhy 1:3e9684e81312 259
asobhy 1:3e9684e81312 260 }
asobhy 14:5777377537a2 261
asobhy 14:5777377537a2 262