Multimodal robot implementing: Manual control Hide and Seek
Dependencies: Motordriver mbed-rtos mbed
Fork of MultiModalRobotSM by
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?
User | Revision | Line number | New 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 | } |