ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ObjectFollower

ObjectFollower

Dependencies:   TPixy-Interface

Fork of PlayBack by ECE4333 - 2018 - Ahmed & Brandon

ui.cpp@14:5777377537a2, 2018-03-03 (annotated)

Committer:
asobhy
Date:
Sat Mar 03 02:14:40 2018 +0000
Revision:
14:5777377537a2
Parent:
12:172c448a359e
Child:
15:cf67f83d5409
averaging code added to following robot but results are not that good

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