From cd48a1ae845915645b6fd756c7e80cbb9c68a607 Mon Sep 17 00:00:00 2001 From: DriveStationComputer Date: Thu, 14 Mar 2019 16:41:05 -0700 Subject: [PATCH 1/7] Fix angles for climbing --- Robot2019/src/main/java/frc/robot/subsystems/Climber.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java index bd5ebb3..391f5a2 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 { @@ -53,6 +54,11 @@ public void stopClimber() { } public double getAngle() { + SmartDashboard.putNumber("AHRS Z", ahrs.getRawAccelZ()); + double zee = ahrs.getRawAccelZ() * -1; + double angle = Math.asin(zee) * 180 / Math.PI; + return angle; + /* double rawAngle = Math.atan2(ahrs.getRawAccelZ(), ahrs.getRawAccelX()); double angle; if (rawAngle > 0) { @@ -61,6 +67,7 @@ public double getAngle() { angle = rawAngle + Math.PI; } return angle * 180 / Math.PI; + */ } //We are erring on the side of changing directions too much From d15070834746325f5179046419175835704c2052 Mon Sep 17 00:00:00 2001 From: DriveStationComputer Date: Thu, 14 Mar 2019 17:25:01 -0700 Subject: [PATCH 2/7] Use atan for climber angle --- Robot2019/src/main/java/frc/robot/subsystems/Climber.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java index 391f5a2..8bd6f87 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java +++ b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java @@ -56,7 +56,8 @@ public void stopClimber() { public double getAngle() { SmartDashboard.putNumber("AHRS Z", ahrs.getRawAccelZ()); double zee = ahrs.getRawAccelZ() * -1; - double angle = Math.asin(zee) * 180 / Math.PI; + double why = ahrs.getRawAccelY() * -1; + double angle = Math.atan2(zee, why) * 180 / Math.PI; return angle; /* double rawAngle = Math.atan2(ahrs.getRawAccelZ(), ahrs.getRawAccelX()); From 0ad84a9fa97a588026e5454234b453cf5603db7a Mon Sep 17 00:00:00 2001 From: Kevin Wamg <20215378+kevinzwang@users.noreply.github.com> Date: Thu, 14 Mar 2019 17:53:24 -0700 Subject: [PATCH 3/7] use pitch instead of accel calculations --- .../main/java/frc/robot/subsystems/Climber.java | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java index 8bd6f87..25ae7f8 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java +++ b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java @@ -54,21 +54,7 @@ public void stopClimber() { } public double getAngle() { - SmartDashboard.putNumber("AHRS Z", ahrs.getRawAccelZ()); - double zee = ahrs.getRawAccelZ() * -1; - double why = ahrs.getRawAccelY() * -1; - double angle = Math.atan2(zee, why) * 180 / Math.PI; - return angle; - /* - 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 From 71caeabd9ab5968e0ab190c80089cf6f1d0f9577 Mon Sep 17 00:00:00 2001 From: alexander-mcdowell Date: Fri, 15 Mar 2019 14:32:31 -0700 Subject: [PATCH 4/7] change rails to toggle --- Robot2019/src/main/java/frc/robot/OI.java | 4 ++-- .../{ActuateClimberRails.java => ToggleClimberRails.java} | 6 +++--- Robot2019/src/main/java/frc/robot/subsystems/Climber.java | 8 ++++++-- 3 files changed, 11 insertions(+), 7 deletions(-) rename Robot2019/src/main/java/frc/robot/commands/{ActuateClimberRails.java => ToggleClimberRails.java} (69%) 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/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..a4460b7 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/Climber.java +++ b/Robot2019/src/main/java/frc/robot/subsystems/Climber.java @@ -40,8 +40,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) { From f218806206423ca29fa75f579918fbc3dbc088e5 Mon Sep 17 00:00:00 2001 From: DriverStationComputer Date: Sat, 16 Mar 2019 16:13:50 -0700 Subject: [PATCH 5/7] changes to controls requested by drive team --- Robot2019/src/main/java/frc/robot/OI.java | 8 +++---- .../java/frc/robot/commands/ManualClimb.java | 2 +- .../java/frc/robot/commands/NormalDrive.java | 21 +++---------------- .../java/frc/robot/commands/TeleopDrive.java | 4 ++-- 4 files changed, 10 insertions(+), 25 deletions(-) diff --git a/Robot2019/src/main/java/frc/robot/OI.java b/Robot2019/src/main/java/frc/robot/OI.java index 06eec8e..e469ff2 100644 --- a/Robot2019/src/main/java/frc/robot/OI.java +++ b/Robot2019/src/main/java/frc/robot/OI.java @@ -67,7 +67,7 @@ public class OI { arcadeOrTankBtn = new JoystickButton(leftJoy, 4); arcadeOrTankBtn.whenPressed(new SetArcadeOrTank()); normDriveBtn = new JoystickButton(leftJoy, 3); - normDriveBtn.whileHeld(new NormalDrive()); + normDriveBtn.whenPressed(new NormalDrive()); hatchIntakeBtn = new JoystickButton(manipulator, Manip.X); hatchIntakeBtn.whenPressed(new ToggleHatchIntake(hp)); @@ -79,13 +79,13 @@ public class OI { cargoEjectBtn = new JoystickButton(manipulator, Manip.B); // TODO: set ports to correct values cargoEjectBtn.whenPressed(new EjectCargo(cargo)); - climberRailBtn = new JoystickButton(manipulator, Manip.LB_lShoulder); + climberRailBtn = new JoystickButton(manipulator, Manip.RB_rShoulder); climberRailBtn.whenPressed(new ToggleClimberRails(climber)); - autoClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger); + autoClimbBtn = new JoystickButton(manipulator, Manip.LT_lTrigger); autoClimbBtn.toggleWhenPressed(new Climb(climber, dt, leftJoy)); - manualClimbBtn = new JoystickButton(manipulator, Manip.LT_lTrigger); + manualClimbBtn = new JoystickButton(manipulator, Manip.RT_rTrigger); manualClimbBtn.toggleWhenPressed(new ManualClimb(climber, manipulator)); toggleCameraBtn = new JoystickButton(leftJoy, 2); diff --git a/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java b/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java index 93a095b..357679b 100644 --- a/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java +++ b/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java @@ -19,7 +19,7 @@ public class ManualClimb extends Command { final private double retractDist = 0; final private double climbDist = 24.46; // In inches - final private int climbJoyAxis = 1; + final private int climbJoyAxis = 3; public ManualClimb(Climber climber, Joystick manip) { // Use requires() here to declare subsystem dependencies diff --git a/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java b/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java index f30bb46..5465555 100644 --- a/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java +++ b/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java @@ -7,26 +7,11 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.InstantCommand; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -public class NormalDrive extends Command { +public class NormalDrive extends InstantCommand { protected void initialize() { - SmartDashboard.putBoolean("Characterized Drive", false); - } - - @Override - protected boolean isFinished() { - return false; - } - - @Override - protected void end() { - SmartDashboard.putBoolean("Characterized Drive", true); - } - - @Override - protected void interrupted() { - end(); + SmartDashboard.putBoolean("Characterized Drive", SmartDashboard.getBoolean("Characterized Drive", true)); } } diff --git a/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java b/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java index 7d4979c..35c14f9 100644 --- a/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java @@ -84,7 +84,7 @@ private void arcadeDrive() { } } - if (SmartDashboard.getBoolean("Characterized Drive", true)) { + if (SmartDashboard.getBoolean("Characterized Drive", false)) { charDrive(left, right); } else { dt.drive(left, right); @@ -107,7 +107,7 @@ private void tankDrive() { right *= SmartDashboard.getNumber("Speed Slow Ratio", 0.5); } - if (SmartDashboard.getBoolean("Characterized Drive", true)) { + if (SmartDashboard.getBoolean("Characterized Drive", false)) { charDrive(left, right); } else { dt.drive(left, right); From 33039fcc1597f0c29dc16972e35001636a30d8d2 Mon Sep 17 00:00:00 2001 From: DriverStationComputer Date: Sat, 16 Mar 2019 16:17:36 -0700 Subject: [PATCH 6/7] lol --- Robot2019/src/main/java/frc/robot/commands/NormalDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java b/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java index 5465555..43cffac 100644 --- a/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java +++ b/Robot2019/src/main/java/frc/robot/commands/NormalDrive.java @@ -12,6 +12,6 @@ public class NormalDrive extends InstantCommand { protected void initialize() { - SmartDashboard.putBoolean("Characterized Drive", SmartDashboard.getBoolean("Characterized Drive", true)); + SmartDashboard.putBoolean("Characterized Drive", !SmartDashboard.getBoolean("Characterized Drive", true)); } } From 5366132d58c2b1205ef8ef105367103fb9ef2ee1 Mon Sep 17 00:00:00 2001 From: DriverStationComputer Date: Fri, 22 Mar 2019 00:32:09 -0700 Subject: [PATCH 7/7] change slow climb formula the goal of this new formula is to find a set voltage/joystick value where the robot will be more or less supported statically by the elevator, and use that as the neutral value of the manipulator --- .../src/main/java/frc/robot/commands/ManualClimb.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java b/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java index 1339d6b..861540c 100644 --- a/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java +++ b/Robot2019/src/main/java/frc/robot/commands/ManualClimb.java @@ -29,8 +29,8 @@ public ManualClimb(Climber climber, Joystick manip) { this.climber = climber; this.manip = manip; - if (!SmartDashboard.containsKey("Slow Climb Minimum")) { - SmartDashboard.putNumber("Slow CLimb Minimum", 0); + if (!SmartDashboard.containsKey("Slow Climb Neutral")) { + SmartDashboard.putNumber("Slow Climb Neutral", -0.5); } } @@ -48,7 +48,9 @@ protected void execute() { double climbSpeed = manip.getRawAxis(climbJoyAxis); if (SmartDashboard.getBoolean("Slow Climb", false)) { - climbSpeed = (climbSpeed - 1) / 2; + double neutral = SmartDashboard.getNumber("Slow Climb Neutral", -0.5); + // this part doesn't make any immediate intuitive sense but trust me it is the correct formula + climbSpeed = climbSpeed * (1 + neutral) + neutral; } // if (climbSpeed > 0 && climber.getEncDistance() >= climbDist) {