#define FRONT_MOTOR 1 #define BACK_MOTOR 3 #define ROT_MOTOR 0 #define FAN_MOTOR 2 #define LEFT_DELTA 10 #define RIGHT_DELTA 5 #use explego.icb /* * Absolute value helper function */ int abs(int num) { if (num < 0) return -1 * num; else return num; } /* * Move will move the robot for time steps at speed speed. * If speed is positive, the robot will go forward. If it * is negative, the robot will go backward. */ void move(int time, int speed) { int i = 0; for (i = 0; i <= time; i++) { motor(FRONT_MOTOR, speed); motor(BACK_MOTOR, speed); } } /* * Turn turns the robot for time steps at rotational speed speed. * If speed is positive, the robot will turn left. If it is * negative, the robot will turn right. While the robot is * rotating its wheels, it will not move forward or backward. */ void turn(int time, int speed) { int i = 0; for (i = 0; i <= time; i++) { motor(ROT_MOTOR, speed); motor(FRONT_MOTOR, 0); motor(BACK_MOTOR, 0); } motor(ROT_MOTOR, 0); // Stop turning when done } /* * Stop simply stops all motors from running. */ void stop() { motor(FRONT_MOTOR, 0); motor(BACK_MOTOR, 0); motor(ROT_MOTOR, 0); motor(FAN_MOTOR, 0); } /* * Blow out turns on the fan and moves forward until the light disappears. * It returns 1 when completed successfully, 0 otherwise. */ int blowOut(int leftLightBase, int rightLightBase) { int leftBase = analog(16); int rightBase = analog(17); int left = leftBase; int right = rightBase; stop(); motor(FAN_MOTOR, 100); sleep(7.0); while ((analog(16) < (leftLightBase - LEFT_DELTA)) && (analog(17) < (rightLightBase - RIGHT_DELTA))) { move(100, 35); stop(); motor(FAN_MOTOR, 100); sleep(7.0); } if ((analog(16) >= leftBase) && (analog(17) >= rightBase)) { stop(); return 1; } stop(); return 0; } /* * Obstacle avoid implements routines for navigating around obstacles. * If the robot detects an obstacle on the left, it will turn right. * If it detects an obstacle on the right, it will turn left. If * it detects an obstacle on both sides, it will turn right. * * The avoidance involves backing up, making the turn, and straightening * out. */ void obstacleAvoid(int left, int right, int front) { int i; if (front == 1) // Obstacle in front { printf("BOTH BUMPED\n"); move(260, -50); // Back up a lot turn(20, -15); // Turn dramatically to the right move(260, 50); // Go forward a while turn(20, 15); // Straighten the wheels } else if (left == 1) // obstacleLeft { printf("LEFT BUMPED\n"); move(200, -50); // Back up turn(15, -15); // Rotate wheels move(200, 50); // Go forward turn(15, 15); // Straighten the wheels } else // obstacleRight { printf("RIGHT BUMPED\n"); move(200, -50); // Back up turn(15, 15); // Rotate wheels move(200, 50); // Go forward turn(15, -15); // Straighten the wheels } } /* * Recenter fixes the orientation of the robot's wheels. * In some cases, when going over obstacles, the wheels get * pushed to one side or the other by the obstacles. This * routine uses a rotation sensor to re-orient the wheels to the * original straight-ahead orientation (approximately). */ void recenter() { int i = 0; int count = 0; int rotation = lego3_counts; printf("RECENTERING\n"); while (rotation != 0) { rotation = lego3_counts; for (i = 0; i < 100; i++) printf("Rot Center = %d\n", rotation); if (count > 500) // If we are stuck (wheels can't move) break; // stop trying if (rotation > 0) { motor(ROT_MOTOR, -10); } else motor(ROT_MOTOR, 10); count++; } motor(ROT_MOTOR, 0); // Stop turning wheels } int main() { int leftLight; // Value of the left light sensor int rightLight; // Value of the right light sensor int obstacleLeft; // 1 if obstacle on left int obstacleRight; // 1 if obstacle on right int obstacleFront; // 1 if obstacle in front int leftLightBase; // Left sensor's base reading int rightLightBase; // Right sensor's base reading int rotation = 0; // Rotation sensor reading int i = 0; // Loop counter int sensed = 0; // 1 if we have sensed the candle /* * The timesteps variables are used to make sure that our sensor * readings are not a fluke before turning. */ int numTimestepsBoth = 0; int numTimestepsLeft = 0; int numTimestepsRight = 0; int result; // Result of blowOut function lego3_counts = 0; // Initialize current rotation to zero rotation = lego3_counts; // Load current rotation leftLightBase = analog(16); // Load base light reading on left rightLightBase = analog(17); // Load base light reading on right /* * Main program loop. Here we first recenter our wheels, then avoid * obstacles. * If we are done with that, we check the light sensors and attempt to * orient the * robot towards the candle. When the robot is aimed straight at the * candle, it will * stop. */ while(1) { /* * Load all variables from their ports. */ printf("NO SENSE\n"); sensed = 0; rotation = lego3_counts; leftLight = analog(16); rightLight = analog(17); obstacleLeft = digital(15); obstacleRight = digital(14); obstacleFront = digital(13); /* * If we sense an obstacle on either side, do obstacle avoidance. */ while (obstacleLeft == 1 || obstacleRight == 1 || obstacleFront == 1) { obstacleAvoid(obstacleLeft, obstacleRight, obstacleFront); obstacleLeft = digital(15); obstacleRight = digital(14); obstacleFront = digital(13); } /* * If we sense a light on the left and the right, and this is not * a fluke, we stop. */ if (((leftLightBase - leftLight) >= LEFT_DELTA) && ((rightLightBase - rightLight) >= RIGHT_DELTA)) { numTimestepsLeft = 0; // Reset the timesteps for left and right numTimestepsRight = 0; numTimestepsBoth++; /* * If we have been sensing "both" for eight timesteps, we * assume it is a correct reading. */ if (numTimestepsBoth >= 8) { printf("BOTH SENSED\n"); sensed = 1; stop(); result = blowOut(leftLightBase, rightLightBase); if (result == 1) { move(200, -50); turn(15, 10); move(200, 50); turn(15, -10); stop(); } else move(100, -40); } } /* * If we sense a light on the left, but not on the right, and this * is not a fluke, * we turn to the left. */ else if (leftLightBase - leftLight >= LEFT_DELTA) { numTimestepsRight = 0; // Reset the timesteps for right and both numTimestepsBoth = 0; numTimestepsLeft++; /* * If we have been sensing left for eight timesteps, it is * not a fluke. */ if (numTimestepsLeft >= 8) { printf("LEFT SENSED!\n"); sensed = 1; move(200, -40); // Back up turn(15, 15); // Turn left move(190, 40); // Go forward turn(15, -15); // Straighten out } } /* * If we sense a light on the right, but not on the left, and * this is not a fluke, * we turn to the right. */ else if (rightLightBase - rightLight >= RIGHT_DELTA) { numTimestepsLeft = 0; // Reset the timesteps for left and both numTimestepsBoth = 0; numTimestepsRight++; /* * If we have been sensing right for eight timesteps, it * is not a fluke. */ if (numTimestepsRight >= 8) { printf("RIGHT SENSED!\n"); sensed = 1; move(200, -40); // Back up turn(15, -15); // Turn right move(190, 40); // Go forward turn(15, 15); // Straighten out } } /* * If we are not sensing anything (or all sensor readings are * flukes) go forward. */ if (sensed == 0) { move(1, 50); } } }