Baijun Desai / Mbed 2 deprecated MultiModalRobotSM

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Motordriver mbed-rtos mbed

Fork of MultiModalRobotSM by Hemanth Koralla

main.cpp@17:740028fe5c99, 2017-12-04 (annotated)

Committer:
kshah521
Date:
Mon Dec 04 16:17:53 2017 +0000
Revision:
17:740028fe5c99
Parent:
16:47d3b1f2e90d
Child:
18:1b0196cf779f
State machine 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 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);
kshah521 17:740028fe5c99 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
kshah521 17:740028fe5c99 18 DigitalOut myled1(LED1);
kshah521 17:740028fe5c99 19 DigitalOut myled2(LED2);
kshah521 17:740028fe5c99 20 DigitalOut myled3(LED3);
kshah521 17:740028fe5c99 21
kshah521 17:740028fe5c99 22 void thread_read() { // Read from Serial Port Thread
kshah521 17:740028fe5c99 23 myled1 = 0;
kshah521 17:740028fe5c99 24 char buffer[4];
kshah521 17:740028fe5c99 25 while (true) {
kshah521 17:740028fe5c99 26 if (!py.readable()) { // nothing read - serial empty
kshah521 17:740028fe5c99 27 visible = false;
kshah521 17:740028fe5c99 28 } else { // found something
kshah521 17:740028fe5c99 29 y = py.getc();
kshah521 17:740028fe5c99 30 if (y != 'c') {
kshah521 17:740028fe5c99 31 continue;
kshah521 17:740028fe5c99 32 } else {
kshah521 17:740028fe5c99 33 visible = true;
kshah521 17:740028fe5c99 34 myled3 = !myled3;
kshah521 17:740028fe5c99 35 //for (int count = 0; count < 4; count++) {
kshah521 17:740028fe5c99 36 // y = py.getc();
kshah521 17:740028fe5c99 37 // buffer[count%4] = y;
kshah521 17:740028fe5c99 38 // count++;
kshah521 17:740028fe5c99 39 // if (count % 3 == 0) {
kshah521 17:740028fe5c99 40 // myled1 = !myled1;
kshah521 17:740028fe5c99 41 // int i;
kshah521 17:740028fe5c99 42 // memcpy(&i, buffer, 4);
kshah521 17:740028fe5c99 43 // if (i == 84) {
kshah521 17:740028fe5c99 44 // myled2 = 1;
kshah521 17:740028fe5c99 45 // }
kshah521 17:740028fe5c99 46 // }
kshah521 17:740028fe5c99 47 // }
kshah521 17:740028fe5c99 48 }
kshah521 17:740028fe5c99 49 wait(.01);
kshah521 17:740028fe5c99 50 }
kshah521 17:740028fe5c99 51 }
kshah521 17:740028fe5c99 52 }
kshah521 17:740028fe5c99 53
emilmont 1:491820ee784d 54 int main() {
baijun 16:47d3b1f2e90d 55 char bnum = 0;
baijun 16:47d3b1f2e90d 56 char bhit = 0;
baijun 16:47d3b1f2e90d 57 char needToStopRobot = 0;
baijun 16:47d3b1f2e90d 58 float leftSpeed = 0;
baijun 16:47d3b1f2e90d 59 float rightSpeed = 0;
baijun 16:47d3b1f2e90d 60 float DEFAULT_SPEED = 0.75;
kshah521 17:740028fe5c99 61 float ROTATE_SPEED = 0.50;
kshah521 17:740028fe5c99 62 while(1) {
baijun 16:47d3b1f2e90d 63 wait(0.1);
kshah521 17:740028fe5c99 64 if(blue.readable() && blue.getc() == '!') {
kshah521 17:740028fe5c99 65 if(blue.readable() && blue.getc() == 'B') {
baijun 16:47d3b1f2e90d 66 bnum = blue.getc();
baijun 16:47d3b1f2e90d 67 bhit = blue.getc();
baijun 16:47d3b1f2e90d 68 blue.getc();
kshah521 17:740028fe5c99 69 switch(bnum) {
baijun 16:47d3b1f2e90d 70 case '1':
baijun 16:47d3b1f2e90d 71 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 72 break;
kshah521 17:740028fe5c99 73 case '2': // become follower
kshah521 17:740028fe5c99 74 t1.start(thread_read);
kshah521 17:740028fe5c99 75 while (1) {
kshah521 17:740028fe5c99 76 if (visible) { // CAMERA CAN SEE A CIRCLE
kshah521 17:740028fe5c99 77 leftSpeed = DEFAULT_SPEED;
kshah521 17:740028fe5c99 78 rightSpeed = DEFAULT_SPEED;
kshah521 17:740028fe5c99 79 robot.driveWheels(leftSpeed, rightSpeed);
kshah521 17:740028fe5c99 80 } else { // CIRCLE NOT VISIBLE
kshah521 17:740028fe5c99 81 leftSpeed = ROTATE_SPEED;
kshah521 17:740028fe5c99 82 rightSpeed = -ROTATE_SPEED;
kshah521 17:740028fe5c99 83 robot.driveWheels(leftSpeed, rightSpeed);
kshah521 17:740028fe5c99 84 }
kshah521 17:740028fe5c99 85 }
kshah521 17:740028fe5c99 86 // stop thread
baijun 16:47d3b1f2e90d 87 case '5': //up
baijun 16:47d3b1f2e90d 88 if(bhit=='1'){
baijun 16:47d3b1f2e90d 89 leftSpeed = rightSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 90 } else {
baijun 16:47d3b1f2e90d 91 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 92 }
kshah521 17:740028fe5c99 93 break;
baijun 16:47d3b1f2e90d 94 case '6': //down
kshah521 17:740028fe5c99 95 if(bhit=='1') {
baijun 16:47d3b1f2e90d 96 leftSpeed = rightSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 97 } else {
baijun 16:47d3b1f2e90d 98 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 99 }
baijun 16:47d3b1f2e90d 100 break;
baijun 16:47d3b1f2e90d 101 case '7': //left
kshah521 17:740028fe5c99 102 if(bhit=='1') {
baijun 16:47d3b1f2e90d 103 leftSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 104 rightSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 105 } else {
baijun 16:47d3b1f2e90d 106 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 107 }
baijun 16:47d3b1f2e90d 108 break;
baijun 16:47d3b1f2e90d 109 case '8': //right
kshah521 17:740028fe5c99 110 if(bhit=='1') {
baijun 16:47d3b1f2e90d 111 leftSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 112 rightSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 113 } else {
baijun 16:47d3b1f2e90d 114 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 115 }
baijun 16:47d3b1f2e90d 116 break;
baijun 16:47d3b1f2e90d 117 default:
baijun 16:47d3b1f2e90d 118 robot.stop(0.5);
baijun 16:47d3b1f2e90d 119 break;
baijun 16:47d3b1f2e90d 120 }
kshah521 17:740028fe5c99 121 }
baijun 16:47d3b1f2e90d 122 }
baijun 16:47d3b1f2e90d 123 if(needToStopRobot){
baijun 16:47d3b1f2e90d 124 robot.stop(0.5);
baijun 16:47d3b1f2e90d 125 needToStopRobot = 0;
baijun 16:47d3b1f2e90d 126 leftSpeed = 0;
baijun 16:47d3b1f2e90d 127 rightSpeed = 0;
kshah521 17:740028fe5c99 128 robot.driveWheels(leftSpeed, rightSpeed);
baijun 16:47d3b1f2e90d 129 } else {
kshah521 17:740028fe5c99 130 robot.driveWheels(leftSpeed, rightSpeed);
baijun 16:47d3b1f2e90d 131 }
emilmont 1:491820ee784d 132 }
kshah521 17:740028fe5c99 133 }