public void changeSpeed(double Y, double Z) {
leftSpeed = (((Y * yBoost) - (Z * zBoost)) * limiter);
rightSpeed = (((Y * yBoost * -1) - (Z * zBoost)) * limiter);
talon1.set(leftSpeed);
talon2.set(leftSpeed);
talon3.set(rightSpeed);
talon4.set(rightSpeed);
}
public void RotateFrame(double s, double distance, double dir) {
reset();
left.set(dir * s);
right.set(dir * s);
do {
SmartDashboard.putNumber("left distance", lEncoder.getDistance());
SmartDashboard.putNumber("right distance", rEncoder.getDistance());
} while (rEncoder.getDistance() <= distance);
left.set(0);
right.set(0);
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
double x = joy1.getX();
double y = -joy1.getY();
double r = joy1.getZ();
if (Math.abs(x) < .05) x = 0;
if (Math.abs(y) < .05) y = 0;
if (Math.abs(r) < .05) r = 0;
drive(x / 2, y / 2, r / 2);
shooter1.set(joy2.getY());
shooter2.set(joy2.getY());
}
public void DriveForward(double s, double distance) {
reset();
left.set(s);
right.set(-1 * s);
do {
SmartDashboard.putNumber("left distance", lEncoder.getDistance());
SmartDashboard.putNumber("right distance", rEncoder.getDistance());
} while (rEncoder.getDistance() <= distance);
left.set(0);
right.set(0);
reset();
}
public void stopMotor() {
if (RobotMap.IS_REAL_BOT == true) {
kickerTalon.set(0.0);
} else if (RobotMap.IS_REAL_BOT == false) {
kickerJaguar.set(0.0);
}
}
public void teleopPeriodic() {
double throttle = (driveStick.getThrottle() + 1) / -2;
double xAxis = throttle * (Math.abs(driveStick.getX()) > 0.15 ? driveStick.getX() : 0.0);
double yAxis = throttle * (Math.abs(driveStick.getY()) > 0.15 ? driveStick.getY() : 0.0);
double rotation =
throttle * (Math.abs(driveStick.getTwist()) > 0.15 ? driveStick.getTwist() : 0.0);
// Mecanum drive
drive.mecanumDrive_Cartesian(xAxis, yAxis, rotation, 0);
// Elevator control
elevator.set(driveStick.getRawButton(5) ? -1.0 : driveStick.getRawButton(3) ? 1.0 : 0.0);
// Intake control
intakeTensionMotor.set(
driveStick.getRawButton(2) ? -1.0 : driveStick.getRawButton(7) ? 1.0 : 0.0);
if (Math.abs(controlStick.getTwist()) > 0) {
double toteTurnSpeed = -controlStick.getTwist();
intakeMotors[0].set(
toteTurnSpeed > 0 ? Math.pow(toteTurnSpeed, 2) : -Math.pow(toteTurnSpeed, 2));
intakeMotors[1].set(
toteTurnSpeed > 0 ? Math.pow(toteTurnSpeed, 2) : -Math.pow(toteTurnSpeed, 2));
} else {
double intakeSpeed =
driveStick.getRawButton(1) ? -1.0 : driveStick.getRawButton(8) ? 0.4 : 0.0;
intakeMotors[0].set(-intakeSpeed);
intakeMotors[1].set(intakeSpeed);
}
}
public void drive(
double xAxis,
double yAxis,
double mid) { // xaxis rotates, yaxis forward and back, mid goes side to side
hDrive.arcadeDrive(yAxis, xAxis);
middleMotor1.set(-mid);
}
public void arcadeDrive() {
lT = OI.getDriveStick().getLeftX() - OI.getDriveStick().getLeftY();
rT = OI.getDriveStick().getLeftX() + OI.getDriveStick().getLeftY();
// maxV = Math.max(Math.abs(lT), Math.abs(rT));
if (maxV > 1) {
lT = lT / maxV;
rT = rT / maxV;
}
left.set(lT);
right.set(rT);
SmartDashboard.putNumber("left distance while driving", lEncoder.getDistance());
SmartDashboard.putNumber("right distance while driving", rEncoder.getDistance());
}
private void scooperControl(double speed) {
if (isAtUpperLimit() && speed > 0) {
speed = 0;
} else if (isAtLowerLimit() && speed < 0) {
speed = 0;
}
scooperMotor.set(speed);
}
/*
*
* @param direction -1 to draw the arm back, +1 to rotate motor forward away from arm.
*
*/
public void reload(int direction, double backSpeed, double frontSpeed) {
final int REVERSE = -1;
if (direction == DRAW_KICKER_BACK) {
if (RobotMap.IS_REAL_BOT == true) {
kickerTalon.set(
REVERSE
* (backSpeed)); // TODO: Check to make sure the motors will draw back when recieving
// a positive voltage.
} else if (RobotMap.IS_REAL_BOT == false) {
kickerJaguar.set(REVERSE * backSpeed);
}
} else if (direction == ROTATE_KICKER_FORWARD) {
if (RobotMap.IS_REAL_BOT == true) {
kickerTalon.set(frontSpeed);
} else if (RobotMap.IS_REAL_BOT == false) {
kickerJaguar.set(frontSpeed);
}
}
}
protected void execute() {
if (time.get() < 3.0) {
talonLeft.set(1);
talonRight.set(1);
} else if (time.get() < 5.0) {
talonLeft.set(0);
talonRight.set(0);
soleLiftDown.set(true);
} else if (time.get() < 6.0) {
talonLeft.set(-1);
talonRight.set(-1);
} else talonLeft.set(1);
talonRight.set(1);
}
public ShooterSubsystem() {
kickerLatch = new DoubleSolenoid(RobotMap.KICKER_LATCH_RELEASE, RobotMap.KICKER_LATCH_RELOAD);
kickerLatch.set(DoubleSolenoid.Value.kForward);
if (RobotMap.IS_REAL_BOT == true) {
kickerTalon = new Talon(RobotMap.KICKER_MOTOR);
kickerTalon.set(0.0);
} else if (RobotMap.IS_REAL_BOT == false) {
kickerJaguar = new Jaguar(RobotMap.KICKER_MOTOR);
kickerJaguar.set(0.0);
}
kickerLimitSwitch = new DigitalInput(RobotMap.KICKER_RETRACTED_SWITCH);
kickerMotorReturnSwitch = new DigitalInput(RobotMap.KICKER_MOTOR_RETURN_SWITCH);
}
/** This function is called once each time the robot enters operator control. */
public void operatorControl() {
drivetrain.setSafetyEnabled(true);
compressor.start();
while (isOperatorControl() && isEnabled()) {
// The throttle goes from 1 to -1, bottom to top. This
// scales it to go 0 to 1, bottom to top.
double throttle = (-leftStick.getRawAxis(3) + 1) / 2;
double drive = deadband(leftStick.getY()) * throttle;
double steer = deadband(-rightStick.getX()) * throttle;
drivetrain.arcadeDrive(drive, steer);
boolean shouldSpinWheels = leftStick.getRawButton(1);
boolean shouldFire = shouldSpinWheels && rightStick.getRawButton(1);
shooterWheels.set(shouldSpinWheels ? -1 : 0);
shooter.set(shouldFire);
Timer.delay(0.005); // wait for a motor update time
}
}
public void shoot(float speed) {
kickerTalon.set(speed);
}
public void disable() {
leftMotor.set(0.0);
rightMotor.set(0.0);
rightEncoder.reset();
leftEncoder.reset();
} // end disable
/**
* Reads input from the joystick in order to control the lifting part of the gatherer, which
* operates on a three-tier elevator-like system. Three different buttons are read from the
* joystick in order to move the {@link Talon} to one of the three tiers. Update periodically.
*
* @param input The joystick that will be used to control the gatherer.
*/
public void gathererTalon(Joystick input) {
if (gatherLift == null) {
System.err.println(
"The gatherer motor controllers have not been initialized. Call this class's method \"initGatherers()\" in order to do so.");
return;
}
switch (gatherState) {
case 0: // checking for input
if (input.getRawButton(Wiring.BTN_GATHERER_TO_TOP) && diGathererTop.get()) {
gatherLift.set(0.6);
System.out.println("Gatherer up");
gatherCount = 0;
gatherState = 1;
} else if (input.getRawButton(Wiring.BTN_GATHERER_TO_BOT) && diGathererBot.get()) {
gatherLift.set(-0.6);
System.out.println("Gatherer down");
gatherCount = 0;
gatherState = 3;
} else if (input.getRawButton(Wiring.BTN_GATHERER_TO_MID)) {
if (!diGathererTop.get()) {
gatherLift.set(-0.6);
System.out.println("Gatherer down");
gatherCount = 0;
gatherState = 5;
} else if (!diGathererBot.get()) {
gatherLift.set(0.6);
System.out.println("Gatherer up");
gatherCount = 0;
gatherState = 6;
}
}
break;
case 1: // should be going up, wait until it hits the top
if (!diGathererTop.get()) {
gatherLift.set(0.0);
System.out.println("Top limit");
gatherState = 2;
}
break;
case 2: // at top, waiting for driver to stop holding the button
if (!input.getRawButton(Wiring.BTN_GATHERER_TO_TOP)) {
gatherState = 0;
}
break;
case 3: // should be going down, wait until it hist the bottom
if (!diGathererBot.get()) {
gatherLift.set(0.0);
System.out.println("Bottom limit");
gatherState = 4;
}
break;
case 4: // at bottom, waiting for driver to stop holding the button
if (!input.getRawButton(Wiring.BTN_GATHERER_TO_BOT)) {
gatherState = 0;
}
break;
case 5: // go down to mid
gatherLift.set(-0.6);
if (counter.get() >= 1) {
gatherLift.set(0.0); // stahp
counter.reset();
gatherState = 0;
}
break;
case 6: // go up to mid
gatherLift.set(0.6);
if (counter.get() >= 1) {
gatherLift.set(0.0); // stahp
counter.reset();
gatherState = 0;
}
break;
}
}
public void gathererSpark(Joystick input, PowerDistributionPanel pdp) {
switch (gatherState) {
case 0:
if (input.getRawButton(7)) {
gatherLift.set(0.6);
System.out.println("Gatherer up");
gatherState = 1;
} else if (input.getRawButton(8)) {
gatherLift.set(-0.6);
System.out.println("Gatherer down");
gatherState = 4;
} else if (input.getRawButton(5)) {
}
break;
case 1:
if (pdp.getCurrent(13) > 1.25) {
System.out.println("Motor Running");
gatherState = 2;
} else if (gatherCount > 3) {
gatherState = 3;
System.out.println("Gather Timeout");
}
gatherCount++;
break;
case 2:
if (pdp.getCurrent(13) <= 1.25) {
gatherLift.set(0.0);
System.out.println("Top limit");
gatherState = 3;
}
break;
case 3:
if (!input.getRawButton(7)) {
gatherCount = 0;
gatherState = 0;
}
break;
case 4:
if (pdp.getCurrent(13) > 1.25) {
System.out.println("Motor Running");
gatherState = 5;
} else if (gatherCount > 3) {
gatherState = 6;
System.out.println("Gather Timeout");
}
gatherCount++;
break;
case 5:
if (pdp.getCurrent(13) <= 1.25) {
gatherLift.set(0.0);
System.out.println("Bottom limit");
gatherState = 6;
}
break;
case 6:
if (!input.getRawButton(8)) {
gatherCount = 0;
gatherState = 0;
}
break;
}
}
public void setCollectorMotor(double speed) {
collectorTalon.set(speed);
}
public void doNothing() {
mainMotor.set(0.0);
}
public void goVariable(double speed) {
mainMotor.set(speed);
SmartDashboard.putNumber("motorspeed", mainMotor.get());
}
public void go() {
mainMotor.set(.3);
}
public void setElevatorMotor(double speed) {
moverUpperTalon.set(speed);
}
public void In() {
Intake.set(in);
}
public void drive(double x, double y, double r) {
front.set(x + r / 2);
left.set(-x / 2 + y + r);
right.set(-x / 2 - y + r);
}
public void Out() {
Intake.set(out);
}
public void Stop() {
Intake.set(0);
}
public void setLassoSpeed(double speed) {
lasso.set(speed);
}
/** 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