/** This function is called periodically during operator control */
public void teleopPeriodic() {
robotDrive.mecanumDrive_Polar();
lifter.upLift();
lifter.downLift();
lifter.QuickRaise();
lifter.toteLowering(); // lowers 6 totes and a can at .1 speed
// lifter.manualUplift();
// lifter.manualDownlift();
servo.runBolt();
pusher.PushandRetract(); // push one button to push and retract
pusher.Push(); // full push with one button press
pusher.Retract(); // full retract with one button press
// pusher.ManualPush();
// pusher.ManualRetract();
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());
}
/** 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
