diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ee178f7..7264d8e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -330,8 +330,8 @@ public static final class Homing{ } public static final class Tilt{ public static final int ID = 2; - public static final double POT_ADJUSTMENT_FACTOR = 100; - public static final double POT_OFFSET = 0; + public static final double POT_ADJUSTMENT_FACTOR = 1;//105; + public static final double POT_OFFSET = 0;//-95; } } public static final class ClimberMotionParameters { @@ -341,11 +341,11 @@ public static final class ClimberMotionParameters { public static final double TILT_PERCENT_OUTPUT = 0.7; - public static final int CLIMBER_TOP = -243000; + public static final int CLIMBER_TOP = -225000; //-243000; public static final int CLIMBER_LOW_BAR_TOP = -110000; public static final int CLIMBER_BOTTOM = 5000; public static final double TILT_START = 7000; - public static final double STATIONARY_LOCK_ANGLE = 22; + public static final double STATIONARY_LOCK_ANGLE = 15; //22; public static final double EXTEND_START_ANGLE = 0; public static final double NEXT_BAR_ANGLE = -5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b8b5482..6c6f687 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -66,7 +67,6 @@ public class RobotContainer { // private final JoystickButton hookDownButton = new JoystickButton(buttonBoard, 1); private final JoystickButton lockStationariesButton = new JoystickButton(buttonBoard, 1); private final JoystickButton nextBarButton = new JoystickButton(buttonBoard, 16); - private final JoystickButton hookDownButton = new JoystickButton(rightJoystick, 9); //private final JoystickButton hookUpButton = new JoystickButton(rightJoystick, 11); @@ -76,6 +76,7 @@ public class RobotContainer { private final JoystickButton stopAll = new JoystickButton(buttonBoard, 4); private final JoystickButton hookZeroButton = new JoystickButton(rightJoystick, 7); + private final JoystickButton indexTwoButton = new JoystickButton(buttonBoard, gitforcepushorginmaster); private final JoystickButton climberControlButton = new JoystickButton(leftJoystick, 3); @@ -148,7 +149,6 @@ private void configureButtonBindings() { ejectButton.whileHeld(new ParallelCommandGroup(intakeEjectCommand, indexerEjectCommand, shootEjectCommand)); hookZeroButton.whileHeld(hookZeroCommand); - // hookDownButton.whenPressed(hookDownCommand); // hookUpButton.whenPressed(hookUpCommand); // hookUpLowButton.whenPressed(new HookUpLow(climber)); diff --git a/src/main/java/frc/robot/commands/climber/HookZero.java b/src/main/java/frc/robot/commands/climber/HookZero.java index 62ae7a7..2c03ac5 100644 --- a/src/main/java/frc/robot/commands/climber/HookZero.java +++ b/src/main/java/frc/robot/commands/climber/HookZero.java @@ -28,13 +28,15 @@ public void initialize() { @Override public void execute() { System.out.println("zeroing"); - climber.setWinch(Constants.ClimberConstants.ClimberMotionParameters.HOOK_ZERO_SPEED); + climber.setWinch(-Constants.ClimberConstants.ClimberMotionParameters.HOOK_ZERO_SPEED); climber.zeroWinch(); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + climber.zeroWinch(); + climber.setWinch(0); } diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index c740ada..aad01d4 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -110,7 +110,9 @@ public double tiltAngle(){ // return 90-((tilt.getSelectedSensorPosition()/2048)+45); // return 45-(tilt.getSelectedSensorPosition()/2048/2000*360); //return tilt.getSelectedSensorPosition()/2048/2000*360;//previously used one - return pot.get(); + //return pot.get(); + double p = pot.get(); + return ((p - 0.082) * -345) +256 - 9.3; } @Override @@ -119,11 +121,15 @@ public void periodic() { SmartDashboard.putNumber("TiltAngle", tiltAngle()); SmartDashboard.putNumber("Legacy TiltAngle", legacyTiltAngle()); + //SmartDashboard.putNumber("Old value testing", ((tiltAngle() - 0.082) * -340) +256 - 10.8); + SmartDashboard.putBoolean("mainLocked", mainLocked()); SmartDashboard.putBoolean("stationaryLocked", stationaryLocked()); SmartDashboard.putBoolean("sensorLeft", getMainSensorLeft()); SmartDashboard.putBoolean("sensorRight", getMainSensorRight()); SmartDashboard.putBoolean("allingment switch", getHomeSwitch()); + + SmartDashboard.putNumber("Winch", winch.getSelectedSensorPosition()); } } \ No newline at end of file