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@19:91bfed0e5929, 2017-12-08 (annotated)

Committer:
baijun
Date:
Fri Dec 08 01:15:16 2017 +0000
Revision:
19:91bfed0e5929
Parent:
18:1b0196cf779f
Child:
20:e1a78ee68726
changes;

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 16:47d3b1f2e90d 6
baijun 14:f413a2b209b0 7 Motor lw(p26, p29, p30, 1); // pwm, fwd, rev LEFT WHEEL
baijun 14:f413a2b209b0 8 Motor rw(p25, p28, p27, 1); // pwm, fwd, rev RIGHT WHEEL
baijun 15:9bc36f47c8cf 9 MultiModalRobot robot(lw, rw);
kshah521 17:740028fe5c99 10
kshah521 17:740028fe5c99 11 Serial blue(p13, p14);
Hemank101 18:1b0196cf779f 12 Serial py(USBTX, USBRX);
kshah521 17:740028fe5c99 13 char y;
kshah521 17:740028fe5c99 14
kshah521 17:740028fe5c99 15 bool visible = false;
kshah521 17:740028fe5c99 16 Thread t1;
kshah521 17:740028fe5c99 17
baijun 19:91bfed0e5929 18 Mutex ymut;
baijun 19:91bfed0e5929 19 int ypos;
baijun 19:91bfed0e5929 20
kshah521 17:740028fe5c99 21 DigitalOut myled1(LED1);
kshah521 17:740028fe5c99 22 DigitalOut myled2(LED2);
kshah521 17:740028fe5c99 23 DigitalOut myled3(LED3);
Hemank101 18:1b0196cf779f 24 DigitalOut myled4(LED4);
kshah521 17:740028fe5c99 25
kshah521 17:740028fe5c99 26 void thread_read() { // Read from Serial Port Thread
kshah521 17:740028fe5c99 27 char buffer[4];
kshah521 17:740028fe5c99 28 while (true) {
kshah521 17:740028fe5c99 29 if (!py.readable()) { // nothing read - serial empty
baijun 19:91bfed0e5929 30 // myled4 = 1;
kshah521 17:740028fe5c99 31 visible = false;
kshah521 17:740028fe5c99 32 } else { // found something
baijun 19:91bfed0e5929 33 // myled4 = 0;
kshah521 17:740028fe5c99 34 y = py.getc();
Hemank101 18:1b0196cf779f 35 if (y == 'c') {
kshah521 17:740028fe5c99 36 visible = true;
baijun 19:91bfed0e5929 37 // myled3 = !myled3;
baijun 19:91bfed0e5929 38 for (int count = 0; count < 4; count++) {
baijun 19:91bfed0e5929 39 y = py.getc();
baijun 19:91bfed0e5929 40 buffer[count] = y;
baijun 19:91bfed0e5929 41 count++;
baijun 19:91bfed0e5929 42 if (count % 3 == 0) {
kshah521 17:740028fe5c99 43 // myled1 = !myled1;
baijun 19:91bfed0e5929 44
baijun 19:91bfed0e5929 45 //lock
baijun 19:91bfed0e5929 46 ymut.lock();
baijun 19:91bfed0e5929 47 //char snum[5];
baijun 19:91bfed0e5929 48 memcpy(&ypos, buffer, 4);
baijun 19:91bfed0e5929 49 //sprintf(snum, "%d", ypos);
baijun 19:91bfed0e5929 50 //py.printf(snum);
baijun 19:91bfed0e5929 51 ymut.unlock();
baijun 19:91bfed0e5929 52 //unlock
baijun 19:91bfed0e5929 53 // if (i == 84) {
kshah521 17:740028fe5c99 54 // myled2 = 1;
kshah521 17:740028fe5c99 55 // }
baijun 19:91bfed0e5929 56 }
baijun 19:91bfed0e5929 57 }
kshah521 17:740028fe5c99 58 }
baijun 19:91bfed0e5929 59 wait(.5);
kshah521 17:740028fe5c99 60 }
kshah521 17:740028fe5c99 61 }
kshah521 17:740028fe5c99 62 }
kshah521 17:740028fe5c99 63
emilmont 1:491820ee784d 64 int main() {
baijun 16:47d3b1f2e90d 65 char bnum = 0;
baijun 16:47d3b1f2e90d 66 char bhit = 0;
baijun 16:47d3b1f2e90d 67 char needToStopRobot = 0;
baijun 16:47d3b1f2e90d 68 float leftSpeed = 0;
baijun 16:47d3b1f2e90d 69 float rightSpeed = 0;
baijun 19:91bfed0e5929 70 float DEFAULT_SPEED = 0.25;
baijun 19:91bfed0e5929 71 float ROTATE_SPEED = 0.25;
baijun 19:91bfed0e5929 72 t1.start(thread_read);
baijun 19:91bfed0e5929 73
kshah521 17:740028fe5c99 74 while(1) {
baijun 16:47d3b1f2e90d 75 wait(0.1);
kshah521 17:740028fe5c99 76 if(blue.readable() && blue.getc() == '!') {
kshah521 17:740028fe5c99 77 if(blue.readable() && blue.getc() == 'B') {
baijun 16:47d3b1f2e90d 78 bnum = blue.getc();
baijun 16:47d3b1f2e90d 79 bhit = blue.getc();
baijun 16:47d3b1f2e90d 80 blue.getc();
kshah521 17:740028fe5c99 81 switch(bnum) {
baijun 16:47d3b1f2e90d 82 case '1':
baijun 16:47d3b1f2e90d 83 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 84 break;
kshah521 17:740028fe5c99 85 case '2': // become follower
kshah521 17:740028fe5c99 86 while (1) {
kshah521 17:740028fe5c99 87 if (visible) { // CAMERA CAN SEE A CIRCLE
baijun 19:91bfed0e5929 88 // myled1 = 1;
baijun 19:91bfed0e5929 89 // myled2 = 0;
baijun 19:91bfed0e5929 90 ymut.lock();
baijun 19:91bfed0e5929 91 if(ypos<159 && ypos>80) {
baijun 19:91bfed0e5929 92 leftSpeed = DEFAULT_SPEED;
baijun 19:91bfed0e5929 93 rightSpeed = DEFAULT_SPEED;
baijun 19:91bfed0e5929 94 robot.driveWheels(leftSpeed, rightSpeed);
baijun 19:91bfed0e5929 95 }
baijun 19:91bfed0e5929 96 else if(ypos>180){
baijun 19:91bfed0e5929 97 myled4 = 1;
baijun 19:91bfed0e5929 98 leftSpeed = ROTATE_SPEED;
baijun 19:91bfed0e5929 99 rightSpeed = -ROTATE_SPEED;
baijun 19:91bfed0e5929 100 robot.driveWheels(leftSpeed, rightSpeed);
baijun 19:91bfed0e5929 101 myled4 = 0;
baijun 19:91bfed0e5929 102 }
baijun 19:91bfed0e5929 103 else if(ypos<60){
baijun 19:91bfed0e5929 104 myled2 = 1;
baijun 19:91bfed0e5929 105 leftSpeed = -ROTATE_SPEED;
baijun 19:91bfed0e5929 106 rightSpeed = ROTATE_SPEED;
baijun 19:91bfed0e5929 107 robot.driveWheels(leftSpeed, rightSpeed);
baijun 19:91bfed0e5929 108 myled2 = 0;
baijun 19:91bfed0e5929 109 }
baijun 19:91bfed0e5929 110
baijun 19:91bfed0e5929 111 ymut.unlock();
baijun 19:91bfed0e5929 112
kshah521 17:740028fe5c99 113 } else { // CIRCLE NOT VISIBLE
baijun 19:91bfed0e5929 114 // myled1 = 0;
baijun 19:91bfed0e5929 115 // myled2 = 1;
kshah521 17:740028fe5c99 116 leftSpeed = ROTATE_SPEED;
kshah521 17:740028fe5c99 117 rightSpeed = -ROTATE_SPEED;
kshah521 17:740028fe5c99 118 robot.driveWheels(leftSpeed, rightSpeed);
Hemank101 18:1b0196cf779f 119 wait(0.1);
kshah521 17:740028fe5c99 120 }
baijun 19:91bfed0e5929 121 wait(1);
kshah521 17:740028fe5c99 122 }
kshah521 17:740028fe5c99 123 // stop thread
baijun 16:47d3b1f2e90d 124 case '5': //up
baijun 16:47d3b1f2e90d 125 if(bhit=='1'){
baijun 16:47d3b1f2e90d 126 leftSpeed = rightSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 127 } else {
baijun 16:47d3b1f2e90d 128 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 129 }
kshah521 17:740028fe5c99 130 break;
baijun 16:47d3b1f2e90d 131 case '6': //down
kshah521 17:740028fe5c99 132 if(bhit=='1') {
baijun 16:47d3b1f2e90d 133 leftSpeed = rightSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 134 } else {
baijun 16:47d3b1f2e90d 135 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 136 }
baijun 16:47d3b1f2e90d 137 break;
baijun 16:47d3b1f2e90d 138 case '7': //left
kshah521 17:740028fe5c99 139 if(bhit=='1') {
baijun 16:47d3b1f2e90d 140 leftSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 141 rightSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 142 } else {
baijun 16:47d3b1f2e90d 143 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 144 }
baijun 16:47d3b1f2e90d 145 break;
baijun 16:47d3b1f2e90d 146 case '8': //right
kshah521 17:740028fe5c99 147 if(bhit=='1') {
baijun 16:47d3b1f2e90d 148 leftSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 149 rightSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 150 } else {
baijun 16:47d3b1f2e90d 151 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 152 }
baijun 16:47d3b1f2e90d 153 break;
baijun 16:47d3b1f2e90d 154 default:
baijun 16:47d3b1f2e90d 155 robot.stop(0.5);
baijun 16:47d3b1f2e90d 156 break;
baijun 16:47d3b1f2e90d 157 }
kshah521 17:740028fe5c99 158 }
baijun 16:47d3b1f2e90d 159 }
baijun 16:47d3b1f2e90d 160 if(needToStopRobot){
baijun 16:47d3b1f2e90d 161 robot.stop(0.5);
baijun 16:47d3b1f2e90d 162 needToStopRobot = 0;
baijun 16:47d3b1f2e90d 163 leftSpeed = 0;
baijun 16:47d3b1f2e90d 164 rightSpeed = 0;
kshah521 17:740028fe5c99 165 robot.driveWheels(leftSpeed, rightSpeed);
baijun 16:47d3b1f2e90d 166 } else {
kshah521 17:740028fe5c99 167 robot.driveWheels(leftSpeed, rightSpeed);
baijun 16:47d3b1f2e90d 168 }
emilmont 1:491820ee784d 169 }
kshah521 17:740028fe5c99 170 }