Fix angles for climbing (#94)

* Fix angles for climbing

* Change climber piston ports

* Use atan for climber angle

* use pitch instead of accel calculations

* change rails to toggle

Author: SaltyJoke

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.ToggleHatchEject;
@@ -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));

…/robot/commands/ActuateClimberRails.java …c/robot/commands/ToggleClimberRails.javaRobot2019/src/main/java/frc/robot/commands/ActuateClimberRails.java renamed to Robot2019/src/main/java/frc/robot/commands/ToggleClimberRails.java Robot2019/src/main/java/frc/robot/commands/ActuateClimberRails.java renamed to 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();
   }
 }

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