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() {
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");
}
public void shoot(double timeToShoot) {
m_timeToShoot = timeToShoot;
if (false == shooting && false == retracting) {
shooting = true;
retracting = false;
shootTime.start();
shootTime.reset();
}
}
protected void execute() {
double time = _timer.get();
if (time >= _period) {
Robot.ledStrip.setColor(
_colors[index].getRed(), _colors[index].getGreen(), _colors[index].getBlue());
index = (index + 1) % _colors.length;
_timer.reset();
}
}
/** Resets the recorder so we can record properly next time */
public void reset() {
if (m_bRecStarted) {
writeDataToFile();
m_List.removeAllElements();
m_Index = 0;
m_tmRecorder.stop();
m_tmRecorder.reset();
m_bRecDone = false;
m_bRecStarted = false;
}
}
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;
}
}
protected void execute() {
double delta = timer.get();
distance += (drivetrain.getAverageForwardVelocity() * delta);
timer.reset();
// PID tuning code
pidControllerDistance.setPID(
pidConfigDistance.getP(P),
pidConfigDistance.getI(I),
pidConfigDistance.getD(D),
pidConfigDistance.getF(F));
pidConfigDistance.setSetpoint(pidControllerDistance.getSetpoint());
pidConfigDistance.setValue(drivetrainRotation.pidGet());
}
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();
}
/** 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();
}
/** 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;
}
/** 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();
}
public void reset() {
delayTimer.reset();
delayTimer.start();
}
// Called just before this Command runs the first time
protected void initialize() {
timer.start();
timer.reset();
intake.set(Constants.intakeSpeed);
}
public void startTime() {
System.out.println("timer started");
timer.start();
timer.reset();
}
public static void Reinit() {
timer.stop();
timer.reset();
timer.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