Karan Shah / Mbed 2 deprecated MultiModalRobotSM

Dependencies:   Motordriver mbed-rtos mbed

Fork of MultiModalRobot by Baijun Desai

main.cpp@16:47d3b1f2e90d, 2017-11-20 (annotated)

Committer:
baijun
Date:
Mon Nov 20 04:01:33 2017 +0000
Revision:
16:47d3b1f2e90d
Parent:
15:9bc36f47c8cf
Child:
17:740028fe5c99
Basic Manual Control Implemented

Who changed what in which revision?

UserRevisionLine numberNew contents of line
baijun 14:f413a2b209b0 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 14:f413a2b209b0 6
baijun 16:47d3b1f2e90d 7
baijun 16:47d3b1f2e90d 8
baijun 14:f413a2b209b0 9 Motor lw(p26, p29, p30, 1); // pwm, fwd, rev LEFT WHEEL
baijun 14:f413a2b209b0 10 Motor rw(p25, p28, p27, 1); // pwm, fwd, rev RIGHT WHEEL
baijun 15:9bc36f47c8cf 11 MultiModalRobot robot(lw, rw);
emilmont 1:491820ee784d 12
baijun 16:47d3b1f2e90d 13 Serial blue(p13, p14);
baijun 16:47d3b1f2e90d 14
emilmont 1:491820ee784d 15 int main() {
baijun 16:47d3b1f2e90d 16 char bnum = 0;
baijun 16:47d3b1f2e90d 17 char bhit = 0;
baijun 16:47d3b1f2e90d 18 char needToStopRobot = 0;
baijun 16:47d3b1f2e90d 19 float leftSpeed = 0;
baijun 16:47d3b1f2e90d 20 float rightSpeed = 0;
baijun 16:47d3b1f2e90d 21 float DEFAULT_SPEED = 0.75;
baijun 16:47d3b1f2e90d 22 while(1){
baijun 16:47d3b1f2e90d 23 wait(0.1);
baijun 16:47d3b1f2e90d 24 if(blue.readable() && blue.getc() == '!'){
baijun 16:47d3b1f2e90d 25 if(blue.readable() && blue.getc() == 'B'){
baijun 16:47d3b1f2e90d 26 bnum = blue.getc();
baijun 16:47d3b1f2e90d 27 bhit = blue.getc();
baijun 16:47d3b1f2e90d 28 blue.getc();
baijun 16:47d3b1f2e90d 29 switch(bnum){
baijun 16:47d3b1f2e90d 30 case '1':
baijun 16:47d3b1f2e90d 31 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 32 break;
baijun 16:47d3b1f2e90d 33 case '5': //up
baijun 16:47d3b1f2e90d 34 if(bhit=='1'){
baijun 16:47d3b1f2e90d 35 leftSpeed = rightSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 36 } else {
baijun 16:47d3b1f2e90d 37 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 38 }
baijun 16:47d3b1f2e90d 39 break;
baijun 16:47d3b1f2e90d 40 case '6': //down
baijun 16:47d3b1f2e90d 41 if(bhit=='1'){
baijun 16:47d3b1f2e90d 42 leftSpeed = rightSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 43 } else {
baijun 16:47d3b1f2e90d 44 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 45 }
baijun 16:47d3b1f2e90d 46 break;
baijun 16:47d3b1f2e90d 47 case '7': //left
baijun 16:47d3b1f2e90d 48 if(bhit=='1'){
baijun 16:47d3b1f2e90d 49 leftSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 50 rightSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 51 } else {
baijun 16:47d3b1f2e90d 52 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 53 }
baijun 16:47d3b1f2e90d 54 break;
baijun 16:47d3b1f2e90d 55 case '8': //right
baijun 16:47d3b1f2e90d 56 if(bhit=='1'){
baijun 16:47d3b1f2e90d 57 leftSpeed = DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 58 rightSpeed = -DEFAULT_SPEED;
baijun 16:47d3b1f2e90d 59 } else {
baijun 16:47d3b1f2e90d 60 needToStopRobot = 1;
baijun 16:47d3b1f2e90d 61 }
baijun 16:47d3b1f2e90d 62 break;
baijun 16:47d3b1f2e90d 63 default:
baijun 16:47d3b1f2e90d 64 robot.stop(0.5);
baijun 16:47d3b1f2e90d 65 break;
baijun 16:47d3b1f2e90d 66 }
baijun 16:47d3b1f2e90d 67 }
baijun 16:47d3b1f2e90d 68 }
baijun 16:47d3b1f2e90d 69 if(needToStopRobot){
baijun 16:47d3b1f2e90d 70 robot.stop(0.5);
baijun 16:47d3b1f2e90d 71 needToStopRobot = 0;
baijun 16:47d3b1f2e90d 72 leftSpeed = 0;
baijun 16:47d3b1f2e90d 73 rightSpeed = 0;
baijun 16:47d3b1f2e90d 74 } else {
baijun 16:47d3b1f2e90d 75 robot.driveWheels(leftSpeed, rightSpeed);
baijun 16:47d3b1f2e90d 76 }
emilmont 1:491820ee784d 77 }
baijun 13:4c609396ed75 78 }