Baijun Desai / Mbed 2 deprecated MultiModalRobotSM

Multimodal robot implementing: Manual control Hide and Seek

Dependencies:   Motordriver mbed-rtos mbed

Fork of MultiModalRobotSM by Hemanth Koralla

main.cpp@21:21f72bd7f649, 2017-12-12 (annotated)

Committer:
baijun
Date:
Tue Dec 12 03:11:49 2017 +0000
Revision:
21:21f72bd7f649
Parent:
20:e1a78ee68726 
Working code;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kshah521 17:740028fe5c99 1 //This will rotate the wheels so that after 5 seconds the robot counterclockwise and then clockwise
emilmont 1:491820ee784d 2 #include "mbed.h"
mbed_official 11:0309bef74ba8 3 #include "rtos.h"
baijun 14:f413a2b209b0 4 #include "motordriver.h"
baijun 15:9bc36f47c8cf 5 #include "MultiModalRobot.h"
baijun 20:e1a78ee68726 6 #include "uLCD_4DGL.h"
baijun 20:e1a78ee68726 7
baijun 20:e1a78ee68726 8 uLCD_4DGL uLCD(p9,p10,p11); // serial tx, serial rx, reset pin;
baijun 16:47d3b1f2e90d 9
baijun 14:f413a2b209b0 10 Motor lw(p26, p29, p30, 1); // pwm, fwd, rev LEFT WHEEL
baijun 14:f413a2b209b0 11 Motor rw(p25, p28, p27, 1); // pwm, fwd, rev RIGHT WHEEL
baijun 15:9bc36f47c8cf 12 MultiModalRobot robot(lw, rw);
kshah521 17:740028fe5c99 13
baijun 20:e1a78ee68726 14 bool light = true;
kshah521 17:740028fe5c99 15
baijun 20:e1a78ee68726 16 Serial blue(p13, p14);
baijun 20:e1a78ee68726 17 Serial py(USBTX, USBRX);
baijun 19:91bfed0e5929 18
baijun 20:e1a78ee68726 19 BusOut leds(LED1,LED2,LED3,LED4);
baijun 20:e1a78ee68726 20
kshah521 17:740028fe5c99 21
baijun 20:e1a78ee68726 22 //Class to control an RGB LED using three PWM pins
baijun 20:e1a78ee68726 23 class RGBLed
baijun 20:e1a78ee68726 24 {
baijun 20:e1a78ee68726 25 public:
baijun 20:e1a78ee68726 26 RGBLed(PinName redpin, PinName greenpin, PinName bluepin);
baijun 20:e1a78ee68726 27 void write(float red,float green, float blue);
baijun 20:e1a78ee68726 28 private:
baijun 20:e1a78ee68726 29 PwmOut _redpin;
baijun 20:e1a78ee68726 30 PwmOut _greenpin;
baijun 20:e1a78ee68726 31 PwmOut _bluepin;
baijun 20:e1a78ee68726 32 };
baijun 20:e1a78ee68726 33
baijun 20:e1a78ee68726 34 RGBLed::RGBLed (PinName redpin, PinName greenpin, PinName bluepin)
baijun 20:e1a78ee68726 35 : _redpin(redpin), _greenpin(greenpin), _bluepin(bluepin)
baijun 20:e1a78ee68726 36 {
baijun 20:e1a78ee68726 37 //50Hz PWM clock default a bit too low, go to 2000Hz (less flicker)
baijun 20:e1a78ee68726 38 _redpin.period(0.0005);
kshah521 17:740028fe5c99 39 }
kshah521 17:740028fe5c99 40
baijun 20:e1a78ee68726 41 void RGBLed::write(float red,float green, float blue)
baijun 20:e1a78ee68726 42 {
baijun 20:e1a78ee68726 43 _redpin = red;
baijun 20:e1a78ee68726 44 _greenpin = green;
baijun 20:e1a78ee68726 45 _bluepin = blue;
baijun 20:e1a78ee68726 46 }
baijun 20:e1a78ee68726 47 //class could be moved to include file
baijun 20:e1a78ee68726 48
baijun 20:e1a78ee68726 49 RGBLed myRGBled(p21,p22,p23); //RGB PWM pins
baijun 20:e1a78ee68726 50
baijun 20:e1a78ee68726 51 bool visible;
baijun 20:e1a78ee68726 52 char lastSeen;
baijun 20:e1a78ee68726 53
baijun 20:e1a78ee68726 54 int getYpos(){
baijun 20:e1a78ee68726 55 int ypos;
baijun 20:e1a78ee68726 56 char buffer[4];
baijun 20:e1a78ee68726 57 char y;
baijun 20:e1a78ee68726 58 while(1){
baijun 20:e1a78ee68726 59 char start = 0;
baijun 20:e1a78ee68726 60 start = py.getc();
baijun 20:e1a78ee68726 61 if(start == 's' || start == 'v'){
baijun 20:e1a78ee68726 62
baijun 20:e1a78ee68726 63 } else {
baijun 20:e1a78ee68726 64 uLCD.printf("\nUnexpected char %c\n", start);
baijun 20:e1a78ee68726 65 continue;
baijun 20:e1a78ee68726 66 }
baijun 20:e1a78ee68726 67 if (start=='s') {
baijun 20:e1a78ee68726 68 if(py.getc()=='t') {
baijun 20:e1a78ee68726 69 if (py.getc() == 'a') {
baijun 20:e1a78ee68726 70 if (py.getc() == 'r') {
baijun 20:e1a78ee68726 71 if (py.getc() == 't') {
baijun 20:e1a78ee68726 72 if (py.getc() == 'c') {
baijun 20:e1a78ee68726 73 for (int count = 0; count < 4; count++) {
baijun 20:e1a78ee68726 74 y = py.getc();
baijun 20:e1a78ee68726 75 buffer[count] = y;
baijun 20:e1a78ee68726 76 if (count % 3 == 0) {
baijun 20:e1a78ee68726 77 //char snum[5];
baijun 20:e1a78ee68726 78 memcpy(&ypos, buffer, 4);
baijun 20:e1a78ee68726 79 if(ypos>120){
baijun 20:e1a78ee68726 80 lastSeen = 'r';
baijun 20:e1a78ee68726 81 } else {
baijun 20:e1a78ee68726 82 lastSeen = 'l';
baijun 20:e1a78ee68726 83 }
baijun 20:e1a78ee68726 84 if (light) { //Cop lights
baijun 20:e1a78ee68726 85 myRGBled.write(1.0,0.0,0.0);
baijun 20:e1a78ee68726 86 light = false;
baijun 20:e1a78ee68726 87 } else {
baijun 20:e1a78ee68726 88 myRGBled.write(0.0,0.0,1.0);
baijun 20:e1a78ee68726 89 light = true;
baijun 20:e1a78ee68726 90 }
baijun 20:e1a78ee68726 91 }
baijun 20:e1a78ee68726 92 }
baijun 20:e1a78ee68726 93 break;
baijun 20:e1a78ee68726 94 }
baijun 20:e1a78ee68726 95 }
baijun 20:e1a78ee68726 96 }
baijun 20:e1a78ee68726 97 }
baijun 20:e1a78ee68726 98 }
baijun 20:e1a78ee68726 99 } else if(start=='v'){
baijun 20:e1a78ee68726 100 if(py.getc()=='w'){
baijun 20:e1a78ee68726 101 if(py.getc()=='x'){
baijun 20:e1a78ee68726 102 if(py.getc()=='y'){
baijun 20:e1a78ee68726 103 if(py.getc()=='z'){
baijun 20:e1a78ee68726 104 ypos = -1;
baijun 20:e1a78ee68726 105 break;
baijun 20:e1a78ee68726 106 }
baijun 20:e1a78ee68726 107 }
baijun 20:e1a78ee68726 108 }
baijun 20:e1a78ee68726 109 }
baijun 20:e1a78ee68726 110 } else {
baijun 20:e1a78ee68726 111 uLCD.printf("\nUNEXPECTED\n"); //Default Green on black text
baijun 20:e1a78ee68726 112 }
baijun 20:e1a78ee68726 113 }
baijun 20:e1a78ee68726 114 uLCD.cls();
baijun 20:e1a78ee68726 115 uLCD.printf("\nReceived %d\n", ypos); //Default Green on black text
baijun 20:e1a78ee68726 116 //uLCD.freeBUFFER();
baijun 20:e1a78ee68726 117 return ypos;
baijun 20:e1a78ee68726 118 }
baijun 20:e1a78ee68726 119
baijun 20:e1a78ee68726 120
emilmont 1:491820ee784d 121 int main() {
baijun 20:e1a78ee68726 122 lastSeen = 'n';
baijun 20:e1a78ee68726 123 py.baud(9600);
baijun 16:47d3b1f2e90d 124 char bnum = 0;
baijun 16:47d3b1f2e90d 125 char bhit = 0;
baijun 16:47d3b1f2e90d 126 char needToStopRobot = 0;
baijun 16:47d3b1f2e90d 127 float leftSpeed = 0;
baijun 16:47d3b1f2e90d 128 float rightSpeed = 0;
baijun 20:e1a78ee68726 129 float DEFAULT_SPEED = 1.0;
baijun 19:91bfed0e5929 130 float ROTATE_SPEED = 0.25;
baijun 19:91bfed0e5929 131
kshah521 17:740028fe5c99 132 while(1) {
baijun 16:47d3b1f2e90d 133 wait(0.1);
kshah521 17:740028fe5c99 134 if(blue.readable() && blue.getc() == '!') {
kshah521 17:740028fe5c99 135 if(blue.readable() && blue.getc() == 'B') {
baijun 16:47d3b1f2e90d 136 bnum = blue.getc();
baijun 16:47d3b1f2e90d 137 bhit = blue.getc();
baijun 16:47d3b1f2e90d 138 blue.getc();
kshah521 17:740028fe5c99 139 switch(bnum) {
baijun 16:47d3b1f2e90d 140 case '1':
baijun 16:47d3b1f2e90d 141 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 142 break;
kshah521 17:740028fe5c99 143 case '2': // become follower
baijun 20:e1a78ee68726 144 py.putc('s');
baijun 20:e1a78ee68726 145 int ypos;
kshah521 17:740028fe5c99 146 while (1) {
baijun 20:e1a78ee68726 147 ypos = getYpos();
baijun 20:e1a78ee68726 148 if (ypos == 251) { //exit condition since camera is 240 wide
baijun 20:e1a78ee68726 149 myRGBled.write(0.0,1.0,0.0);
baijun 20:e1a78ee68726 150 robot.driveWheels(0, 0);
baijun 20:e1a78ee68726 151 break;
baijun 20:e1a78ee68726 152 }
baijun 20:e1a78ee68726 153 visible = (ypos>=0);
kshah521 17:740028fe5c99 154 if (visible) { // CAMERA CAN SEE A CIRCLE
baijun 20:e1a78ee68726 155 if (light) { //Cop lights
baijun 20:e1a78ee68726 156 myRGBled.write(1.0,0.0,0.0);
baijun 20:e1a78ee68726 157 light = false;
baijun 20:e1a78ee68726 158 } else {
baijun 20:e1a78ee68726 159 myRGBled.write(0.0,0.0,1.0);
baijun 20:e1a78ee68726 160 light = true;
baijun 20:e1a78ee68726 161 }
baijun 20:e1a78ee68726 162 leds[3] = 1;
baijun 19:91bfed0e5929 163 if(ypos<159 && ypos>80) {
baijun 19:91bfed0e5929 164 leftSpeed = DEFAULT_SPEED;
baijun 19:91bfed0e5929 165 rightSpeed = DEFAULT_SPEED;
baijun 19:91bfed0e5929 166 robot.driveWheels(leftSpeed, rightSpeed);
baijun 20:e1a78ee68726 167 leds[0] = 1;
baijun 20:e1a78ee68726 168 leds[1] = 0;
baijun 20:e1a78ee68726 169 leds[2] = 0;
baijun 20:e1a78ee68726 170 } else if(ypos>159) {
baijun 20:e1a78ee68726 171 while(ypos>120){
baijun 20:e1a78ee68726 172 ypos = getYpos();
baijun 20:e1a78ee68726 173 if(ypos<0){
baijun 20:e1a78ee68726 174 break;
baijun 20:e1a78ee68726 175 }
baijun 20:e1a78ee68726 176 leds[0] = 0;
baijun 20:e1a78ee68726 177 leds[1] = 1;
baijun 20:e1a78ee68726 178 leds[2] = 0;
baijun 20:e1a78ee68726 179 leftSpeed = ROTATE_SPEED;
baijun 20:e1a78ee68726 180 rightSpeed = -ROTATE_SPEED;
baijun 20:e1a78ee68726 181 robot.driveWheels(leftSpeed, rightSpeed);
baijun 20:e1a78ee68726 182 }
baijun 20:e1a78ee68726 183 robot.stop(0.5);
baijun 20:e1a78ee68726 184 } else {
baijun 20:e1a78ee68726 185 while(ypos<120){
baijun 20:e1a78ee68726 186 ypos = getYpos();
baijun 20:e1a78ee68726 187 if(ypos<0){
baijun 20:e1a78ee68726 188 break;
baijun 20:e1a78ee68726 189 }
baijun 20:e1a78ee68726 190 leds[0] = 0;
baijun 20:e1a78ee68726 191 leds[1] = 0;
baijun 20:e1a78ee68726 192 leds[2] = 1;
baijun 20:e1a78ee68726 193 leftSpeed = -ROTATE_SPEED;
baijun 20:e1a78ee68726 194 rightSpeed = ROTATE_SPEED;
baijun 20:e1a78ee68726 195 robot.driveWheels(leftSpeed, rightSpeed);
baijun 20:e1a78ee68726 196 }
baijun 20:e1a78ee68726 197 robot.stop(0.5);
baijun 19:91bfed0e5929 198 }
baijun 20:e1a78ee68726 199
baijun 20:e1a78ee68726 200
kshah521 17:740028fe5c99 201 } else { // CIRCLE NOT VISIBLE
baijun 20:e1a78ee68726 202 leds[0] = 0;
baijun 20:e1a78ee68726 203 leds[1] = 0;
baijun 20:e1a78ee68726 204 leds[2] = 0;
baijun 20:e1a78ee68726 205 leds[3] = 0;
baijun 20:e1a78ee68726 206 if(lastSeen=='r'){
baijun 20:e1a78ee68726 207 leftSpeed = ROTATE_SPEED;
baijun 20:e1a78ee68726 208 rightSpeed = -ROTATE_SPEED;
baijun 20:e1a78ee68726 209 } else {
baijun 20:e1a78ee68726 210 leftSpeed = -ROTATE_SPEED;
baijun 20:e1a78ee68726 211 rightSpeed = ROTATE_SPEED;
baijun 20:e1a78ee68726 212 }
kshah521 17:740028fe5c99 213 robot.driveWheels(leftSpeed, rightSpeed);
kshah521 17:740028fe5c99 214 }
kshah521 17:740028fe5c99 215 }
kshah521 17:740028fe5c99 216 // stop thread
baijun 16:47d3b1f2e90d 217 case '5': //up
baijun 16:47d3b1f2e90d 218 if(bhit=='1'){
baijun 16:47d3b1f2e90d 219 leftSpeed = rightSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 220 } else {
baijun 16:47d3b1f2e90d 221 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 222 }
kshah521 17:740028fe5c99 223 break;
baijun 16:47d3b1f2e90d 224 case '6': //down
kshah521 17:740028fe5c99 225 if(bhit=='1') {
baijun 16:47d3b1f2e90d 226 leftSpeed = rightSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 227 } else {
baijun 16:47d3b1f2e90d 228 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 229 }
baijun 16:47d3b1f2e90d 230 break;
baijun 16:47d3b1f2e90d 231 case '7': //left
kshah521 17:740028fe5c99 232 if(bhit=='1') {
baijun 16:47d3b1f2e90d 233 leftSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 234 rightSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 235 } else {
baijun 16:47d3b1f2e90d 236 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 237 }
baijun 16:47d3b1f2e90d 238 break;
baijun 16:47d3b1f2e90d 239 case '8': //right
kshah521 17:740028fe5c99 240 if(bhit=='1') {
baijun 16:47d3b1f2e90d 241 leftSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 242 rightSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 243 } else {
baijun 16:47d3b1f2e90d 244 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 245 }
baijun 16:47d3b1f2e90d 246 break;
baijun 16:47d3b1f2e90d 247 default:
baijun 16:47d3b1f2e90d 248 robot.stop(0.5);
baijun 16:47d3b1f2e90d 249 break;
baijun 16:47d3b1f2e90d 250 }
kshah521 17:740028fe5c99 251 }
baijun 16:47d3b1f2e90d 252 }
baijun 16:47d3b1f2e90d 253 if(needToStopRobot){
baijun 16:47d3b1f2e90d 254 robot.stop(0.5);
baijun 16:47d3b1f2e90d 255 needToStopRobot = 0;
baijun 16:47d3b1f2e90d 256 leftSpeed = 0;
baijun 16:47d3b1f2e90d 257 rightSpeed = 0;
kshah521 17:740028fe5c99 258 robot.driveWheels(leftSpeed, rightSpeed);
baijun 16:47d3b1f2e90d 259 } else {
kshah521 17:740028fe5c99 260 robot.driveWheels(leftSpeed, rightSpeed);
baijun 16:47d3b1f2e90d 261 }
emilmont 1:491820ee784d 262 }
kshah521 17:740028fe5c99 263 }