protected void initialize() {
time = new Timer();
time.reset();
time.start();
}
/*
* reads the value of the digital input of the specified channel
* requires that the readArudiono() method has already been called
* param 1: the channel to be read
* return: value of the specified digital input
*/
public boolean getInput(int channelID) {
byte temp;
if (timer.get() > 2.0) {
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser3, 1, " ");
DriverStationLCD.getInstance().updateLCD();
timer.stop();
timer.reset();
}
if (Math.abs(channelID - (13 / 2)) - (13 / 2)
> 0) // returns true if less than zero or greater than thirteen
{
timer.start();
DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1, "Invalid ID");
DriverStationLCD.getInstance().updateLCD();
return false;
} else if (channelID < 8) {
temp = (byte) (loByte & (1 << channelID));
return temp != 0;
} else {
temp = (byte) (hiByte & (1 << channelID - 8));
return temp != 0;
}
}
// Called just before this Command runs the first time
protected void initialize() {
counter++;
delayTimer = RobotMap.ShooterDelayTimer;
delayTimer.start();
display.println(
DriverStationLCD.Line.kUser2, 1, "Pass command " + counter + " ");
display.updateLCD();
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.logger.log("Initialize", 1);
finished = false;
timer.reset();
timer.start();
lastOffset = 0;
stillCycles = 0;
}
@Override
protected void initialize() {
beginTime = Integer.MAX_VALUE;
triggered = false;
timer1.reset();
timer1.start();
System.out.println("Shoot Init");
}
protected void startAction() {
shuttle = Shuttle.getInstance();
shuttle.down();
timer = new Timer();
timer.start();
}
// Called just before this Command runs the first time
protected void initialize() {
Robot.drivetrain.resetGyro();
Robot.drivetrain.drivePID.setSetpoint(STRAIGHT_SET_POINT);
Robot.drivetrain.throttleInt.useJoystick(false);
Robot.drivetrain.throttleInt.setThrottle(thisDriveSpeed);
timer2.start();
startTime2 = timer2.get();
state = State.START_DRIVE; // initializes the state
}
public void shoot(double timeToShoot) {
m_timeToShoot = timeToShoot;
if (false == shooting && false == retracting) {
shooting = true;
retracting = false;
shootTime.start();
shootTime.reset();
}
}
public void autonomousInit() {
// Timer section
timerMain = new Timer();
timerMain.reset();
timerMain.start();
timer1Left = new Timer();
timer2Left = new Timer();
timer1Right = new Timer();
timer2Right = new Timer();
timer1Center = new Timer();
timer2Center = new Timer();
controlChuteLeft = new boolean[] {false, false};
controlChuteRight = new boolean[] {false, false};
controlChuteCenter = new boolean[] {false, false};
controlChuteLeft[0] = true;
controlChuteRight[0] = true;
controlChuteCenter[0] = true;
/* Boolean setting section
* Robot Aligns Left
*/
if (DigitalInput7.get() == false && DigitalInput8.get() == true) {
b_leftAutonomous = true;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = true;
b_leftDriveAutonomous = false;
}
// Robot Aligns Right
if (DigitalInput7.get() == true && DigitalInput8.get() == false) {
b_leftAutonomous = false;
b_rightAutonomous = true;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = true;
b_rightDriveAutonomous = false;
}
// Robot Aligns Center
if (DigitalInput7.get() == true && DigitalInput8.get() == true) {
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = true;
b_upAutonomous = false;
b_downAutonomous = true;
b_centerDriveAutonomous = false;
}
} // End autonomousInit
private double m_shoot() {
if (encoder.get() >= desired || shootTime.get() > m_timeToShoot) {
shooting = false;
retracting = true;
shootTime.stop();
retractTime.start();
retractTime.reset();
return .1;
} else { // encoder < desired && shootTime < 0.2
error = desired - encoder.get();
return -1;
}
}
public Shooter(int encoderA, int encoderB, int victor1, int victor2, int victor3) {
encoder = new Encoder(encoderA, encoderB);
encoder.start();
encoder.reset();
motor1 = new Victor(victor1); // 2
motor2 = new Victor(victor2); // 5
motor3 = new Victor(victor3); // 4
time = new Timer();
shootTime = new Timer();
retractTime = new Timer();
time.start();
time.reset();
}
/**
* Records bots movements to specified file
*
* @param sFileName
*/
public void record(String sFileName) {
if (!m_bRecStarted) {
m_sFile = sFileName;
m_tmRecorder.start();
m_bRecStarted = true;
}
if (!m_bRecDone) {
m_Index++;
m_botDataAuto = new BotData();
m_botDataAuto.setValues(m_tmRecorder.get(), m_bot);
m_List.addElement(m_botDataAuto);
} else {
Vars.fnDisableDrive();
m_bot.stopRobot();
m_tmRecorder.stop();
m_tmRecorder.reset();
}
}
/** Unjams the shooter conveyor */
public static void shakeShooter() {
timer.start();
while (timer.get() < 2) {
if (direction) {
try {
shooterConveyor.setX(Constants.CONV_SHOOTER_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
} else {
try {
shooterConveyor.setX(-Constants.CONV_SHOOTER_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
}
direction = !direction;
timer.reset();
}
@Override
public boolean isCompleted() {
// If the Controller is completable, then the error will need to be
// between a certain threshold before isCompleted return true
if (completable) {
if (Math.abs(previousError) < THRESHOLD) {
if (timerStarted) {
if (doneTimer.get() > TIME_TO_BE_COMPLETE_MILLISECONDS) {
return true;
}
} else {
doneTimer.start();
timerStarted = true;
}
} else {
timerStarted = false;
}
}
return false;
}
/** Called every time tele-op mode starts */
public void teleopInit() {
vicDriveRight.set(0);
vicDriveLeft.set(0);
armVictor.set(0);
winchRelay.set(Relay.Value.kOff);
shooterWheel.set(0);
shooterLoft.set(0);
bettyRelay.set(Relay.Value.kOff);
lightsOutput1.set(true);
lightsOutput2.set(true);
lightsOutput3.set(true);
lightsTimer.reset();
lightsTimer.start();
teleopActive = false;
controlFlip = true;
controlFlipClean = true;
shootingSpeed = 0.0;
starterDelay = 0;
}
/** Unjams the ingestor conveyor */
public static void shakeIngest() {
timer.start();
while (timer.get() < 2) {
if (direction) {
try {
ingestConveyor.setX(Constants.CONV_INGEST_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
} else {
try {
ingestConveyor.setX(-Constants.CONV_INGEST_POWER);
} catch (CANTimeoutException ex) {
ex.printStackTrace();
}
}
}
direction = !direction;
timer.reset();
}
/** This function is called periodically during autonomous */
public void autonomousInit() {
autoTimer.reset();
autoTimer.start();
gyro.reset();
relay.set(Relay.Value.kOn);
setSpeedFast();
setLEDTeamColour();
sol4.set(false);
sol5.set(true);
sol7.set(true);
sol8.set(false);
autoStop = false;
autonomousStopped = false;
autoTimeAtStop = 99999;
drive.setRun(false);
loadAndShoot.setRun(false);
autoForward();
}
protected void initialize() {
_timer.start();
}
protected void routine() {
boolean goLeft = false;
Timer t = new Timer();
t.start();
// Turn on wheel
shooterController.setVelocityGoal(4000);
// Grab balls from ground
pinniped.wantFront = false;
pinniped.wantRear = false;
frontIntake.wantBumperGather = true;
rearIntake.wantBumperGather = true;
waitTime(.5);
// Drive to correct place
if (goLeft) path.goLeft();
else path.goRight();
drivePath(path, 10);
// Hold position
drivebase.resetEncoders();
System.out.println("Drive time: " + t.get());
headingController.setDistance(0);
headingController.setHeading(Math.toDegrees(Math.PI / 6.0) * (goLeft ? 1.0 : -1.0));
drivebase.useController(headingController);
// Wait for 5 seconds in
waitTime(.75);
System.out.println("Shooting 1st ball at: " + t.get());
// Shoot first ball
rearIntake.wantShoot = frontIntake.wantShoot = true;
waitTime(.25);
pinniped.wantShot = true;
waitTime(.5);
pinniped.wantShot = false;
rearIntake.wantShoot = frontIntake.wantShoot = false;
// Speed up for 2nd and 3rd shots
shooterController.setVelocityGoal(4300);
// Queue 2nd ball
rearIntake.wantBumperGather = false;
rearIntake.wantDown = true;
rearIntake.setManualRollerPower(1.0);
waitTime(0.3);
rearIntake.wantDown = false;
waitTime(.4);
rearIntake.setManualRollerPower(0);
// Settle time
waitTime(.75);
// Shoot second ball
frontIntake.wantShoot = true;
waitTime(.25);
rearIntake.setManualRollerPower(1);
pinniped.wantShot = true;
waitTime(.5);
pinniped.wantShot = false;
rearIntake.setManualRollerPower(0);
waitTime(0.3);
frontIntake.wantShoot = false;
// Queue 3rd ball
frontIntake.wantBumperGather = false;
frontIntake.wantDown = true;
frontIntake.setManualRollerPower(0.5);
waitTime(0.3);
frontIntake.wantDown = false;
waitTime(0.5);
frontIntake.setManualRollerPower(0.00);
// Settle time
waitTime(1.0);
// Shoot thirdball
rearIntake.setManualRollerPower(1);
pinniped.wantShot = true;
waitTime(.5);
pinniped.wantShot = false;
// Print out time
System.out.println(t.get());
// Clean up
rearIntake.setManualRollerPower(0);
shooterController.setVelocityGoal(0);
}
@Override
protected void initialize() {
timer.start();
}
// Called just before this Command runs the first time
protected void initialize() {
timer.start();
timer.reset();
intake.set(Constants.intakeSpeed);
}
/** This is called when the command is first added to the task manager. */
@Override
public void init() {
timer = new Timer();
timer.start();
}
protected void initialize() {
pidControllerDistance.enable();
pidControllerDistance.setSetpoint(0);
drivetrainStrafe.mecanumPolarStrafe(FORWARD_SPEED, 0);
timer.start();
}
public void startTime() {
System.out.println("timer started");
timer.start();
timer.reset();
}
public void reset() {
delayTimer.reset();
delayTimer.start();
}
/** This function is called periodically during autonomous */
public void autonomousPeriodic() {
// Start Timer
MainTimer = timerMain.get();
// Robot Aligns Left
if (DigitalInput6.get() == false // Ensure pusher is fully retracted
&& DigitalInput7.get() == false
&& DigitalInput8.get() == true
&& b_leftAutonomous == true
&& b_rightAutonomous == false
&& b_centerAutonomous == false
&& b_upAutonomous == false
&& b_downAutonomous == true
&& b_leftDriveAutonomous == false) {
LifterTalon.set(autoInitialDownSpeed);
b_leftAutonomous = true;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = true;
b_downAutonomous = false;
b_leftDriveAutonomous = false;
}
// Lifter Descends To LimitSwitch 1, lifter immediately ascends, reset booleans
if (DigitalInput1.get() == false
&& b_leftAutonomous == true
&& b_rightAutonomous == false
&& b_centerAutonomous == false
&& b_upAutonomous == true
&& b_downAutonomous == false
&& b_leftDriveAutonomous == false) {
LifterTalon.set(autoInitialUpSpeed);
b_leftAutonomous = true;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = false;
b_leftDriveAutonomous = false;
}
// Lifter ascends to LimitSwitch 4, stops motor, reset booleans
if (DigitalInput4.get() == false
&& b_leftAutonomous == true
&& b_rightAutonomous == false
&& b_centerAutonomous == false
&& b_upAutonomous == false
&& b_downAutonomous == false
&& b_leftDriveAutonomous == false) {
LifterTalon.set(autoSpeedStop);
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = false;
b_leftDriveAutonomous = true;
// Delay one second
Timer.delay(1);
timer1Left.reset();
timer1Left.start();
}
// Begin driving no limit switch since we might drive while lifting the can TBD.
if (b_leftDriveAutonomous == true
&& b_leftAutonomous == false
&& b_rightAutonomous == false
&& b_centerAutonomous == false
&& b_upAutonomous == false
&& b_downAutonomous == false) {
// Orchestration A Left
if (controlChuteLeft[0] == true) {
while ((autoTimerStart1Left = timer1Left.get()) < orch_A_Left_Time) {
robotDrive.mecanumDrive_Polar(
orch_A_Left_Mag,
orch_A_Left_Dir,
orch_A_Left_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
}
controlChuteLeft[0] = false;
controlChuteLeft[1] = true;
timer2Left.reset();
timer2Left.start();
}
// Orchestration B Left
if (controlChuteLeft[1] == true) {
while ((autoTimerStart2Left = timer2Left.get()) < orch_B_Left_Time) {
robotDrive.mecanumDrive_Polar(
orch_B_Left_Mag,
orch_B_Left_Dir,
orch_B_Left_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
}
controlChuteLeft[0] = false;
controlChuteLeft[1] = false;
}
// Stop mecanumDrive
if (controlChuteLeft[0] == false && controlChuteLeft[1] == false) {
robotDrive.mecanumDrive_Polar(
stop_mecanum_Mag,
stop_mecanum_Dir,
stop_mecanum_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
b_leftDriveAutonomous = false;
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = false;
}
}
// Robot Aligns Right
if (DigitalInput6.get() == false // pusher is fully retracted
&& DigitalInput7.get() == true
&& DigitalInput8.get() == false
&& b_leftAutonomous == false
&& b_rightAutonomous == true
&& b_centerAutonomous == false
&& b_upAutonomous == false
&& b_downAutonomous == true
&& b_rightDriveAutonomous == false) {
LifterTalon.set(autoInitialDownSpeed);
b_leftAutonomous = false;
b_rightAutonomous = true;
b_centerAutonomous = false;
b_upAutonomous = true;
b_downAutonomous = false;
b_rightDriveAutonomous = false;
}
// Lifter Descends To LimitSwitch 1, lifter immediately ascends, reset booleans
if (DigitalInput1.get() == false
&& b_leftAutonomous == false
&& b_rightAutonomous == true
&& b_centerAutonomous == false
&& b_upAutonomous == true
&& b_downAutonomous == false
&& b_rightDriveAutonomous == false) {
LifterTalon.set(autoInitialUpSpeed);
b_leftAutonomous = false;
b_rightAutonomous = true;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = false;
b_rightDriveAutonomous = false;
}
// Lifter ascends to LimitSwitch 4, stops motor, reset booleans
if (DigitalInput4.get() == false
&& b_leftAutonomous == false
&& b_rightAutonomous == true
&& b_centerAutonomous == false
&& b_upAutonomous == false
&& b_downAutonomous == false
&& b_rightDriveAutonomous == false) {
LifterTalon.set(autoSpeedStop);
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = false;
b_rightDriveAutonomous = true;
// Delay one second
Timer.delay(1);
timer1Right.reset();
timer1Right.start();
}
// Begin driving no limit switch since we might drive while lifting the can TBD.
if (b_rightDriveAutonomous == true
&& b_leftAutonomous == false
&& b_rightAutonomous == false
&& b_centerAutonomous == false
&& b_upAutonomous == false
&& b_downAutonomous == false) {
// Orchestration A Right
if (controlChuteRight[0] == true) {
while ((autoTimerStart1Right = timer1Right.get()) < orch_A_Right_Time) {
robotDrive.mecanumDrive_Polar(
orch_A_Right_Mag,
orch_A_Right_Dir,
orch_A_Right_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
}
controlChuteRight[0] = false;
controlChuteRight[1] = true;
timer2Right.reset();
timer2Right.start();
}
// Orchestration B Right
if (controlChuteRight[1] == true) {
while ((autoTimerStart2Right = timer2Right.get()) < orch_B_Right_Time) {
robotDrive.mecanumDrive_Polar(
orch_B_Right_Mag,
orch_B_Right_Dir,
orch_B_Right_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
}
controlChuteRight[0] = false;
controlChuteRight[1] = false;
}
// Stop mecanumDrive
if (controlChuteRight[0] == false && controlChuteRight[1] == false) {
robotDrive.mecanumDrive_Polar(
stop_mecanum_Mag,
stop_mecanum_Dir,
stop_mecanum_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
b_rightDriveAutonomous = false;
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = false;
}
} // End Robot Aligns Right
// Robot Aligns Center
if (DigitalInput6.get() == false // pusher is fully retracted
&& DigitalInput7.get() == true
&& DigitalInput8.get() == true
&& b_leftAutonomous == false
&& b_rightAutonomous == false
&& b_centerAutonomous == true
&& b_upAutonomous == false
&& b_downAutonomous == true
&& b_centerDriveAutonomous == false) {
LifterTalon.set(autoInitialDownSpeed);
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = true;
b_upAutonomous = true;
b_downAutonomous = false;
b_centerDriveAutonomous = false;
}
// Lifter Descends To LimitSwitch 1, lifter immediately ascends, reset booleans
if (DigitalInput1.get() == false
&& b_leftAutonomous == false
&& b_rightAutonomous == false
&& b_centerAutonomous == true
&& b_upAutonomous == true
&& b_downAutonomous == false
&& b_centerDriveAutonomous == false) {
LifterTalon.set(autoInitialUpSpeed);
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = true;
b_upAutonomous = false;
b_downAutonomous = false;
b_centerDriveAutonomous = false;
}
// Lifter ascends to LimitSwitch 4, stops motor, reset booleans
if (DigitalInput4.get() == false
&& b_leftAutonomous == false
&& b_rightAutonomous == false
&& b_centerAutonomous == true
&& b_upAutonomous == false
&& b_downAutonomous == false
&& b_centerDriveAutonomous == false) {
LifterTalon.set(autoSpeedStop);
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = false;
b_centerDriveAutonomous = true;
// Delay one second
Timer.delay(1);
timer1Center.reset();
timer1Center.start();
}
// Begin driving no limit switch since we might drive while lifting the can TBD.
if (b_centerDriveAutonomous == true
&& b_leftAutonomous == false
&& b_rightAutonomous == false
&& b_centerAutonomous == false
&& b_upAutonomous == false
&& b_downAutonomous == false) {
// Orchestration A Center
if (controlChuteCenter[0] == true) {
while ((autoTimerStart1Center = timer1Center.get()) < orch_A_Center_Time) {
robotDrive.mecanumDrive_Polar(
orch_A_Center_Mag,
orch_A_Center_Dir,
orch_A_Center_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
}
controlChuteCenter[0] = false;
controlChuteCenter[1] = true;
timer2Center.reset();
timer2Center.start();
}
// Orchestration B Center
if (controlChuteCenter[1] == true) {
while ((autoTimerStart2Center = timer2Center.get()) < orch_B_Center_Time) {
robotDrive.mecanumDrive_Polar(
orch_B_Center_Mag,
orch_B_Center_Dir,
orch_B_Center_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
}
controlChuteCenter[0] = false;
controlChuteCenter[1] = false;
}
// Stop mecanumDrive Center
if (controlChuteCenter[0] == false && controlChuteCenter[1] == false) {
robotDrive.mecanumDrive_Polar(
stop_mecanum_Mag,
stop_mecanum_Dir,
stop_mecanum_Rot); // Magnitude (speed), Direction (degrees), Rotation (Turning)
b_centerDriveAutonomous = false;
b_leftAutonomous = false;
b_rightAutonomous = false;
b_centerAutonomous = false;
b_upAutonomous = false;
b_downAutonomous = false;
}
} // End Robot Aligns Center
SmartDashboard.putBoolean("switch 1", DigitalInput1.get());
SmartDashboard.putBoolean("switch 2", DigitalInput2.get());
SmartDashboard.putBoolean("switch 3", DigitalInput3.get());
SmartDashboard.putBoolean("switch 4", DigitalInput4.get());
SmartDashboard.putBoolean("switch 5", DigitalInput5.get());
SmartDashboard.putBoolean("switch 6", DigitalInput6.get());
SmartDashboard.putNumber("Lifting Talon Speed", LifterTalon.get());
SmartDashboard.putNumber("Pushing Talon Speed", PushingTalon.get());
SmartDashboard.putNumber("servo position zero means brake is on", servo.get());
SmartDashboard.putBoolean("Position switch Red", DigitalInput7.get());
SmartDashboard.putBoolean("Position switch Green", DigitalInput8.get());
SmartDashboard.putNumber("MainTimer", MainTimer);
SmartDashboard.putNumber("autoTimerStart1Left", autoTimerStart1Left);
SmartDashboard.putNumber("autoTimerStart2Left", autoTimerStart2Left);
SmartDashboard.putNumber("autoTimerStart1Right", autoTimerStart1Right);
SmartDashboard.putNumber("autoTimerStart2Right", autoTimerStart2Right);
SmartDashboard.putNumber("autoTimerStart1Center", autoTimerStart1Center);
SmartDashboard.putNumber("autoTimerStart2Center", autoTimerStart2Center);
} // End autonomousPeriodic
public static void Reinit() {
timer.stop();
timer.reset();
timer.start();
}
protected void initialize() {
time.start();
}
