ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ManualControlFinal

ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

ui.cpp@24:e88753f090b8, 2018-04-03 (annotated)

Committer:
asobhy
Date:
Tue Apr 03 16:00:10 2018 +0000
Revision:
24:e88753f090b8
Parent:
15:cf67f83d5409 
testing reversed camera position picontroller unstable

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