PlayBack

Dependencies:   TPixy-Interface

Fork of ObjectFollower by ECE4333 - 2018 - Ahmed & Brandon

Committer:
asobhy
Date:
Sat Feb 03 00:05:08 2018 +0000
Revision:
1:3e9684e81312
Parent:
0:a355e511bc5d
Child:
2:ca2a7430739b
setpoint represent velocity

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
asobhy 1:3e9684e81312 18 extern float Ki;
asobhy 1:3e9684e81312 19 extern float Kp;
asobhy 0:a355e511bc5d 20
asobhy 1:3e9684e81312 21 extern int vel;
asobhy 1:3e9684e81312 22
asobhy 1:3e9684e81312 23 int16_t position;
asobhy 1:3e9684e81312 24
asobhy 1:3e9684e81312 25 extern int32_t e;
asobhy 0:a355e511bc5d 26
asobhy 0:a355e511bc5d 27 /******************************************************************************
asobhy 0:a355e511bc5d 28 A function to test blutooth communication
asobhy 0:a355e511bc5d 29 ******************************************************************************/
asobhy 0:a355e511bc5d 30 void twoTerminalsTest()
asobhy 0:a355e511bc5d 31 {
asobhy 0:a355e511bc5d 32 if (pc.readable()) { // If a key is pressed on the pc channel
asobhy 0:a355e511bc5d 33 x=pc.getc(); // read the character and send it to both the
asobhy 0:a355e511bc5d 34 bluetooth.putc(x); // bluetooth channel and the pc channel for
asobhy 0:a355e511bc5d 35 pc.putc(x); // display.
asobhy 0:a355e511bc5d 36 }
asobhy 0:a355e511bc5d 37 if (bluetooth.readable()) {
asobhy 0:a355e511bc5d 38 x=bluetooth.getc(); // If there’s a keypress on the bluetooth
asobhy 0:a355e511bc5d 39 pc.putc(x); // channel, read the character and send it to
asobhy 0:a355e511bc5d 40 bluetooth.putc(x); // both the pc channel and the bluetooth
asobhy 0:a355e511bc5d 41 } // channel for display
asobhy 0:a355e511bc5d 42 }
asobhy 0:a355e511bc5d 43
asobhy 0:a355e511bc5d 44
asobhy 0:a355e511bc5d 45 /******************************************************************************
asobhy 0:a355e511bc5d 46 A function to Display startup Messsage
asobhy 0:a355e511bc5d 47 ******************************************************************************/
asobhy 0:a355e511bc5d 48 void displayStartupMsg()
asobhy 0:a355e511bc5d 49 {
asobhy 1:3e9684e81312 50 bluetooth.printf("\r\n************************************");
asobhy 1:3e9684e81312 51 bluetooth.printf("\r\n**** DC Motor Control using PWM ****");
asobhy 1:3e9684e81312 52 bluetooth.printf("\r\n************************************");
asobhy 1:3e9684e81312 53 bluetooth.printf("\r\n-Enter r to reset the watchdog timer");
asobhy 1:3e9684e81312 54 bluetooth.printf("\r\n-press w to increase motor speed");
asobhy 1:3e9684e81312 55 bluetooth.printf("\r\n-press s to decrease motor speed");
asobhy 0:a355e511bc5d 56 }
asobhy 0:a355e511bc5d 57
asobhy 0:a355e511bc5d 58
asobhy 0:a355e511bc5d 59 /******************************************************************************
asobhy 1:3e9684e81312 60 User interface
asobhy 0:a355e511bc5d 61 ******************************************************************************/
asobhy 0:a355e511bc5d 62 void consoleUI(void)
asobhy 0:a355e511bc5d 63 {
asobhy 1:3e9684e81312 64 if (bluetooth.readable()) {
asobhy 1:3e9684e81312 65 x = bluetooth.getc();
asobhy 1:3e9684e81312 66
asobhy 0:a355e511bc5d 67 // if input from console is the letter 'r'
asobhy 0:a355e511bc5d 68 if(x == 'r') {
asobhy 0:a355e511bc5d 69 // reset watchdog timer
asobhy 0:a355e511bc5d 70 WatchdogReset();
asobhy 1:3e9684e81312 71 bluetooth.printf("\r\nWatchdog has been reset");
asobhy 0:a355e511bc5d 72 }
asobhy 0:a355e511bc5d 73
asobhy 0:a355e511bc5d 74 // if w is pressed increase the speed
asobhy 0:a355e511bc5d 75 // by incrementing u
asobhy 0:a355e511bc5d 76 else if(x == 'w') {
asobhy 1:3e9684e81312 77 setpoint_mutex.lock();
asobhy 1:3e9684e81312 78 if ( setpoint < 560 )
asobhy 1:3e9684e81312 79 {
asobhy 1:3e9684e81312 80 //setpoint = setpoint + SPEED_STEP;
asobhy 1:3e9684e81312 81 setpoint = 100;
asobhy 0:a355e511bc5d 82 }
asobhy 1:3e9684e81312 83 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 84
asobhy 0:a355e511bc5d 85 // display speed
asobhy 1:3e9684e81312 86 bluetooth.printf("\r\n %5d", setpoint);
asobhy 0:a355e511bc5d 87 }
asobhy 0:a355e511bc5d 88
asobhy 0:a355e511bc5d 89 // if s is pressed decrease the speed
asobhy 0:a355e511bc5d 90 // by decrementing u
asobhy 0:a355e511bc5d 91 else if(x == 's') {
asobhy 1:3e9684e81312 92
asobhy 1:3e9684e81312 93 setpoint_mutex.lock();
asobhy 1:3e9684e81312 94 if (setpoint > -560)
asobhy 1:3e9684e81312 95 {
asobhy 1:3e9684e81312 96 setpoint = -100;
asobhy 1:3e9684e81312 97 //setpoint = setpoint - SPEED_STEP;
asobhy 0:a355e511bc5d 98 }
asobhy 1:3e9684e81312 99
asobhy 1:3e9684e81312 100 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 101
asobhy 0:a355e511bc5d 102 // display speed
asobhy 1:3e9684e81312 103 bluetooth.printf("\r\n %5d", setpoint);
asobhy 0:a355e511bc5d 104 }
asobhy 0:a355e511bc5d 105
asobhy 0:a355e511bc5d 106 // error wrong input
asobhy 0:a355e511bc5d 107 else {
asobhy 1:3e9684e81312 108 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 109 }
asobhy 0:a355e511bc5d 110 }
asobhy 0:a355e511bc5d 111 position += dPosition;
asobhy 1:3e9684e81312 112 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 0:a355e511bc5d 113
asobhy 0:a355e511bc5d 114 }
asobhy 0:a355e511bc5d 115
asobhy 1:3e9684e81312 116 /******************************************************************************
asobhy 1:3e9684e81312 117 User interface 2
asobhy 1:3e9684e81312 118 ******************************************************************************/
asobhy 1:3e9684e81312 119
asobhy 1:3e9684e81312 120 void consoleUI2(void)
asobhy 1:3e9684e81312 121 {
asobhy 1:3e9684e81312 122
asobhy 1:3e9684e81312 123 if (bluetooth.readable()) {
asobhy 1:3e9684e81312 124 x = bluetooth.getc();
asobhy 1:3e9684e81312 125
asobhy 1:3e9684e81312 126 // if input from console is the letter 'r'
asobhy 1:3e9684e81312 127 if(x == 'r') {
asobhy 1:3e9684e81312 128 // reset watchdog timer
asobhy 1:3e9684e81312 129 WatchdogReset();
asobhy 1:3e9684e81312 130 setpoint = 0;
asobhy 1:3e9684e81312 131 bluetooth.printf("\r\nWatchdog has been reset");
asobhy 1:3e9684e81312 132 }
asobhy 1:3e9684e81312 133
asobhy 1:3e9684e81312 134 // if w is pressed increase the speed
asobhy 1:3e9684e81312 135 // by incrementing u
asobhy 1:3e9684e81312 136 else if(x == 'w') {
asobhy 1:3e9684e81312 137 setpoint_mutex.lock();
asobhy 1:3e9684e81312 138 if ( setpoint < 703 )
asobhy 1:3e9684e81312 139 {
asobhy 1:3e9684e81312 140 //setpoint = setpoint + SPEED_STEP;
asobhy 1:3e9684e81312 141 setpoint = 200;
asobhy 1:3e9684e81312 142 }
asobhy 1:3e9684e81312 143 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 144
asobhy 1:3e9684e81312 145 // display speed
asobhy 1:3e9684e81312 146 bluetooth.printf("\r\n %5d", setpoint);
asobhy 1:3e9684e81312 147 }
asobhy 1:3e9684e81312 148
asobhy 1:3e9684e81312 149 // if s is pressed decrease the speed
asobhy 1:3e9684e81312 150 // by decrementing u
asobhy 1:3e9684e81312 151 else if(x == 's') {
asobhy 1:3e9684e81312 152
asobhy 1:3e9684e81312 153 setpoint_mutex.lock();
asobhy 1:3e9684e81312 154 if (setpoint > -703)
asobhy 1:3e9684e81312 155 {
asobhy 1:3e9684e81312 156 setpoint = -200;
asobhy 1:3e9684e81312 157 //setpoint = setpoint - SPEED_STEP;
asobhy 1:3e9684e81312 158 }
asobhy 1:3e9684e81312 159
asobhy 1:3e9684e81312 160 setpoint_mutex.unlock();
asobhy 1:3e9684e81312 161
asobhy 1:3e9684e81312 162 // display speed
asobhy 1:3e9684e81312 163 bluetooth.printf("\r\n %5d", setpoint);
asobhy 1:3e9684e81312 164 }
asobhy 1:3e9684e81312 165 else if (x=='e')
asobhy 1:3e9684e81312 166 {
asobhy 1:3e9684e81312 167 Ki = Ki + 0.005;
asobhy 1:3e9684e81312 168 }
asobhy 1:3e9684e81312 169 else if (x=='d')
asobhy 1:3e9684e81312 170 {
asobhy 1:3e9684e81312 171 Ki = Ki - 0.005;
asobhy 1:3e9684e81312 172 }
asobhy 1:3e9684e81312 173 else if (x=='t')
asobhy 1:3e9684e81312 174 {
asobhy 1:3e9684e81312 175 Kp = Kp + 0.05;
asobhy 1:3e9684e81312 176 }
asobhy 1:3e9684e81312 177 else if (x=='g')
asobhy 1:3e9684e81312 178 {
asobhy 1:3e9684e81312 179 Kp = Kp - 0.05;
asobhy 1:3e9684e81312 180 }
asobhy 1:3e9684e81312 181
asobhy 1:3e9684e81312 182 // error wrong input
asobhy 1:3e9684e81312 183 else {
asobhy 1:3e9684e81312 184 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 185 }
asobhy 1:3e9684e81312 186 }
asobhy 1:3e9684e81312 187
asobhy 1:3e9684e81312 188 position += dPosition;
asobhy 1:3e9684e81312 189 //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 1:3e9684e81312 190 bluetooth.printf("\r\nKp: %f, Ki: %f, vel: %d, e: %d", Kp, Ki, vel, e);
asobhy 1:3e9684e81312 191
asobhy 1:3e9684e81312 192 }