diff --git a/Robot2019/src/main/java/frc/robot/OI.java b/Robot2019/src/main/java/frc/robot/OI.java index 9277518..05eea9d 100644 --- a/Robot2019/src/main/java/frc/robot/OI.java +++ b/Robot2019/src/main/java/frc/robot/OI.java @@ -11,7 +11,7 @@ import edu.wpi.cscore.VideoSink; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.JoystickButton; -import frc.robot.commands.ActuateClimberRails; +import frc.robot.commands.ToggleClimberRails; import frc.robot.commands.Climb; import frc.robot.commands.EjectCargo; import frc.robot.commands.EjectHatch; @@ -80,7 +80,7 @@ public class OI { cargoEjectBtn.whenPressed(new EjectCargo(cargo)); climberRailBtn = new JoystickButton(manipulator, Manip.LB_lShoulder); - climberRailBtn.whenPressed(new ActuateClimberRails(climber)); + climberRailBtn.whenPressed(new ToggleClimberRails(climber)); autoClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger); autoClimbBtn.toggleWhenPressed(new Climb(climber, dt, leftJoy)); diff --git a/Robot2019/src/main/java/frc/robot/RobotMap.java b/Robot2019/src/main/java/frc/robot/RobotMap.java index f002f1e..bea8cbf 100644 --- a/Robot2019/src/main/java/frc/robot/RobotMap.java +++ b/Robot2019/src/main/java/frc/robot/RobotMap.java @@ -64,7 +64,7 @@ public class RobotMap { // Initialize motors on the climbing mech climberMotor = new VictorSP(1); climberEncoder = new Encoder(new DigitalInput(4), new DigitalInput(5)); - climberPistons = new DoubleSolenoid(6, 1); + climberPistons = new DoubleSolenoid(5, 2); // Initialize motors on the cargo mech cargoRoller = new VictorSP(0); diff --git a/Robot2019/src/main/java/frc/robot/commands/ActuateClimberRails.java b/Robot2019/src/main/java/frc/robot/commands/ToggleClimberRails.java similarity index 69% rename from Robot2019/src/main/java/frc/robot/commands/ActuateClimberRails.java rename to Robot2019/src/main/java/frc/robot/commands/ToggleClimberRails.java index bbb906f..7c5df3c 100644 --- a/Robot2019/src/main/java/frc/robot/commands/ActuateClimberRails.java +++ b/Robot2019/src/main/java/frc/robot/commands/ToggleClimberRails.java @@ -3,10 +3,10 @@ import edu.wpi.first.wpilibj.command.InstantCommand; import frc.robot.subsystems.Climber; -public class ActuateClimberRails extends InstantCommand { +public class ToggleClimberRails extends InstantCommand { private Climber climber; - public ActuateClimberRails(Climber cl) { + public ToggleClimberRails(Climber cl) { super(); requires(cl); climber = cl; @@ -15,6 +15,6 @@ public ActuateClimberRails(Climber cl) { // Called once when the command executes @Override protected void initialize() { - climber.actuateRails(); + climber.toggleRails(); } } \ No newline at end of file diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java index bd5ebb3..5278c50 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java +++ b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.VictorSP; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.commands.KeepClimber; public class Climber extends Subsystem { @@ -40,8 +41,12 @@ public Climber(VictorSP motor, Encoder enc, AHRS ahrs, DoubleSolenoid pistons) { enc.reset(); } - public void actuateRails() { - pistons.set(DoubleSolenoid.Value.kForward); + public void toggleRails() { + if (pistons.get() == DoubleSolenoid.Value.kForward) { + pistons.set(DoubleSolenoid.Value.kReverse); + } else { + pistons.set(DoubleSolenoid.Value.kForward); + } } public void runClimber(double speed) { @@ -53,14 +58,7 @@ public void stopClimber() { } public double getAngle() { - double rawAngle = Math.atan2(ahrs.getRawAccelZ(), ahrs.getRawAccelX()); - double angle; - if (rawAngle > 0) { - angle = rawAngle - Math.PI; - } else { - angle = rawAngle + Math.PI; - } - return angle * 180 / Math.PI; + return ahrs.getPitch(); } //We are erring on the side of changing directions too much