SHOW:
|
|
- or go back to the newest paste.
| 1 | #include "mbed.h" | |
| 2 | #include "physcom.h" | |
| 3 | ||
| 4 | #define SPEED 0.1 | |
| 5 | #define THRESHHOLD 400 | |
| 6 | #define DEBUG | |
| 7 | ||
| 8 | using namespace physcom; | |
| 9 | ||
| 10 | #ifdef DEBUG | |
| 11 | Serial pc(USBTX, USBRX); | |
| 12 | #endif | |
| 13 | ||
| 14 | M3pi robot; | |
| 15 | ||
| 16 | int main() {
| |
| 17 | ||
| 18 | robot.sensor_auto_calibrate(); | |
| 19 | ||
| 20 | wait(2); | |
| 21 | ||
| 22 | while (1) {
| |
| 23 | int sensors[5]; | |
| 24 | robot.calibrated_sensors(sensors); | |
| 25 | ||
| 26 | #ifdef DEBUG | |
| 27 | pc.printf("sensor values: %d; %d; %d; %d; %d;\r\n", sensors[0], sensors[1],sensors[2], sensors[3], sensors[4]);
| |
| 28 | #endif | |
| 29 | ||
| 30 | - | if (sensors[1] >= THRESHHOLD && sensors[2] >=THRESHHOLD && sensors[3] >= THRESHHOLD) break; //We arrived at a T-junction. |
| 30 | + | if (sensors[0] >= THRESHHOLD && sensors[4] >= THRESHHOLD) break; //We arrived at a T-junction. |
| 31 | ||
| 32 | //We are going off the line. Adjust accordingly. | |
| 33 | ||
| 34 | if (sensors[3] >= THRESHHOLD || sensors[4] >= THRESHHOLD) {
| |
| 35 | //The line is to the right. Drive to the right by speeding up the left motor. | |
| 36 | ||
| 37 | if (sensors[4] >= THRESHHOLD) {
| |
| 38 | //More radical turn since were probably on a corner. | |
| 39 | ||
| 40 | robot.activate_motor(0, SPEED); | |
| 41 | robot.activate_motor(1, 0); | |
| 42 | } else {
| |
| 43 | robot.activate_motor(0, SPEED*1.5); | |
| 44 | robot.activate_motor(1, SPEED); | |
| 45 | } | |
| 46 | } else if (sensors[1] >= THRESHHOLD || sensors[0] >= THRESHHOLD) {
| |
| 47 | //The line is to the left. Drive to the left by speding up the right motor. | |
| 48 | ||
| 49 | if (sensors[0] >= THRESHHOLD) {
| |
| 50 | //More radical turn since were probably on a corner. | |
| 51 | ||
| 52 | robot.activate_motor(0, 0); | |
| 53 | robot.activate_motor(1, SPEED); | |
| 54 | } else {
| |
| 55 | robot.activate_motor(0, SPEED); | |
| 56 | robot.activate_motor(1, SPEED*1.5); | |
| 57 | } | |
| 58 | } else {
| |
| 59 | //We are on a straight line. Drive forward. | |
| 60 | ||
| 61 | robot.activate_motor(0, SPEED); | |
| 62 | robot.activate_motor(1, SPEED); | |
| 63 | } | |
| 64 | } | |
| 65 | ||
| 66 | //Stop driving. | |
| 67 | ||
| 68 | robot.activate_motor(0, 0); | |
| 69 | robot.activate_motor(1, 0); | |
| 70 | } |
