Multimodal robot implementing: Manual control Hide and Seek
Dependencies: Motordriver mbed-rtos mbed
Fork of MultiModalRobotSM by
main.cpp
- Committer:
- kshah521
- Date:
- 2017-12-04
- Revision:
- 17:740028fe5c99
- Parent:
- 16:47d3b1f2e90d
- Child:
- 18:1b0196cf779f
File content as of revision 17:740028fe5c99:
//This will rotate the wheels so that after 5 seconds the robot counterclockwise and then clockwise #include "mbed.h" #include "rtos.h" #include "motordriver.h" #include "MultiModalRobot.h" Motor lw(p26, p29, p30, 1); // pwm, fwd, rev LEFT WHEEL Motor rw(p25, p28, p27, 1); // pwm, fwd, rev RIGHT WHEEL MultiModalRobot robot(lw, rw); Serial blue(p13, p14); Serial py(USBTX, USBRX); char y; bool visible = false; Thread t1; DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); void thread_read() { // Read from Serial Port Thread myled1 = 0; char buffer[4]; while (true) { if (!py.readable()) { // nothing read - serial empty visible = false; } else { // found something y = py.getc(); if (y != 'c') { continue; } else { visible = true; myled3 = !myled3; //for (int count = 0; count < 4; count++) { // y = py.getc(); // buffer[count%4] = y; // count++; // if (count % 3 == 0) { // myled1 = !myled1; // int i; // memcpy(&i, buffer, 4); // if (i == 84) { // myled2 = 1; // } // } // } } wait(.01); } } } int main() { char bnum = 0; char bhit = 0; char needToStopRobot = 0; float leftSpeed = 0; float rightSpeed = 0; float DEFAULT_SPEED = 0.75; float ROTATE_SPEED = 0.50; while(1) { wait(0.1); if(blue.readable() && blue.getc() == '!') { if(blue.readable() && blue.getc() == 'B') { bnum = blue.getc(); bhit = blue.getc(); blue.getc(); switch(bnum) { case '1': needToStopRobot = 1; break; case '2': // become follower t1.start(thread_read); while (1) { if (visible) { // CAMERA CAN SEE A CIRCLE leftSpeed = DEFAULT_SPEED; rightSpeed = DEFAULT_SPEED; robot.driveWheels(leftSpeed, rightSpeed); } else { // CIRCLE NOT VISIBLE leftSpeed = ROTATE_SPEED; rightSpeed = -ROTATE_SPEED; robot.driveWheels(leftSpeed, rightSpeed); } } // stop thread case '5': //up if(bhit=='1'){ leftSpeed = rightSpeed = DEFAULT_SPEED; } else { needToStopRobot = 1; } break; case '6': //down if(bhit=='1') { leftSpeed = rightSpeed = -DEFAULT_SPEED; } else { needToStopRobot = 1; } break; case '7': //left if(bhit=='1') { leftSpeed = -DEFAULT_SPEED; rightSpeed = DEFAULT_SPEED; } else { needToStopRobot = 1; } break; case '8': //right if(bhit=='1') { leftSpeed = DEFAULT_SPEED; rightSpeed = -DEFAULT_SPEED; } else { needToStopRobot = 1; } break; default: robot.stop(0.5); break; } } } if(needToStopRobot){ robot.stop(0.5); needToStopRobot = 0; leftSpeed = 0; rightSpeed = 0; robot.driveWheels(leftSpeed, rightSpeed); } else { robot.driveWheels(leftSpeed, rightSpeed); } } }