void moveCar(orientation direction, int distance){
orient(distance);
switch(direction){
case Straight:
driveStraight(distance);
break;
case Right:
turnRight(distance);
driveStraight(distance);
break;
case Left:
turnLeft(distance);
driveStraight(distance);
break;
case Back:
turnRight(distance);
sleep(1);
turnRight(distance);
driveStraight(distance);
break;
default:
motorright.duty = 7.25;
motorleft.duty = 7.25;
break;
}
//bcm2835_close();
return ;
}
void redPost() {
setArm(20);
intakeSet(127);
wait1Msec(750);
setArm(5);
wait1Msec(500);
driveStraight(550);
setArm(120);
wait1Msec(3000);
setLeftTicks(685);
SensorValue[lDriveEncoder] = 0;
SensorValue[rDriveEncoder] = 0;
setRightPo(127);
setLeftPo(127);
wait1Msec(1500);
setRightPo(0);
setLeftPo(0);
SensorValue[lDriveEncoder] = 0;
SensorValue[rDriveEncoder] = 0;
driveStraight(-390);
setArm(95);
wait1Msec(1000);
intakeSet(-127);
SensorValue[dumpSolenoid] = 1;
wait1Msec(1500);
SensorValue[dumpSolenoid] = 0;
SensorValue[lDriveEncoder] = 0;
SensorValue[rDriveEncoder] = 0;
driveStraight(-180);
wait1Msec(500);
setArm(5);
}
task autonomous()
{
SensorType[in8] = sensorNone;
wait1Msec(1000);
//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
SensorType[in8] = sensorGyro;
wait1Msec(2000);
if(SensorValue(limit1) == 0) //hang a cube from the back, move backwards until the limit switch is pressed
{
driveStraight1(-100);
}
else if(SensorValue(limit1) == 1)
{
wait1Msec(1000); //wait for the cube to release from the robot
driveStraight1(100);
}
// gyroTurn(45);//turns 45 degrees
turn(100);
wait1Msec(1000);
turn(0);
driveStraight(sqrt(2), 50);//returns to starting position
driveStraight(1.5, 50);
//do something with intake
if(SensorValue(limit1) == 0)
{
driveStraight1(-100);
}
}
task autonomous() {
//start flywheel
initializeTasks();
setFlywheelRange(2);
wait1Msec(1000);
startTask(fire);
//wait until first set of preloads are fired
waitUntilNotFiring(15000);
while (firing) { EndTimeSlice(); }
stopTask(fire);
turn(120); //turn toward stack
motor[feedMe] = 127;
driveStraight(150, 1, 1, 60); //move to second stack
turn(-30);
driveStraight(3200, 1, 1, 60); //drive across field
autonProgress = 1;
turn(-83); // turn toward net
autonProgress = 2;
//fire remaining balls
startTask(fire);
while (true) { EndTimeSlice(); }
}
void driveUntilIR(int irValue, int motorSpeed)//Drives until the IR value equals a preset number.
{
while(readIR() != irValue)
{
driveStraight (motorSpeed, 0);
}
}
void driveUntilIRNot(int irValue, int motorSpeed)
{
while(readIR() == irValue)
{
driveStraight (motorSpeed, 0);
}
}
void setup() {
Serial.begin(9600);
calibrateAll();
leftServo.attach(9, 1300, 1700);
rightServo.attach(10, 1300, 1700);
while(!finishLine){
driveStraight(10, 50);
readColor();
}
turnLeftInDegrees(90);
driveInInches(36);
for(int i = 0; i < 12; i++){
turnRightInDegrees(90);
driveInInches(3);
turnRightInDegrees(90);
driveInInches(72);
turnLeftInDegrees(90);
driveInInches(3);
turnLeftInDegrees(90);
driveInInches(72);
}
}
void goStraightForTime(int time, int speed)//This function allows locomotion over a predefined period (per the parameters) and stop. We create a timer in order to measure the time elapsed between the beginning and end of execution of this function.
{
time1[T1] = 0;//Creating the aforementioned timer
while (time1[T1] < time)//While loop saying that while the value of the timer is less than the value of the parametrically set stop time.
{
driveStraight (speed, 0);//Drivers forward with no angle change (the null value is set in the parameters)
}
}
void goStraightForTime(int time, int speed)
{
time1[T1] = 0;
while (time1[T1] < time)
{
driveStraight (speed, 0);
}
}
void progSkills()
{
FlywheelPower(60);
wait1Msec(500);
startTask(FlywheelController);
startTask(recoverFromShots);
SensorValue[ANGLE] = 0;
SetTarget(2260,44); // used to be 2050
driveAcross(300);
wait1Msec(150);
turnOther(100); // added by shlaok
//wait1Msec(500);
//turn(0);
while(first_cross)
{
wait1Msec(10);
}
startTask(AutonConveyor);/*
Conveyor(127);
//*/
//SensorValue[ANGLE] = 0;
//turnOther(120); // added by shlaok
while(shotCount<32)
{
wait1Msec(10);
}
turn(0);
driveStraight(-127);
wait1Msec(800);
motor[LeftDrive] = motor[RightDrive] = 0;
SensorValue[ANGLE] = 0;
/*stopTask(FlywheelController);
stopTask(recoverFromShots);
SetTarget(0,0);
FlywheelPower(0);*/
driveAcross(4350); // was 4600
//driveStraight(127);
//wait1Msec(300);
motor[LeftDrive] = motor[RightDrive] = 0;
turn(-120); // used to be positive
/*startTask(FlywheelController);
startTask(recoverFromShots);
FlywheelPower(60);
wait1Msec(300);
SetTarget(2000,34);
while(first_cross)
{
wait1Msec(10);
}*/
while(true)
{}
}
task hoardingAuto() { //push balls into our corner
driveStraight(2000); //drive forward
turn(-15); //turn
driveStraight(-1000, 80); //back up to push first stack into start zone
turn(18); //turn toward second stack
setFlywheelRange(1);
motor[feedMe] = 127; //start feed
driveStraight(2300, 750); //pick up second stack
//fire second stack
startTask(fire);
waitUntilNotFiring();
while (firing) { EndTimeSlice(); }
stopTask(fire);
startTask(feedToTop);
turn(-65); //turn toward third stack
//pick up third stack
driveStraight(1100);
}
void square()
{
int i = 0;
for(i; i < 4; i++)
{
driveStraight();
Wait(1400);
rotateRight();
Wait(560);
}
stop();
}
void setup(){
Serial.begin(9600);
calibrateAll();
leftServo.attach(9, 1300, 1700);
rightServo.attach(10, 1300, 1700);
while(!finishLine){
driveStraight(10, 50);
readColor();
}
turnLeftInDegrees(90);
driveInInches(36);
for(int i = 0; i < 12; i++){
turnRightInDegrees(90);
for(int i = 0; i < 30; i++){
if(reading >= yellowMin && reading <= yellowMax){
foundGold = true;
}
if(!foundGold){
driveInInches(0.1);
}
}
turnRightInDegrees(90);
for(int i = 0; i < 720; i++){
if(reading >= yellowMin && reading <= yellowMax){
foundGold = true;
}
if(!foundGold){
driveInInches(0.1);
}
}
turnLeftInDegrees(90);
for(int i = 0; i < 30; i++){
if(reading >= yellowMin && reading <= yellowMax){
foundGold = true;
}
if(!foundGold){
driveInInches(0.1);
}
}
turnLeftInDegrees(90);
for(int i = 0; i < 720; i++){
if(reading >= yellowMin && reading <= yellowMax){
foundGold = true;
}
if(!foundGold){
driveInInches(0.1);
}
}
}
}
task classicAuto() {
setFlywheelRange(3);
motor[feedMe] = 127;
//fire four initial preloads
startTask(fire);
waitUntilNotFiring();
while (firing) { EndTimeSlice(); }
stopTask(fire);
startTask(feedToTop);
setFlywheelRange(2);
turn(21); //turn toward first stack
//pick up first stack
startTask(feedToTop);
driveStraight(900);
turn(-18); //turn toward net
driveStraight(1150); //drive toward net
stopTask(feedToTop);
startTask(fire);
waitUntilNotFiring(3000);
while (firing) { EndTimeSlice(); }
stopTask(fire);
//pick up second stack
startTask(feedToTop);
driveStraight(950); //drive into net for realignment
driveStraight(-750); //move back
//fire second stack
stopTask(feedToTop);
startTask(fire);
waitUntilNotFiring();
while (firing) { EndTimeSlice(); }
stopTask(fire);
turn(-65); //turn toward third stack
//pick up third stack
driveStraight(1100);
}
void goStraightUntilIRNothing(int speed)//This function allows locomotion over a predefined period (per the parameters) and stop. We create a timer in order to measure the time elapsed between the beginning and end of execution of this function.
{
servo[servo2]=127;
int initValue = 0;
int raw = 0;
initValue = LSvalRaw(LightSensor);
raw = initValue;
while(readIR() > 0)//While loop saying that while the value of the light sensor is less than the value of the parametrically set stop value.
{
raw = LSvalRaw(LightSensor);
nxtDisplayCenteredBigTextLine(1, "%d", raw);
driveStraight (speed, 0);//Drivers forward with no light sensor value change (the null value is set in the parameters)
}
}
task skillz() {
//start flywheel
setFlywheelRange(2);
wait1Msec(1000);
startTask(fire);
//wait until first set of preloads are fired
waitUntilNotFiring(12000);
while (firing) { EndTimeSlice(); }
stopTask(fire);
startTask(feedToTop);
turn(108); //turn toward middle stack
motor[feedMe] = 127; //startTask(feedToTop); //start feeding
driveStraight(2300); //drive across field
turn(-15); // turn toward starting tiles
driveStraight(1200); //drive across field
turn(-60); //turn toward net
//fire remaining balls
stopTask(feedToTop);
startTask(fire);
while (true) { EndTimeSlice(); }
}
task autonomous() {
//start flywheel
initializeTasks();
setFlywheelRange(0);
wait1Msec(1000);
motor[seymore] = 127; //start feed
//wait until first set of preloads are fired
waitUntilNotFiring(2000);
while (firing) { EndTimeSlice(); }
motor[seymore] = 0;
turn(93); //turn toward other starting tile
driveStraight(8000, 1, 1, 60); //drive across field
autonProgress = 1;
turn(-95); // turn toward net
autonProgress = 2;
//fire remaining balls
motor[seymore] = 127;
while (true) { EndTimeSlice(); }
}
void goToPoint()
{
update();
printf("VPS Current Point: (%d, %d)\n", game.coords[0].x, game.coords[0].y);
if(currentPt.y > 0){
desiredPt.x = 3.3;
desiredPt.y = 2.5;
}
else{
desiredPt.x = 3.3;
desiredPt.y = -2.6;
}
printf("Current Point: (%f, %f)\n", currentPt.x, currentPt.y);
printf("Desired Point: (%f, %f)\n", desiredPt.x, desiredPt.y);
printf("Our current heading is: %f\n", (float)game.coords[0].theta/2047*180); //NEED TO CONVERT TO DEGREES
float targetHeading = getTargetTheta(desiredPt);
printf("The target heading is: %f\n", targetHeading);
pointTurn(targetHeading);
float dist = distanceTo(desiredPt);
printf("The distance is: %f\n", dist);
while(dist > TOLERANCE)
{
driveStraight(dist);
printf("Trying to drive straight\n");
update();
dist = distanceTo(desiredPt);
}
brake();
//get location data
//determine angle and position relative to desired point
//turn to face desired point
//drive "straight" to point (unless obstacles)
}
task autonomous()
{
writeDebugStream("Autonomous Started...");
driveStraight(2, BOT_ID);
}
void driveInInches(float distance) {
if(distance <= 0){
driveStraight(10, -171.89 * distance);
}
driveStraight(170, 171.89 * distance);
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
task autonomous()
{
driveStraight(2, BOT_ID);
servoOpen(8);
}
task main ()
{
init();
int coor[2];
int x, y;
int dist;
int xA = 34;
int yA = 205;
int xB = 229;
int yB = 147;
int xC = 277;
int yC = 55;
int xHome;
int yHome;
//int xTerrain2 = 220;
bool foundBeacon;
/************* STEP 1 *************/
//Face the right direction
nxtDisplayTextLine(1, "Step 1");
//turnRight (0); // face north
turnRight (-45); // face north east
//turnRight(-90); // face east
//turnRight(-135); // face south east
//turnRight(180); // face south
//turnRight(135) // face south west
//turnRight(90); // face west
//turnRight(45) // face north west
wait1Msec(300);
/////////////////STEP 2 ///////////////
//Drive to y coordinate of A
nxtDisplayTextLine(2, "Step 2");
//Get home coordinates
getPos(coor);
x = xHome = coor[0];
//wait1Msec(1000);
y = yHome = coor[1];
dist = yA - y;
driveStraight(dist - 5); // Move to (A's y coordinate - 5)
wait1Msec(300);
////////////////STEP 3/////////////////
//Turn West
nxtDisplayTextLine(3, "Step 3");
turnRight(-90);
wait1Msec(300);
///////////////STEP 4/////////////////
//Move to x coord of A
nxtDisplayTextLine(4, "Step 4");
//Get current x coordinate
getPos(coor);
x = coor[0];
//Move to the same x coordinate as A - 10cm
dist = xA - x;
driveStraight(-(dist + 10));
wait1Msec(300);
//////////////STEP 5//////////////////
//Drop
// Look for magnetic beacon
nxtDisplayTextLine (5, "Step 5");
foundBeacon = findBeacon();
//If magnetic beacon is found, drop bins off
if (foundBeacon) {
//Drop bins
driveStraight(-5);
wait1Msec(300);
dropBins();
}
// If couldn't find beacon, move back to A and drop bins off
else {
getPos (coor);
dist = xA - coor[0];
driveStraight (-(dist + 5));
wait1Msec (300);
dropBins();
}
wait1Msec(300);
///////////////STEP 6///////////////////
//Move to x coordinate of B
getPos(coor);
dist = xB - coor[0];
driveStraight(-(dist+6));
wait1Msec(300);
//////////////STEP 7///////////////////
//Turn North
turnRight(90);
wait1Msec(300);
/////////////STEP 8////////////////////
//Move to Y coordinate of B
getPos(coor);
y = coor[1];
dist = yB - y; // Move to (B's y coordinate - 5)
driveStraight((dist - 5));
wait1Msec(300);
/////////////STEP 9///////////////////
// Drop bin at B
foundBeacon = findBeacon();
//If magnetic beacon is found, drop bins off
if (foundBeacon) {
//Drop bins
driveStraight(-5); //cm
wait1Msec(300);
dropBins();
}
// If couldn't find beacon, move back to B and drop bins off
else {
getPos (coor);
y = coor[1];
dist = yB - y;
driveStraight ((dist - 5));
wait1Msec (300);
dropBins();
}
wait1Msec(300);
///////////////STEP 10//////////////////////////
//Go to yC //Pass through rough terrain
getPos(coor);
y = coor[1];
dist = yC - y;
driveStraight(dist);
wait1Msec(300);
//////////////STEP 11/////////////////////////
//Turn East
turnRight (90);
wait1Msec(300);
///////////////STEP 12////////////////////////
//Drive to xC - 5cm
getPos (coor);
x = coor[0];
dist = xC - x;
driveStraight(dist - 5);
wait1Msec(300);
////////////////STEP 13/////////////////////
// Look for magnetic beacon
foundBeacon = findBeacon();
//If magnetic beacon is found, drop bins off
if (foundBeacon) {
//Drop bins
driveStraight(-5); //cm
wait1Msec(300);
dropBins();
}
// If couldn't find beacon, move back to B and drop bins off
else {
getPos (coor);
x = coor[0];
dist = xC - x;
driveStraight (dist - 5);
wait1Msec (300);
dropBins();
}
wait1Msec(300);
////////////////////STEP 14/////////////////////
//Go to x coor of home
getPos(coor);
x = coor[0];
dist = xHome - x;
driveStraight(dist);
wait1Msec(300);
///////////////////STEP 15//////////////////////
//Turn north
turnRight(-90);
wait1Msec(300);
///////////////////STEP 16//////////////////////
//Drive to y position
getPos(coor);
y = coor[1];
dist = yHome - y;
driveStraight(dist);
wait1Msec(300);
}
task main()
{
init();
int coor[2];
int x = 20;
int y = 20;
int dist = 0;
int distone = 0;
int disttwo = 0;
int distthree = 0;
turnRight(90);
do {
getPos(coor);
x = coor[0];
//nxtDisplayTextLine(4, "%d", x);
wait1Msec(1000);
y = coor[1];
dist = coor[1] - 15;
//nxtDisplayTextLine(5, "dist = %d", dist);
wait1Msec(1000);
driveStraight(dist);
distone = distone + dist;
} while (coor[1] > 15);
wait1Msec(1000);
turnRight(-90);
wait1Msec(1000);
do {
getPos(coor);
x = coor[0];
y = coor[1];
dist = 135 - x;
driveStraight(dist);
disttwo = disttwo + dist;
} while (coor[0] < 135);
turnRight(-90);
do {
getPos(coor);
x = coor[0];
y = coor[1];
dist = 75 - y;
driveStraight(dist);
distthree = distthree + dist;
} while (y < 75);
playSound(soundBeepBeep);
playSound(soundBeepBeep);
playSound(soundBeepBeep);
wait1Msec(1000);
// Do whatever needs to be done at goal
turnRight(180);
driveStraight(distthree);
turnRight(90);
driveStraight(disttwo);
turnRight(90);
driveStraight(distone);
}
int main(int argc, char** argv) {
shutDownAfter(119);
msleep(1000); //Wait for other robot to finish. Should be 30s (3000). Is only 1000 for testing
raise_botguy_to(BOTGUY_CLAW_UP);
enable_servos();
printf("camera open response: %i\n", camera_open());
printf("driving to pos.\n");
driveStraight();
msleep(3700);
stop();
turnInPlaceCCW();
msleep(700);
stop();
driveStraight();
msleep(1900); //was 1500
stop();
turnInPlaceCW();
msleep(580);
stop();
msleep(1000);
raise_botguy_to(BOTGUY_CLAW_DOWN);
driveStraight();
msleep(1100);
stop();
close_botguy_claw();
hold_botguy_claw_closed();
//preformApproachBotguy(false, 0);
driveBackward();
msleep(700);
raise_botguy_to(BOTGUY_CLAW_MID_UP);
msleep(900);
raise_botguy_to(BOTGUY_CLAW_FULL_UP);
turnInPlaceCCW();
msleep(600);
stop();
driveBackward();
msleep(1000);
stop();
preformApproachCube(false, 1);
stop();
return 0;
turnInPlaceCW();
msleep(700);
driveStraight();
msleep(3000);
turnInPlaceCCW();
msleep(700);
driveStraight();
msleep(1700);
stop();
msleep(500);
raise_botguy_to(BOTGUY_CLAW_MID_UP);
return (EXIT_SUCCESS);
}
