ECE4333 - 2018 - Ahmed & Brandon / Mbed OS ObjectFollower

ObjectFollower

Dependencies:   TPixy-Interface

Fork of PlayBack by ECE4333 - 2018 - Ahmed & Brandon

ui.cpp@5:b29220d29022, 2018-02-09 (annotated)

Committer:
asobhy
Date:
Fri Feb 09 20:26:04 2018 +0000
Revision:
5:b29220d29022
Parent:
4:417e475239c7 
time constant calculation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
asobhy 0:a355e511bc5d 1 #include "mbed.h"
asobhy 0:a355e511bc5d 2 #include "WatchdogThread.h"
asobhy 0:a355e511bc5d 3 #include "ui.h"
asobhy 0:a355e511bc5d 4
asobhy 0:a355e511bc5d 5 Serial bluetooth(p9,p10); // Define the bluetooth channel and IO pins
asobhy 0:a355e511bc5d 6 Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel
asobhy 0:a355e511bc5d 7
asobhy 0:a355e511bc5d 8 Mutex setpoint_mutex;
asobhy 0:a355e511bc5d 9
asobhy 0:a355e511bc5d 10 // speed
asobhy 1:3e9684e81312 11 int setpoint = 0;
asobhy 0:a355e511bc5d 12
asobhy 0:a355e511bc5d 13 // variable to store character recieved from terminal
asobhy 0:a355e511bc5d 14 char x;
asobhy 0:a355e511bc5d 15
asobhy 1:3e9684e81312 16 extern int16_t dPosition, dTime;
asobhy 1:3e9684e81312 17 extern float Ki;
asobhy 1:3e9684e81312 18 extern float Kp;
asobhy 1:3e9684e81312 19 extern int vel;
asobhy 2:ca2a7430739b 20 extern int32_t e;
asobhy 2:ca2a7430739b 21 extern int32_t xState;
asobhy 2:ca2a7430739b 22 extern int32_t u;
asobhy 1:3e9684e81312 23
asobhy 1:3e9684e81312 24 int16_t position;
asobhy 1:3e9684e81312 25
asobhy 5:b29220d29022 26 bool start_counting = false;
asobhy 2:ca2a7430739b 27
asobhy 0:a355e511bc5d 28
asobhy 0:a355e511bc5d 29 /******************************************************************************
asobhy 0:a355e511bc5d 30 A function to test blutooth communication
asobhy 0:a355e511bc5d 31 ******************************************************************************/
asobhy 0:a355e511bc5d 32 void twoTerminalsTest()
asobhy 0:a355e511bc5d 33 {
asobhy 0:a355e511bc5d 34 if (pc.readable()) { // If a key is pressed on the pc channel
asobhy 0:a355e511bc5d 35 x=pc.getc(); // read the character and send it to both the
asobhy 0:a355e511bc5d 36 bluetooth.putc(x); // bluetooth channel and the pc channel for
asobhy 0:a355e511bc5d 37 pc.putc(x); // display.
asobhy 0:a355e511bc5d 38 }
asobhy 0:a355e511bc5d 39 if (bluetooth.readable()) {
asobhy 0:a355e511bc5d 40 x=bluetooth.getc(); // If there’s a keypress on the bluetooth
asobhy 0:a355e511bc5d 41 pc.putc(x); // channel, read the character and send it to
asobhy 0:a355e511bc5d 42 bluetooth.putc(x); // both the pc channel and the bluetooth
asobhy 0:a355e511bc5d 43 } // channel for display
asobhy 0:a355e511bc5d 44 }
asobhy 0:a355e511bc5d 45
asobhy 0:a355e511bc5d 46
asobhy 0:a355e511bc5d 47 /******************************************************************************
asobhy 0:a355e511bc5d 48 A function to Display startup Messsage
asobhy 0:a355e511bc5d 49 ******************************************************************************/
asobhy 0:a355e511bc5d 50 void displayStartupMsg()
asobhy 0:a355e511bc5d 51 {
asobhy 1:3e9684e81312 52 bluetooth.printf("\r\n************************************");
asobhy 1:3e9684e81312 53 bluetooth.printf("\r\n**** DC Motor Control using PWM ****");
asobhy 1:3e9684e81312 54 bluetooth.printf("\r\n************************************");
asobhy 1:3e9684e81312 55 bluetooth.printf("\r\n-Enter r to reset the watchdog timer");
asobhy 1:3e9684e81312 56 bluetooth.printf("\r\n-press w to increase motor speed");
asobhy 1:3e9684e81312 57 bluetooth.printf("\r\n-press s to decrease motor speed");
asobhy 0:a355e511bc5d 58 }
asobhy 0:a355e511bc5d 59
asobhy 0:a355e511bc5d 60
asobhy 0:a355e511bc5d 61 /******************************************************************************
asobhy 1:3e9684e81312 62 User interface
asobhy 0:a355e511bc5d 63 ******************************************************************************/
asobhy 4:417e475239c7 64 /*
asobhy 0:a355e511bc5d 65 void consoleUI(void)
asobhy 0:a355e511bc5d 66 {
asobhy 1:3e9684e81312 67 if (bluetooth.readable()) {
asobhy 1:3e9684e81312 68 x = bluetooth.getc();
asobhy 1:3e9684e81312 69
asobhy 0:a355e511bc5d 70 // if input from console is the letter 'r'
asobhy 0:a355e511bc5d 71 if(x == 'r') {
asobhy 0:a355e511bc5d 72 // reset watchdog timer
asobhy 0:a355e511bc5d 73 WatchdogReset();
asobhy 1:3e9684e81312 74 bluetooth.printf("\r\nWatchdog has been reset");
asobhy 0:a355e511bc5d 75 }
asobhy 0:a355e511bc5d 76
asobhy 0:a355e511bc5d 77 // if w is pressed increase the speed
asobhy 0:a355e511bc5d 78 // by incrementing u
asobhy 0:a355e511bc5d 79 else if(x == 'w') {
asobhy 1:3e9684e81312 80 setpoint_mutex.lock();
asobhy 1:3e9684e81312 81 if ( setpoint < 560 )
asobhy 1:3e9684e81312 82 {
asobhy 1:3e9684e81312 83 //setpoint = setpoint + SPEED_STEP;
asobhy 1:3e9684e81312 84 setpoint = 100;
asobhy 0:a355e511bc5d 85 }
asobhy 1:3e9684e81312 86 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 87
asobhy 0:a355e511bc5d 88 // display speed
asobhy 1:3e9684e81312 89 bluetooth.printf("\r\n %5d", setpoint);
asobhy 0:a355e511bc5d 90 }
asobhy 0:a355e511bc5d 91
asobhy 0:a355e511bc5d 92 // if s is pressed decrease the speed
asobhy 0:a355e511bc5d 93 // by decrementing u
asobhy 0:a355e511bc5d 94 else if(x == 's') {
asobhy 1:3e9684e81312 95
asobhy 1:3e9684e81312 96 setpoint_mutex.lock();
asobhy 1:3e9684e81312 97 if (setpoint > -560)
asobhy 1:3e9684e81312 98 {
asobhy 1:3e9684e81312 99 setpoint = -100;
asobhy 1:3e9684e81312 100 //setpoint = setpoint - SPEED_STEP;
asobhy 0:a355e511bc5d 101 }
asobhy 1:3e9684e81312 102
asobhy 1:3e9684e81312 103 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 104
asobhy 0:a355e511bc5d 105 // display speed
asobhy 1:3e9684e81312 106 bluetooth.printf("\r\n %5d", setpoint);
asobhy 0:a355e511bc5d 107 }
asobhy 0:a355e511bc5d 108
asobhy 0:a355e511bc5d 109 // error wrong input
asobhy 0:a355e511bc5d 110 else {
asobhy 1:3e9684e81312 111 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 112 }
asobhy 0:a355e511bc5d 113 }
asobhy 0:a355e511bc5d 114 position += dPosition;
asobhy 2:ca2a7430739b 115 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 116
asobhy 0:a355e511bc5d 117 }
asobhy 4:417e475239c7 118 */
asobhy 1:3e9684e81312 119 /******************************************************************************
asobhy 1:3e9684e81312 120 User interface 2
asobhy 1:3e9684e81312 121 ******************************************************************************/
asobhy 1:3e9684e81312 122
asobhy 4:417e475239c7 123 void consoleUI(void)
asobhy 1:3e9684e81312 124 {
asobhy 1:3e9684e81312 125
asobhy 1:3e9684e81312 126 if (bluetooth.readable()) {
asobhy 1:3e9684e81312 127 x = bluetooth.getc();
asobhy 1:3e9684e81312 128
asobhy 1:3e9684e81312 129 // if input from console is the letter 'r'
asobhy 1:3e9684e81312 130 if(x == 'r') {
asobhy 1:3e9684e81312 131 // reset watchdog timer
asobhy 1:3e9684e81312 132 WatchdogReset();
asobhy 1:3e9684e81312 133 setpoint = 0;
asobhy 1:3e9684e81312 134 bluetooth.printf("\r\nWatchdog has been reset");
asobhy 1:3e9684e81312 135 }
asobhy 1:3e9684e81312 136
asobhy 1:3e9684e81312 137 // if w is pressed increase the speed
asobhy 1:3e9684e81312 138 // by incrementing u
asobhy 1:3e9684e81312 139 else if(x == 'w') {
asobhy 1:3e9684e81312 140 setpoint_mutex.lock();
asobhy 2:ca2a7430739b 141 if ( setpoint < 506 )
asobhy 1:3e9684e81312 142 {
asobhy 1:3e9684e81312 143 //setpoint = setpoint + SPEED_STEP;
asobhy 5:b29220d29022 144 setpoint = 420;
asobhy 5:b29220d29022 145 start_counting = true;
asobhy 1:3e9684e81312 146 }
asobhy 1:3e9684e81312 147 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 148
asobhy 1:3e9684e81312 149 // display speed
asobhy 1:3e9684e81312 150 bluetooth.printf("\r\n %5d", setpoint);
asobhy 1:3e9684e81312 151 }
asobhy 1:3e9684e81312 152
asobhy 1:3e9684e81312 153 // if s is pressed decrease the speed
asobhy 1:3e9684e81312 154 // by decrementing u
asobhy 1:3e9684e81312 155 else if(x == 's') {
asobhy 1:3e9684e81312 156
asobhy 1:3e9684e81312 157 setpoint_mutex.lock();
asobhy 2:ca2a7430739b 158 if (setpoint > -560)
asobhy 1:3e9684e81312 159 {
asobhy 4:417e475239c7 160 setpoint = -400;
asobhy 1:3e9684e81312 161 //setpoint = setpoint - SPEED_STEP;
asobhy 1:3e9684e81312 162 }
asobhy 1:3e9684e81312 163
asobhy 1:3e9684e81312 164 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 165
asobhy 1:3e9684e81312 166 // display speed
asobhy 1:3e9684e81312 167 bluetooth.printf("\r\n %5d", setpoint);
asobhy 1:3e9684e81312 168 }
asobhy 4:417e475239c7 169 else if (x=='i')
asobhy 1:3e9684e81312 170 {
asobhy 4:417e475239c7 171 Ki = Ki + 0.001;
asobhy 1:3e9684e81312 172 }
asobhy 4:417e475239c7 173 else if (x=='k')
asobhy 1:3e9684e81312 174 {
asobhy 4:417e475239c7 175 Ki = Ki - 0.001;
asobhy 1:3e9684e81312 176 }
asobhy 4:417e475239c7 177 else if (x=='o')
asobhy 1:3e9684e81312 178 {
asobhy 4:417e475239c7 179 Kp = Kp + 0.01;
asobhy 1:3e9684e81312 180 }
asobhy 4:417e475239c7 181 else if (x=='l')
asobhy 1:3e9684e81312 182 {
asobhy 4:417e475239c7 183 Kp = Kp - 0.01;
asobhy 1:3e9684e81312 184 }
asobhy 1:3e9684e81312 185
asobhy 1:3e9684e81312 186 // error wrong input
asobhy 1:3e9684e81312 187 else {
asobhy 1:3e9684e81312 188 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 189 }
asobhy 1:3e9684e81312 190 }
asobhy 1:3e9684e81312 191
asobhy 1:3e9684e81312 192 position += dPosition;
asobhy 2:ca2a7430739b 193
asobhy 1:3e9684e81312 194 //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 195
asobhy 5:b29220d29022 196 //bluetooth.printf("\r\ndP: %d, vel: %d, Kp: %f, Ki: %f, e: %d, xState: %d", dPosition, vel, Kp, Ki, e, xState);
asobhy 2:ca2a7430739b 197
asobhy 2:ca2a7430739b 198 //bluetooth.printf("\r\ne: %d, Pos: %d, dP: %d, xState: %d, u: %d, dT: %d", e, position, dPosition, xState, u, dTime);
asobhy 1:3e9684e81312 199
asobhy 5:b29220d29022 200 bluetooth.printf("\r\n%d", dPosition);
asobhy 5:b29220d29022 201
asobhy 5:b29220d29022 202
asobhy 5:b29220d29022 203
asobhy 1:3e9684e81312 204 }