Пример #1
0
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 ;
}
Пример #2
0
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);
	}
}
Пример #4
0
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);
	}
}
Пример #7
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);
	}
}
Пример #10
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)
	{}
}
Пример #11
0
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);
}
Пример #12
0
void square()
{
	int i = 0;
	for(i; i < 4; i++)
	{
		driveStraight();
		Wait(1400);
		rotateRight();
		Wait(560);
	}
	stop();
}
Пример #13
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);
    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);
      }
    }
  }
  
  
}
Пример #14
0
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)
	}
}
Пример #16
0
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(); }
}
Пример #17
0
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(); }
}
Пример #18
0
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)
}
Пример #19
0
task autonomous()
{
	writeDebugStream("Autonomous Started...");
	driveStraight(2, BOT_ID);
}
Пример #20
0
void driveInInches(float distance) {
  if(distance <= 0){
    driveStraight(10, -171.89 * distance);
  }
  driveStraight(170, 171.89 * distance);
}
Пример #21
0
/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 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);
}
Пример #22
0
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);
}
Пример #23
0
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);

}
Пример #24
0
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);
}