diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java index 6bf7382..9584792 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java @@ -39,9 +39,7 @@ public class OperatorController { public OperatorController(CommandGamepad g, Robot r) { robot = r; gamepad = g; - AssignNamedControllerButton(); - BindControls(); } @@ -109,15 +107,17 @@ public void bindKidShampooControls() { Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem) ); dumpWrist.whenPressed( - Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem) + Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem) ); scoopWrist.whenPressed( - Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem) + Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem) ); } public void bindArmControls() { - armIntake.whenPressed(Command.create(robot.armSubsystem::setArmToIntake, robot.armSubsystem)); + armIntake.whenPressed( + Command.create(robot.armSubsystem::setArmToIntake, robot.armSubsystem) + ); // armLowNet.whenPressed(Command.create(robot.armSubsystem::lowBasket, robot.armSubsystem)); // armLowSpecimen.whenPressed(Command.create(robot.armSubsystem::lowSpecimen, robot.armSubsystem)); // armHighSpecimen.whenPressed(Command.create(robot.armSubsystem::highSpecimen, robot.armSubsystem)); @@ -128,12 +128,13 @@ public void bindArmControls() { armIncrement.whenPressed(Command.create(robot.armSubsystem::increment, robot.armSubsystem)); armDecrement.whenPressed(Command.create(robot.armSubsystem::decrement, robot.armSubsystem)); slideIn.whenPressed(Command.create(robot.armSubsystem::slideDecrement, robot.armSubsystem)); - slideOut.whenPressed(Command.create(robot.armSubsystem::slideIncrement, robot.armSubsystem)); + slideOut.whenPressed( + Command.create(robot.armSubsystem::slideIncrement, robot.armSubsystem) + ); } public void bindHangControls() { //suspend.whenPressed(Command.create(robot.hangSubsystem::suspend, robot.hangSubsystem)); } - } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/JustDrivingTeleOp.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/JustDrivingTeleOp.java index abe2754..f476052 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/JustDrivingTeleOp.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/JustDrivingTeleOp.java @@ -33,6 +33,7 @@ public void uponInit() { if (Setup.Connected.DRIVEBASE) { controlsDriver = new DriverController(driverGamepad, robot); robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.OBSERVATION_START.toPose()); + CommandScheduler.scheduleForState( EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem), OpModeState.INIT diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java index a0418c6..6f3fd7d 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java @@ -42,6 +42,7 @@ public class ArmSubsystem implements Subsystem, Loggable { public static double MAX_SLIDE_MOTOR_POWER = 0.5; public static double SLIDE_FEEDFORWARD_GRAVITY_VALUE = 0.3; public static double SLIDE_FEEDFORWARD_INTAKE_POS = 0; + // This is "5 degrees" if our numbers are correct: public static int ARM_POS_CLOSE_ENOUGH = Math.abs(ARM_HORIZONTAL - ARM_VERTICAL) / 18; @@ -82,10 +83,9 @@ private void setArmPos(int e) { } private void setSlidePos(int e) { - e = Range.clip(e,0,SLIDE_MAX_POS); + e = Range.clip(e, 0, SLIDE_MAX_POS); slidePidController.setTargetPosition(e); slideTargetPos = e; - } public ArmSubsystem(Hardware hw) { @@ -124,9 +124,8 @@ The downward torque due to gravity is cos(T) * Gravity (9.8m/s^2) */ (ticks, velocity) -> { - armFeedFwdValue = FEEDFORWARD_COEFFICIENT * - Math.cos(getArmAngle(ticks)) * - getSlideLength(); + armFeedFwdValue = + FEEDFORWARD_COEFFICIENT * Math.cos(getArmAngle(ticks)) * getSlideLength(); if (Math.abs(armFeedFwdValue) < 0.1) { armFeedFwdValue = 0.0; @@ -152,11 +151,13 @@ The downward torque due to gravity is cos(T) * Gravity (9.8m/s^2) private static double getArmAngle(double ticks) { // our horizontal value starts at ARM_HORIZONTAL, so we need to // subtract it - return (Math.PI/2.0) * (ticks - ARM_HORIZONTAL) /(ARM_VERTICAL - ARM_HORIZONTAL); + return ((Math.PI / 2.0) * (ticks - ARM_HORIZONTAL)) / (ARM_VERTICAL - ARM_HORIZONTAL); } - private double getSlideLength(){ + + private double getSlideLength() { return getSlideUnmodifiedPosition() + SLIDE_OFFSET; } + private boolean isArmHorizontal() { return Math.abs(getArmCurrentPos() - ARM_HORIZONTAL) < ARM_POS_CLOSE_ENOUGH; } @@ -200,13 +201,15 @@ public void slideIncrement() { public void slideDecrement() { setSlidePos(slideTargetPos - SLIDE_INC_DEC); } + private int getCurrentSlidePos() { return getSlideUnmodifiedPosition() - slideResetPos; } private int getArmCurrentPos() { - if (isHardware) + if (isHardware) { return -armEncoder.getPosition(); + } return 0; }