Esempio n. 1
0
void grabBall(void)
{
	SetPin(PIN_F2, blink_on);
	SetPin(PIN_F3, !blink_on);

	//Open hand
	SetPin(PIN_F3, blink_on);
	SetPin(PIN_F3, !blink_on);
	SetServo(handServo, 0.2f);
	Wait(1.0);

	lowerArm();
	
	//Close hand
	SetPin(PIN_F3, blink_on);
	SetPin(PIN_F2, !blink_on);
	SetServo(handServo, 0.65f);
	Wait(1.0);

	
	//arm goes up
	SetPin(PIN_F2, blink_on);
	SetPin(PIN_F3, !blink_on);
	raiseArm();

}
Esempio n. 2
0
void positionArm(int position)
{// Positions arm to a specified place
	if (getArmPotentiometer() < position )
	{
		while(getArmPotentiometer() < position && vexRT[Btn8R] == 0)
		{
			raiseArm();
			driveWithJoysticks();
			intakeControl();
		}

		stopArm();
	}
	else if (getArmPotentiometer() > position)
	{
		while (getArmPotentiometer() > position && vexRT[Btn8R] == 0)
		{
			lowerArm();
			driveWithJoysticks();
			intakeControl();
		}

		stopArm();
	}
}
Esempio n. 3
0
task main()
{
	initializeRobot();
	//waitForStart();
	moveDownWrist();
	moveRobot();
	liftArm();
	wait1Msec(500);
	rightTurnRobot();
	wait1Msec(750);
	shortMoveRobot();
	wait1Msec(500);
	moveUpWrist();
	lowerArm();

}
Esempio n. 4
0
void forLols() {
	setTank();
	powerArm(0);
	raiseArm(650);
	powerArm(10);
	motor[Intake1] = motor[Intake2] = -127;
	forward(430);
	wait1Msec(1000);
	back(430);
	basicMove(0, 0, 0);
	//waitForButton();
	wait1Msec(4000);
	forward(480);
	wait1Msec(1000);
	back(500);
	basicMove(0, 0, 0);
	lowerArm(0);
	motor[Intake1] = motor[Intake2] = 0;
	basicMove(0, 0, 0);
}
Esempio n. 5
0
// Most basic, scores preload, knocks over big balls.
void forGreaterJustice() {
	setTank();
	forward(1100);
	raiseArm(1200);
	powerArm(10);
	//back(50);
	basicMove(0, 0, 0);
	motor(Intake1) =motor(Intake2) = -127;
	wait1Msec(800);
	back(400);
	motor(Intake1) =motor(Intake2) = 0;
	lowerArm(10);
	back(600);
	basicMove(0, 0, 0);
	waitForButton();
	// Push over big balls
	//raiseArm(650);
	powerArm(0);
	//stopArm();
	motor[Arm1] = motor[Arm2] = 0;
	forLols();
}
Esempio n. 6
0
task usercontrol()
{
	while(true)
	{
		driveWithJoysticks();
		intakeControl();

		if(vexRT[Btn6U]== 1 && vexRT[Btn6D] == 0 && vexRT[Btn8U]== 0 && vexRT[Btn8D]== 0)
		{
			raiseArm();
		}
		else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 1 && vexRT[Btn8U] == 0 && vexRT[Btn8D] == 0 && vexRT[Btn8R] == 0)
		{
			lowerArm();
		}
		else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 0 && vexRT[Btn8U] == 1 && vexRT[Btn8D] == 0 && vexRT[Btn8R] == 0)
		{
			positionArm(Arm_ScoreHeight);
		}
		else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 0 && vexRT[Btn8U] == 0 && vexRT[Btn8D] == 1 && vexRT[Btn8R] == 0)
		{
			positionArm(Arm_MinHeight);
		}
		else
		{
			stopArm();
		}

		if(vexRT[Btn7U] == 1)
		{
			if(btn7UPressed != 1)
			{
				currentIntakeSpeedStep++;

				if(currentIntakeSpeedStep > 2)
				{
					currentIntakeSpeedStep = 2;
				}

				btn7DPressed = true;
			}
		}
		else
		{
			btn7UPressed = false;
		}

		if(vexRT[Btn7D] == 1)
		{
			if(btn7DPressed != 1)
			{
				currentIntakeSpeedStep--;

				if(currentIntakeSpeedStep < 1)
				{
					currentIntakeSpeedStep = 1;
				}

				btn7DPressed = true;
			}
		}
		else
		{
			btn7DPressed = false;
		}

		switch(currentIntakeSpeedStep)
		{
		case 1:
			currentIntakeSpeed = IntakeSlow;
			break;

		case 2:
			currentIntakeSpeed = IntakeFast;
			break;

		default:
			currentIntakeSpeed = IntakeFast;
			break;
		}
	}

	while ((vexRT[Btn7D] == 1) || (vexRT[Btn7U] == 1)	|| (vexRT[Btn7R] == 1)	|| (vexRT[Btn7L] == 1))
	{
		// Wait until button is released to go back to single joystick control.
		driveWithJoysticks();
	}
}
Esempio n. 7
0
task main() {
	waitForStart();
  /*int raw = 0;
  int nrm = 0;
  bool active = true;

  // Turn the light on
  LSsetActive(LEGOLS);

  nNxtButtonTask  = -2;

  nxtDisplayCenteredTextLine(0, "Lego");
  nxtDisplayCenteredBigTextLine(1, "LIGHT");
  nxtDisplayCenteredTextLine(3, "SMUX Test");
  nxtDisplayCenteredTextLine(5, "Connect SMUX to");
  nxtDisplayCenteredTextLine(6, "S1 and sensor to");
  nxtDisplayCenteredTextLine(7, "SMUX Port 1");
  wait1Msec(2000);

  nxtDisplayClearTextLine(7);
  nxtDisplayTextLine(5, "Press [enter]");
  nxtDisplayTextLine(6, "to toggle light");
  wait1Msec(2000);*/

  //while (true) {
    // The enter button has been pressed, switch
    // to the other mode
    /*if (nNxtButtonPressed == kEnterButton) {
      active = !active;
      if (!active)
        LSsetInactive(LEGOLS);
      else
        LSsetActive(LEGOLS);

      // wait 500ms to debounce the switch
      wait1Msec(500);
    }

    nxtDisplayClearTextLine(5);
    nxtDisplayClearTextLine(6);
    raw = LSvalRaw(LEGOLS);
    nrm = LSvalNorm(LEGOLS);
    nxtDisplayTextLine(5, "Raw:  %4d", raw);
    nxtDisplayTextLine(6, "Norm: %4d", nrm);
    wait1Msec(50);
  }*/

	int ser=0,fs1=0;
	servo[servo1]=ser;
  servo[servo2]=215-ser;
  bool rightOfLine = true,touch=false;
  LSsetActive(LEGOLS);
  touch = TSreadState(LEGOTS);
  lsVal = LSvalRaw(LEGOLS);
	nMotorEncoder[arms] = 0; //Initiate Encoder Pos

  wait1Msec(2500);


  while(lsVal<360){
  	lsVal = LSvalRaw(LEGOLS);
  	forward(15);
  }
  stop();
  //reverse(50);
  wait1Msec(500);
  stop();
  wait1Msec(1000);
  while(nMotorEncoder[arms]<30){
		raiseArm(50);
		nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[arms]);
	}stop();
	wait1Msec(1000);
  while(lsVal<210){
  	nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
  	lsVal = LSvalRaw(LEGOLS);
  	left(50);
  }
  stop();
  wait1Msec(1000);
 	fs1 = HTFreadSensor(HTFS1);
 	nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
	ClearTimer(T1);
	while(time1[T1]<2500){
		nxtDisplayCenteredTextLine(3,"Time:%i",time1[T1]);
	  fs1 = HTFreadSensor(HTFS1);
  	lsVal = LSvalRaw(LEGOLS);
  	touch = TSreadState(LEGOTS);
    nxtDisplayTextLine(5, "Raw:  %4d", lsVal);
		if(lsVal>210){
			while(time1[T1]<2500){
				forward(20);
  			lsVal = LSvalRaw(LEGOLS);
   			nxtDisplayTextLine(5, "Raw:  %4d", lsVal);
			}
			rightOfLine=!rightOfLine;
		}
		else if(lsVal<210){
			if(rightOfLine){
				rotateLeft(50);
			}else{
				rotateRight(50);
			}
		}
	}
	stop();

	ser=100;
	servo[servo1]=ser;
  servo[servo2]=215-ser;

  while(nMotorEncoder[arms]>5){
		lowerArm(50);
		nxtDisplayCenteredTextLine(1,"Encoder:%i",nMotorEncoder[arms]);
	}stop();
	reverse(50);
	wait1Msec(2500);
}