FLL 2013 All Missions
#pragma config(Sensor, S1, touchSensor, sensorTouch)
#pragma config(Sensor, S3, lightSensorA, sensorLightActive)
#pragma config(Sensor, S4, touchSensorB, sensorTouch)
#pragma config(Sensor, S2, sonarSensor, sensorSONAR)
//#pragma config(Sensor, S2, lightSensorB, sensorLightActive)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "Library2013.c";
/*------
Date: 10/5/13
Description: All Missions
-------*/
void calibrate() { wait(1); calibrateLightSensor(); }
void ninetyDegreeTest() {
driveForward(24);
wait(40);
pointTurnClockwise(90);
wait(5);
pointTurnCounterClockwise(90);
wait(5);
}
///////////////////////////////////////////////////////////////////////////////
// Release the plane
//
void baseToCargoPlane() {
setPowerLevel(50);
driveForward(2); // Start Moving a little
backIntoWall(40); // Back into South Wall
driveForward(1.5); // Move away from the South wall
pointTurnClockwise(90); // Turn to face the East
backIntoWall(40); // Back into West Wall and face the plane
accessoryDown(); // Reset the arm
accessoryUp(60); // Get the Arm ready for the plane launcher
driveForward(); // Move a little away from the wall
untilDistance(6);
setPowerLevel(30);
driveForward();
untilLine(40);
//stopOnLine(40);
setPowerLevel(50);
driveForward(10); // Move toward the plane launcher
}
void releaseCargoPlane() {
setPowerLevel(30);
int angleTowardRedLever = 46; // Let's initialize a variable to store the degrees to turn toward the red lever
pointTurnClockwise(angleTowardRedLever); // Turn toward the Red lever
driveForward(4); // Move toward the Red lever
accessoryDown(); // Depress the lever
accessoryUp(30); // Get the arm off of the lever
pointTurnCounterClockwise(angleTowardRedLever); // Face the East wall again
driveForward(5);
pointTurnClockwise(40); // Face the Water Bottle
accessoryDown(); // Get arm ready for pick up
gradualTurnCounterClockwise(10,6); // Turn toward the water
driveForward(1); // Spear the water bottle
accessoryUp(30); // Pick up the Water
//
// Go from the plane to the Green Man
//
setPowerLevel(30);
driveBackward(2); // Now that you have the water, backup and turn a little
pointTurnCounterClockwise(120);
backIntoWall();
driveForward();untilDistance(11);// driveForward(11);
swingTurnClockwise(45);
driveForward();untilLine(); // The line by the house
driveForward();untilDistance(3);// Drive a little bit beyond the line
pointTurnCounterClockwise(42); // Aim toward the Green Man
driveBackward(0.5);
}
void baseToGreenMan() {
}
void getGreenManAndHouse() {
accessoryDown(); // Put arm down to Get the Green person by the house
driveForward(5);
accessoryUp(60);
driveBackward(1);
pointTurnClockwise(20); // Turn a little away from the house
driveForward(1); // Move to the East of the house
pointTurnCounterClockwise(5); // Turn toward the North wall
driveForward(2); // Move to the East of the house
pointTurnCounterClockwise(20); // Turn toward the North wall
//
// Get Ready to Lift the House
//
setPowerLevel(20); // Slow down when approaching the wall
followLine(RIGHTSIDE);
stopIfSonarLessThan(4);
driveForward();
setPowerLevel(15); // Slow down even more when approaching the wall
driveForward(2.4);
pointTurnClockwise(85);
driveBackward(2);
setPowerLevel(30); // Restore default power
pointTurnClockwise(5);
driveBackward(8);
//
// Now get the Gas Can
//
driveForward();
stopOnLine();
driveForward(8);
pointTurnClockwise(90);
backIntoWall();
driveForward(2); // Move away from North Wall
pointTurnCounterClockwise(87); // Turn toward the Gas Can
accessoryDown(); // Prepare the Arm
}
void baseToGasCan() {
driveForward(2); // Start Moving a little
backIntoWall(40); // Back into South Wall
driveForward(6); // Move away from the South wall
pointTurnClockwise(90); // Turn to face the East
backIntoWall(40); // Back into West Wall and face the plane
accessoryDown(); // Reset the arm
accessoryUp(60); // Get the Arm ready for the plane launcher
driveForward(7); // Move a little away from the wall
stopOnLine(50); // Border of the Base roughly at the north east corner
gradualTurnCounterClockwise(45,17);// Move toward the center of the board
driveForward();
stopOnLine();
driveForward(5); // Drive a little bit beyond the line
pointTurnCounterClockwise(42); // Aim toward the North Wall
followLine(RIGHTSIDE);
untilSonarLessThan(4);
stopMoving();
driveForward(2);
pointTurnClockwise(180);
backIntoWall();
driveForward(1); // Move away from North Wall
pointTurnCounterClockwise(87); // Turn toward the Gas Can
accessoryDown();
//driveForward(3);
}
void getGasCan() { // Robot is starting with back to the North Wall
driveForward(3); // This mission does the Gas Can, Sign, and Battery
accessoryDown();
driveForward(3);
accessoryDown();
driveForward(2);
accessoryUp(60); // Raise the Gas Can
pointTurnClockwise(45);
setPowerLevel(50);
driveForward();untilDistance(10);
accessoryDown(); // Crawl under the string
driveForward();untilDistance(9);
accessoryUp(60);
setPowerLevel(40); // Slow down when approaching the line
driveForward(); // Move toward the line slowly
untilLine(); // Stop
setPowerLevel(30); // Resume default power level
driveBackward();untilDistance(8);// Get ready to raise the sign
pointTurnCounterClockwise(48);
driveForward();
stopIfSonarLessThan(4);
driveForward(6); // Push the sign up
int BatteryDistanceEWShort = 2;
driveBackward(BatteryDistanceEWShort);
pointTurnClockwise(98);
driveForward();stopOnLine();
driveBackward();untilDistance(9);
int angleTowardBattery = 75; // Let's initialize a variable to store the degrees to turn toward the battery
pointTurnCounterClockwise(angleTowardBattery);
accessoryDown();
driveForward(BatteryDistanceEWShort);
accessoryUp(65);
driveBackward();untilDistance(1);
pointTurnClockwise(angleTowardBattery-3);
driveForward();stopOnLine();
pointTurnCounterClockwise(50);
driveForward(); // Drive into the Corner
untilDistance(6);
accessoryDown(); // Drop off your stuff
untilDistance(2);
driveBackward(); // Back out of the corner
stopAfterDistance(14);
driveBackward();
gradualTurnCounterClockwise(45,18);
driveBackward(5);
accessoryUp(100);
driveForward();
untilLine(); // Find the line next to the Earthquake
driveForward(4);
pointTurnCounterClockwise(87); // Turn and Face the Buildings
setPowerLevel(30);
}
void baseToEarthquake() { // Robot needs to be centered over black line facing the North Wall
driveForward(2); // Start Moving a little
backIntoWall(40); // Back into South Wall
driveForward(6); // Move away from the South wall
pointTurnClockwise(90); // Turn to face the East
backIntoWall(40); // Back into West Wall and face the plane
accessoryDown(); // Reset the arm
accessoryUp(100); // Get the Arm ready for the plane launcher
driveForward(7); // Move a little away from the wall
stopOnLine(50); // Border of the Base roughly at the north east corner
gradualTurnCounterClockwise(45,17);// Move toward the center of the board
setPowerLevel(50);
driveForward();
stopOnLine();
driveForward(5); // Drive a little bit beyond the line
gradualTurnClockwise(48,12); // Aim toward the North Wall
driveForward(5);
driveForward();
stopOnLine();
setPowerLevel(30);
driveForward(4);
pointTurnCounterClockwise(87); // Turn and Face the Buildings
driveBackward(4);
}
void getEarthquake() { // Robot is centered over black line facing the North Wall
driveForward(); // This mission does the Earthquake, Tsunami, Ambulance
untilSonarLessThan(4);
driveForward(4, 70);
setPowerLevel(50);
driveBackward(1);
// Now position the robot so that it will get the Flashlight, Cat, and Dog
pointTurnCounterClockwise(87); // Turn toward the West
driveForward(); // Find the line next to the house
stopOnLine();
pointTurnCounterClockwise(90);
backIntoWall();
setPowerLevel(30);
driveForward(1);
pointTurnClockwise(97);
setPowerLevel(50);
driveForward(12);
stopMoving();
pointTurnCounterClockwise(15);
driveForward(); // Find the line next to the house
stopOnLine();
driveBackward(3);
pointTurnCounterClockwise(90);
backIntoWall(); // Align with North wall
setPowerLevel(30); // Move slower
driveForward(2); // Move Away from the wall
pointTurnClockwise(130); // Turn toward the flashlight
accessoryDown(); // Get Arm Ready
driveForward(2, 20); // Spear the Flashlight
accessoryUp(100, 20); // Pick up the Flashlight which will move to the base of the arm
accessoryDown(); // Reposition the arm for the tree branch
accessoryUp(33); // Get it to the right height
driveForward(0.5);
pointTurnCounterClockwise(60,60); // Need to turn a total of 130 degrees CounterClockwise
wait(.5);
accessoryDown(5); // Reposition the arm for the tree branch
pointTurnClockwise(40,60); // Using several short turns to do it
wait(.5);
pointTurnCounterClockwise(110,60);
backIntoWall();
accessoryUp(70);
driveForward(12);
driveBackward(1);
pointTurnCounterClockwise(50);
accessoryDown();
pointTurnClockwise(60,70);
accessoryUp(60);
driveForward(8);
pointTurnCounterClockwise(90);
backIntoWall(); // Align with the West wall
driveForward(3);
stopOnLine();
driveBackward(2);
pointTurnCounterClockwise(89);
accessoryDown();
driveForward(3);
gradualTurnCounterClockwise(12,25);
accessoryUp(90);
driveBackward(10);
pointTurnClockwise(10);
driveBackward();
backIntoWall();
wait(40);
}
task main() {
startup();
setPowerLevel(30);
configureRobotDimensions(95, 32, (float) ((float)24/(float)40), BACKWARD, BACKWARD);
menu[lastMenuItem] = addMenuItem("Calibrate Light", FUNCTION_1);
menu[lastMenuItem] = addMenuItem("90 Degree Test", FUNCTION_2);
menu[lastMenuItem] = addMenuItem("Cargo Plane", FUNCTION_3);
menu[lastMenuItem] = addMenuItem("GreenMan House", FUNCTION_4);
menu[lastMenuItem] = addMenuItem("Get Gas Can", FUNCTION_5);
menu[lastMenuItem] = addMenuItem("EarthQuake", FUNCTION_6);
menu[lastMenuItem] = addMenuItem("Battery", FUNCTION_7);
menu[lastMenuItem] = addMenuItem("Dog Cat Tree", FUNCTION_8);
menu[lastMenuItem] = addMenuItem("Man House", FUNCTION_9);
int selectedItem;
bool proceedFromBase = true;
showMenu();
selectedItem = startMenu(3);
clearDisplay();
display(1, menu_prompts[selectedItem]);
string str = "";
StringFormat(str, "Test: %3d ", menu[selectedItem]);
display(2, str);
switch (menu[selectedItem])
{
case FUNCTION_1: calibrate(); break;
case FUNCTION_2: ninetyDegreeTest(); break;
case FUNCTION_3: if(proceedFromBase) { baseToCargoPlane(); proceedFromBase=false; } releaseCargoPlane();
case FUNCTION_4: if(proceedFromBase) { baseToGreenMan(); proceedFromBase=false; } getGreenManAndHouse();
case FUNCTION_5: if(proceedFromBase) { baseToGasCan(); proceedFromBase=false; } getGasCan();
case FUNCTION_6: if(proceedFromBase) { baseToEarthquake(); proceedFromBase=false; } getEarthquake();
case FUNCTION_7: if(proceedFromBase) { baseToBattery(); proceedFromBase=false; } getBattery();
case FUNCTION_8: getDogCat();
case FUNCTION_9: getFlashlightHouse();
}
return;
}
