From 0f76954f8aaef4d1447d10dda3b9dd2a88c8a0fb Mon Sep 17 00:00:00 2001 From: ysthakur <45539777+ysthakur@users.noreply.github.com> Date: Fri, 4 Mar 2022 20:14:35 -0500 Subject: [PATCH 01/25] Get IntakeTestMap to compile with new changes --- src/main/deploy/pathplanner/1ball blue 2.path | 2 +- .../frc/team449/javaMaps/IntakeTestMap.java | 105 +++++++++--------- 2 files changed, 54 insertions(+), 53 deletions(-) diff --git a/src/main/deploy/pathplanner/1ball blue 2.path b/src/main/deploy/pathplanner/1ball blue 2.path index dce29e4b..44ca257e 100644 --- a/src/main/deploy/pathplanner/1ball blue 2.path +++ b/src/main/deploy/pathplanner/1ball blue 2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":7.614591741639,"y":2.957976022713611},"prevControl":null,"nextControl":{"x":7.311960531394373,"y":2.1867545514450453},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.584033943868601,"y":1.962221718037742},"prevControl":{"x":6.335730820927835,"y":1.962221718037742},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":2.0,"maxAcceleration":1.0,"isReversed":false} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":7.558283470997322,"y":2.8936766734292965},"prevControl":null,"nextControl":{"x":7.255652260752695,"y":2.122455202160731},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.789777604871666,"y":0.7523559350916172},"prevControl":{"x":7.89394996511512,"y":1.0764477225156979},"nextControl":{"x":7.685605244628211,"y":0.4282641476675366},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.544284506290531,"y":1.6667577638952737},"prevControl":{"x":5.9493992405706315,"y":1.2269189095340214},"nextControl":{"x":5.13916977201043,"y":2.106596618256526},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.951823498583706,"y":2.8010790198795585},"prevControl":{"x":7.73190407140308,"y":2.3728148722120235},"nextControl":{"x":7.73190407140308,"y":2.3728148722120235},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.510584929933365,"y":1.3238518673916093},"prevControl":{"x":7.281886978562657,"y":2.14816412734927},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":2.0,"maxAcceleration":0.6,"isReversed":false} \ No newline at end of file diff --git a/src/main/java/frc/team449/javaMaps/IntakeTestMap.java b/src/main/java/frc/team449/javaMaps/IntakeTestMap.java index 9a5b1986..92f2b350 100644 --- a/src/main/java/frc/team449/javaMaps/IntakeTestMap.java +++ b/src/main/java/frc/team449/javaMaps/IntakeTestMap.java @@ -3,7 +3,6 @@ import com.pathplanner.lib.PathPlanner; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.hal.simulation.SimDeviceDataJNI; -import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; @@ -14,21 +13,17 @@ import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.SerialPort; -import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.team449.CommandContainer; import frc.team449.RobotMap; import frc.team449._2022robot.cargo.Cargo2022; -import frc.team449._2022robot.climber.ClimberArm; -import frc.team449._2022robot.climber.PivotingTelescopingClimber; +import frc.team449.ahrs.PIDAngleControllerBuilder; import frc.team449.components.RunningLinRegComponent; import frc.team449.drive.unidirectional.DriveUnidirectionalWithGyro; import frc.team449.drive.unidirectional.commands.UnidirectionalNavXDefaultDrive; @@ -207,41 +202,46 @@ public static RobotMap createRobotMap() { false); var driveDefaultCmd = - new UnidirectionalNavXDefaultDrive<>( - 0, - new Debouncer(1.5), - 0, - 0.6, - null, - 2, - 3.0, - false, - .01, - 0, - 0.03, - new Debouncer(0.15), - drive, - oi, - null); + new UnidirectionalNavXDefaultDrive<>( + 3.0, + new Debouncer(0.15), + drive, + oi, + null, + new PIDAngleControllerBuilder() + .absoluteTolerance(0) + .onTargetBuffer(new Debouncer(1.5)) + .minimumOutput(0) + .maximumOutput(0.6) + .loopTimeMillis(null) + .deadband(2) + .inverted(false) + .pid(0.01, 0, 0.03) + .build()); Supplier resetDriveOdometry = () -> new InstantCommand(() -> drive.resetOdometry(new Pose2d()), drive); SmartDashboard.putData("Reset odometry", resetDriveOdometry.get()); var cargo = - new Cargo2022( - new SparkMaxConfig() - .setName("intakeMotor") - .setPort(INTAKE_LEADER_PORT) - .addSlaveSpark(FollowerUtils.createFollowerSpark(INTAKE_FOLLOWER_PORT), true) - .createReal(), - new SparkMaxConfig() - .setName("spitterMotor") - .setPort(SPITTER_PORT) - .setEnableBrakeMode(false) - .createReal(), - INTAKE_SPEED, - SPITTER_SPEED); + new Cargo2022( + new SparkMaxConfig() + .setName("intakeMotor") + .setPort(INTAKE_LEADER_PORT) + .addSlaveSpark(FollowerUtils.createFollowerSpark(INTAKE_FOLLOWER_PORT), true) + .createReal(), + new SparkMaxConfig() + .setName("spitterMotor") + .setPort(SPITTER_PORT) + .setEnableBrakeMode(false) + .createReal(), + new DoubleSolenoid( + FullMap.PCM_MODULE, + PneumaticsModuleType.CTREPCM, + FullMap.INTAKE_PISTON_FWD_CHANNEL, + FullMap.INTAKE_PISTON_REV_CHANNEL), + INTAKE_SPEED, + SPITTER_SPEED); Supplier runIntake = () -> new InstantCommand(cargo::runIntake, cargo) @@ -284,8 +284,7 @@ public static RobotMap createRobotMap() { // PUT YOUR SUBSYSTEM IN HERE AFTER INITIALIZING IT var subsystems = List.of(drive, cargo /*, climber*/); - var updater = - new Updater(List.of(pdp, navx, oi, () -> field.setRobotPose(drive.getCurrentPose()))); + Updater.subscribe(() -> field.setRobotPose(drive.getCurrentPose())); // Button bindings here // Take in balls but don't shoot @@ -298,21 +297,23 @@ public static RobotMap createRobotMap() { .whenReleased(cargo::stop, cargo); // TODO BUTTON BINDINGS HERE -// new JoystickButton(mechanismsJoystick, XboxController.Button.kA.value) -// .whileActiveContinuous( -// new WaitCommand(0.01) -// .andThen(() -> climber.setSetpoint(climber.getSetpoint() + 0.01), climber)); -// new JoystickButton(mechanismsJoystick, XboxController.Button.kB.value) -// .whileActiveContinuous( -// new WaitCommand(0.01) -// .andThen(() -> climber.setSetpoint(climber.getSetpoint() - 0.01), climber)); + // new JoystickButton(mechanismsJoystick, XboxController.Button.kA.value) + // .whileActiveContinuous( + // new WaitCommand(0.01) + // .andThen(() -> climber.setSetpoint(climber.getSetpoint() + 0.01), + // climber)); + // new JoystickButton(mechanismsJoystick, XboxController.Button.kB.value) + // .whileActiveContinuous( + // new WaitCommand(0.01) + // .andThen(() -> climber.setSetpoint(climber.getSetpoint() - 0.01), + // climber)); var ramsetePrototype = - new RamseteBuilder() - .drivetrain(drive) - .leftPidController(new PIDController(DRIVE_KP_VEL, 0, DRIVE_KD_VEL)) - .rightPidController(new PIDController(DRIVE_KP_VEL, 0, DRIVE_KD_VEL)) - .field(field); + new RamseteBuilder() + .drivetrain(drive) + .leftPid(new PIDController(DRIVE_KP_VEL, 0, DRIVE_KD_VEL)) + .rightPid(new PIDController(DRIVE_KP_VEL, 0, DRIVE_KD_VEL)) + .field(field); // // var sCurve = // spit.get() @@ -365,7 +366,7 @@ public static RobotMap createRobotMap() { new CommandContainer( robotStartupCommands, autoStartupCommands, teleopStartupCommands, testStartupCommands); - return new RobotMap(subsystems, pdp, updater, allCommands, false); + return new RobotMap(subsystems, pdp, allCommands, false); } /** Generate a trajectory for the S-shaped curve we're using to test */ From 2ed313cf46eb22d62765c4e5dbf1c0945abf3948 Mon Sep 17 00:00:00 2001 From: ysthakur <45539777+ysthakur@users.noreply.github.com> Date: Sat, 5 Mar 2022 10:24:00 -0500 Subject: [PATCH 02/25] Make climber arms profiled, add hall sensor --- src/main/java/frc/team449/Robot.java | 2 +- .../_2022robot/climber/ClimberArm.java | 48 +++---- .../climber/PivotingTelescopingClimber.java | 66 ++++++--- .../java/frc/team449/javaMaps/FullMap.java | 132 ++++++++---------- 4 files changed, 127 insertions(+), 121 deletions(-) diff --git a/src/main/java/frc/team449/Robot.java b/src/main/java/frc/team449/Robot.java index 9d9004bf..1c254a52 100644 --- a/src/main/java/frc/team449/Robot.java +++ b/src/main/java/frc/team449/Robot.java @@ -22,7 +22,7 @@ public class Robot extends TimedRobot { /** The method that runs when the robot is turned on. Initializes all subsystems from the map. */ public static @NotNull RobotMap loadMap() { - return IntakeTestMap.createRobotMap(); + return FullMap.createRobotMap(); } /** diff --git a/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java b/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java index 50fe8bf1..20886492 100644 --- a/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java +++ b/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java @@ -1,24 +1,20 @@ package frc.team449._2022robot.climber; import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.simulation.ElevatorSim; -import edu.wpi.first.wpilibj2.command.PIDSubsystem; -import frc.team449.generalInterfaces.updatable.Updatable; -import frc.team449.other.Updater; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; import frc.team449.wrappers.WrappedMotor; +import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.NotNull; -public class ClimberArm extends PIDSubsystem { +public class ClimberArm extends ProfiledPIDSubsystem { private final WrappedMotor motor; private final ElevatorFeedforward feedforward; public ClimberArm( @NotNull WrappedMotor motor, - @NotNull PIDController controller, - //todo feedforward isn't needed right now, so maybe only add later? + @NotNull ProfiledPIDController controller, @NotNull ElevatorFeedforward feedforward) { super(controller); this.motor = motor; @@ -26,8 +22,8 @@ public ClimberArm( } @Override - protected void useOutput(double output, double setpoint) { - this.motor.setVoltage(output + feedforward.calculate(output)); + protected void useOutput(double output, TrapezoidProfile.State setpoint) { + this.motor.setVoltage(output + feedforward.calculate(setpoint.velocity)); } @Override @@ -35,6 +31,15 @@ protected double getMeasurement() { return this.motor.getPositionUnits(); } + @Log + public double getError() { + return this.getController().getPositionError(); + } + + public void stop() { + this.getController().reset(0); + } + /** * Directly set the velocity. Only for testing/debugging */ @@ -42,23 +47,4 @@ protected double getMeasurement() { public void set(double velocity) { this.motor.set(velocity); } - - public static class ClimberArmSim extends ClimberArm implements Updatable { - private final ElevatorSim sim; - - public ClimberArmSim( - @NotNull WrappedMotor motor, - @NotNull PIDController controller, - @NotNull ElevatorFeedforward feedforward, - double distanceTopBottom) { - super(motor, controller, feedforward); - this.sim = new ElevatorSim(DCMotor.getNeo550(1), 1.0, 1.0, 1.0, 0.0, distanceTopBottom); - Updater.subscribe(this); - } - - @Override - public void update() { - this.sim.setInput(RobotController.getBatteryVoltage()); - } - } } diff --git a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java index 921ab0aa..612748b1 100644 --- a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java +++ b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java @@ -1,5 +1,6 @@ package frc.team449._2022robot.climber; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.shuffleboard.EventImportance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -8,6 +9,8 @@ import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.NotNull; +import java.util.function.BooleanSupplier; + public class PivotingTelescopingClimber extends SubsystemBase implements Loggable { /** Distance the climber can travel, in meters */ public final double distanceTopBottom; @@ -15,48 +18,63 @@ public class PivotingTelescopingClimber extends SubsystemBase implements Loggabl private @NotNull ClimberState state; private final @NotNull ClimberArm leftArm; private final @NotNull DoubleSolenoid pivotPiston; - /** The current setpoint, which constantly moves */ - private double setpoint; + private final @NotNull BooleanSupplier hallSensor; + /** The current goal */ + private double goal; + /** + * + * @param leftArm Climber's left arm + * @param rightArm Climber's right arm + * @param pivotPiston Piston used to pivot climber arms + * @param hallSensor The hall sensor that detects whether or not one climber arm's reached the bottom + * @param distanceTopBottom How much the climber arms extend up + */ public PivotingTelescopingClimber( - @NotNull ClimberArm leftArm, @NotNull ClimberArm rightArm, DoubleSolenoid pivotPiston, double distanceTopBottom) { + @NotNull ClimberArm leftArm, + @NotNull ClimberArm rightArm, + @NotNull DoubleSolenoid pivotPiston, + @NotNull BooleanSupplier hallSensor, + double distanceTopBottom) { this.leftArm = leftArm; this.rightArm = rightArm; this.pivotPiston = pivotPiston; this.distanceTopBottom = distanceTopBottom; + this.hallSensor = hallSensor; // Start arm retracted this.state = ClimberState.RETRACTED; - this.setpoint = 0; + this.goal = 0; } @Log.ToString + @NotNull public ClimberState getState() { return state; } - public void setState(ClimberState state) { + public void setState(@NotNull ClimberState state) { this.state = state; } @Log - public double getSetpoint() { - return setpoint; + public double getGoal() { + return goal; } - public void setSetpoint(double setpoint) { - if (setpoint < 0) { - setpoint = 0; + public void setGoal(double goal) { + if (goal < 0) { + goal = 0; Shuffleboard.addEventMarker( - "Tried setting climber setpoint below 0, clipping", EventImportance.kLow); - } else if (setpoint > this.distanceTopBottom) { - setpoint = this.distanceTopBottom; + "Tried setting climber goal below 0, clipping", EventImportance.kLow); + } else if (goal > this.distanceTopBottom) { + goal = this.distanceTopBottom; Shuffleboard.addEventMarker( - "Tried setting climber setpoint above " + distanceTopBottom + ", clipping", + "Tried setting climber goal above " + distanceTopBottom + ", clipping", EventImportance.kLow); } - this.setpoint = setpoint; - this.leftArm.setSetpoint(setpoint); - this.rightArm.setSetpoint(setpoint); + this.goal = goal; + this.leftArm.setGoal(goal); + this.rightArm.setGoal(goal); } public void pivotTelescopingArmOut() { @@ -84,6 +102,20 @@ public void disable() { this.rightArm.disable(); } + public void stop() { + this.leftArm.stop(); + this.rightArm.stop(); + } + + public boolean onTarget() { + return this.leftArm.getController().atGoal() && this.rightArm.getController().atGoal(); + } + + /** Whether or not the hall sensor is on, meaning one arm hit the bottom */ + public boolean hitBottom() { + return hallSensor.getAsBoolean(); + } + public enum ClimberState { EXTENDED, RETRACTED, diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 44871220..f750cbf1 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -12,6 +13,7 @@ import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; @@ -22,10 +24,7 @@ import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.team449.CommandContainer; import frc.team449.RobotMap; @@ -73,21 +72,7 @@ public class FullMap { SPITTER_PORT = 9, RIGHT_CLIMBER_MOTOR_PORT = 6, LEFT_CLIMBER_MOTOR_PORT = 5; - /* - public int chVEL = 1; - public void changeVel() { - if (chVEL == 1) { - DRIVE_KP_VEL = 0.0001; - chVEL = 2; - } - else { - DRIVE_KP_VEL = 0.001; - chVEL = 1; - } - } - - */ // Other CAN IDs public static final int PDP_CAN = 1, PCM_MODULE = 0; // Controller ports @@ -103,6 +88,14 @@ public void changeVel() { public static final double DRIVE_WHEEL_RADIUS = Units.inchesToMeters(2); public static final double DRIVE_GEARING = 5.86; public static final int DRIVE_CURRENT_LIM = 50; + public static final double DRIVE_KP_VEL = 27.2, + DRIVE_KI_VEL = 0.0, + DRIVE_KD_VEL = 0, + DRIVE_KP_POS = 45.269, + DRIVE_KD_POS = 3264.2, + DRIVE_FF_KS = 0.15084, + DRIVE_FF_KV = 2.4303, + DRIVE_FF_KA = 0.5323; // old value from measuring from the outside of the wheel: 0.6492875 // measuring from the inside of the wheel : .57785 public static final double DRIVE_TRACK_WIDTH = 0.6492875; @@ -111,21 +104,19 @@ public void changeVel() { public static final double MASS = 60; // Climber public static final int CLIMBER_PISTON_FWD_CHANNEL = 0, CLIMBER_PISTON_REV_CHANNEL = 1; + // todo find out what the channel numbers are + public static final int CLIMBER_SENSOR_CHANNEL = 0; // todo find out what this really is + public static final double CLIMBER_MAX_VEL = 0.1, CLIMBER_MAX_ACCEL = 0.1; + public static final double CLIMBER_DISTANCE = 0.5; + public static final double CLIMBER_KP = 12; + public static final double CLIMBER_FF_KS = 0, + CLIMBER_FF_KV = 0, + CLIMBER_FF_KA = 0, + CLIMBER_FF_KG = 0; // Intake public static final int INTAKE_PISTON_FWD_CHANNEL = 2, INTAKE_PISTON_REV_CHANNEL = 3; // todo find out what the channel numbers are - // Other constants - public static final double CLIMBER_DISTANCE = 0.5; - public static final double DRIVE_KP_VEL = 27.2, - DRIVE_KI_VEL = 0.0, - DRIVE_KD_VEL = 0, - DRIVE_KP_POS = 45.269, - DRIVE_KD_POS = 3264.2, - DRIVE_FF_KS = 0.15084, - DRIVE_FF_KV = 2.4303, - DRIVE_FF_KA = 0.5323; -//if changeVEL being used change from constant to changable variable private FullMap() {} @NotNull @@ -220,7 +211,7 @@ public static RobotMap createRobotMap() { .axis(XboxController.Axis.kRightTrigger.value) .inverted(false) .build())), - new RampComponent(.7, .50)); + new RampComponent(.6, .50)); var oi = new OIArcadeWithDPad( rotThrottle, @@ -275,11 +266,6 @@ public static RobotMap createRobotMap() { INTAKE_PISTON_REV_CHANNEL), INTAKE_SPEED, SPITTER_SPEED); - Supplier runIntake = - () -> - new InstantCommand(cargo::runIntake, cargo) - .andThen(new WaitCommand(3)) - .andThen(cargo::stop); Supplier spit = () -> new InstantCommand(cargo::spit, cargo).andThen(new WaitCommand(2)).andThen(cargo::stop); @@ -301,8 +287,12 @@ public static RobotMap createRobotMap() { .setPostEncoderGearing(10) .setReverseOutput(true) .createReal(), - new PIDController(12, 0, 0), - new ElevatorFeedforward(0, 0, 0)); + new ProfiledPIDController( + CLIMBER_KP, + 0, + 0, + new TrapezoidProfile.Constraints(CLIMBER_MAX_VEL, CLIMBER_MAX_ACCEL)), + new ElevatorFeedforward(CLIMBER_FF_KS, CLIMBER_FF_KG, CLIMBER_FF_KV, CLIMBER_FF_KA)); var rightArm = new ClimberArm( driveMasterPrototype @@ -312,15 +302,25 @@ public static RobotMap createRobotMap() { .setPostEncoderGearing(10) .setReverseOutput(false) .createReal(), - new PIDController(12, 0, 0), - new ElevatorFeedforward(0, 0, 0)); + new ProfiledPIDController( + CLIMBER_KP, + 0, + 0, + new TrapezoidProfile.Constraints(CLIMBER_MAX_VEL, CLIMBER_MAX_ACCEL)), + new ElevatorFeedforward(CLIMBER_FF_KS, CLIMBER_FF_KG, CLIMBER_FF_KV, CLIMBER_FF_KA)); var pivotPiston = new DoubleSolenoid( PCM_MODULE, PneumaticsModuleType.CTREPCM, CLIMBER_PISTON_FWD_CHANNEL, CLIMBER_PISTON_REV_CHANNEL); - var climber = new PivotingTelescopingClimber(leftArm, rightArm, pivotPiston, CLIMBER_DISTANCE); + var climber = + new PivotingTelescopingClimber( + leftArm, + rightArm, + pivotPiston, + RobotBase.isReal() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get : () -> false, //todo this is janky af + CLIMBER_DISTANCE); // PUT YOUR SUBSYSTEM IN HERE AFTER INITIALIZING IT var subsystems = List.of(drive, cargo, climber); @@ -349,14 +349,28 @@ public static RobotMap createRobotMap() { // Move climber arm up new JoystickButton(climberJoystick, XboxController.Button.kA.value) - .whileActiveContinuous( - new WaitCommand(0.01) - .andThen(() -> climber.setSetpoint(climber.getSetpoint() + 0.01), climber)); + .whenPressed( + new InstantCommand(() -> climber.setGoal(climber.distanceTopBottom), climber) + .andThen(new WaitUntilCommand(climber::onTarget))); + // .whileActiveContinuous( + // new WaitCommand(0.01) + // .andThen(() -> climber.setGoal(climber.getGoal() + 0.01), climber)); // Move climber arm down new JoystickButton(climberJoystick, XboxController.Button.kY.value) - .whileActiveContinuous( - new WaitCommand(0.01) - .andThen(() -> climber.setSetpoint(climber.getSetpoint() - 0.01), climber)); + .whenPressed( + new InstantCommand(() -> climber.setGoal(0), climber) + .andThen(new WaitUntilCommand(climber::onTarget)) + .withInterrupt(climber::hitBottom) + .andThen( + () -> { + if (climber.hitBottom()) { + climber.stop(); + climber.setState(PivotingTelescopingClimber.ClimberState.RETRACTED); + } + })); + // .whileActiveContinuous( + // new WaitCommand(0.01) + // .andThen(() -> climber.setGoal(climber.getGoal() - 0.01), climber)); // Extend climber arm out new JoystickButton(climberJoystick, XboxController.Button.kB.value) .whenPressed(climber::pivotTelescopingArmOut); @@ -384,13 +398,6 @@ public static RobotMap createRobotMap() { .build()) .angleTimeout(4) .field(field); - // - // var sCurve = - // spit.get() - // .andThen(cargo::runIntake, cargo) - // - // .andThen(ramsetePrototype.copy().name("scurvetraj").traj(sCurveTraj(drive)).build()) - // .andThen(spit.get()); // (assume blue alliance) // Start at bottom next to hub, shoot preloaded ball, then get the two balls in that region and @@ -474,19 +481,6 @@ public static RobotMap createRobotMap() { return new RobotMap(subsystems, pdp, allCommands, false); } - /** Generate a trajectory for the S-shaped curve we're using to test */ - @NotNull - private static Trajectory sCurveTraj(@NotNull DriveUnidirectionalWithGyro drive) { - var ballPos = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(0)); - var fwdTraj = - TrajectoryGenerator.generateTrajectory( - new Pose2d(), List.of(), ballPos, trajConfig(drive).setReversed(false)); - var revTraj = - TrajectoryGenerator.generateTrajectory( - ballPos, List.of(), new Pose2d(), trajConfig(drive).setReversed(true)); - return fwdTraj.concatenate(revTraj); - } - private static Trajectory testTraj(@NotNull DriveUnidirectionalWithGyro drive) { var ballPos = new Translation2d(2.5, 4); var speedConstraint = @@ -535,12 +529,6 @@ private static Trajectory loadPathPlannerTraj(@NotNull String trajName) { return traj; } - @NotNull - private static Trajectory emptyTraj( - @NotNull DriveUnidirectionalWithGyro drive, @NotNull Pose2d pose) { - return TrajectoryGenerator.generateTrajectory(pose, List.of(), pose, trajConfig(drive)); - } - /** Create a command that immediately spits a ball, then goes to pick up a ball and comes back */ private static Command oneThenOneBallTraj( @NotNull DriveUnidirectionalWithGyro drive, From c35f08cba3ed1695626b5e68b47b81bae2e5c35d Mon Sep 17 00:00:00 2001 From: Teddy Date: Sat, 5 Mar 2022 12:21:39 -0500 Subject: [PATCH 03/25] Deploy intake before pushing out climber arms --- build.gradle | 2 +- .../java/frc/team449/javaMaps/FullMap.java | 37 ++++++++++++++----- 2 files changed, 28 insertions(+), 11 deletions(-) diff --git a/build.gradle b/build.gradle index a8f5cd0b..68d9f6e6 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ plugins { id 'java' id 'idea' - id "edu.wpi.first.GradleRIO" version "2022.3.1" + id "edu.wpi.first.GradleRIO" version "2022.4.1" } def ROBOT_MAIN_CLASS = "frc.team449.Main" diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index f750cbf1..48b83b89 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -121,7 +121,8 @@ private FullMap() {} @NotNull public static RobotMap createRobotMap() { - var pdp = new PDP(PDP_CAN, new RunningLinRegComponent(250, 0.75), PowerDistribution.ModuleType.kCTRE); + var pdp = + new PDP(PDP_CAN, new RunningLinRegComponent(250, 0.75), PowerDistribution.ModuleType.kCTRE); var cargoJoystick = new RumbleableJoystick(CARGO_JOYSTICK_PORT); var driveJoystick = new RumbleableJoystick(DRIVE_JOYSTICK_PORT); var climberJoystick = new RumbleableJoystick(CLIMBER_JOYSTICK_PORT); @@ -319,7 +320,9 @@ public static RobotMap createRobotMap() { leftArm, rightArm, pivotPiston, - RobotBase.isReal() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get : () -> false, //todo this is janky af + RobotBase.isReal() + ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get + : () -> false, // todo this is janky af CLIMBER_DISTANCE); // PUT YOUR SUBSYSTEM IN HERE AFTER INITIALIZING IT @@ -344,8 +347,8 @@ public static RobotMap createRobotMap() { .whenPressed(cargo::deployIntake); // Run intake in reverse to feed ball from top new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) - .whileHeld(cargo::runIntakeReverse, cargo) - .whenReleased(cargo::stop, cargo); + .whileHeld(cargo::runIntakeReverse, cargo) + .whenReleased(cargo::stop, cargo); // Move climber arm up new JoystickButton(climberJoystick, XboxController.Button.kA.value) @@ -360,7 +363,7 @@ public static RobotMap createRobotMap() { .whenPressed( new InstantCommand(() -> climber.setGoal(0), climber) .andThen(new WaitUntilCommand(climber::onTarget)) - .withInterrupt(climber::hitBottom) + .until(climber::hitBottom) .andThen( () -> { if (climber.hitBottom()) { @@ -373,10 +376,12 @@ public static RobotMap createRobotMap() { // .andThen(() -> climber.setGoal(climber.getGoal() - 0.01), climber)); // Extend climber arm out new JoystickButton(climberJoystick, XboxController.Button.kB.value) - .whenPressed(climber::pivotTelescopingArmOut); + .whenPressed( + // Deploy intake first so it doesn't get in the way + new InstantCommand(cargo::deployIntake).andThen(climber::pivotTelescopingArmOut)); // Retract climber arm in with piston new JoystickButton(climberJoystick, XboxController.Button.kX.value) - .whenPressed(climber::pivotTelescopingArmIn); + .whenPressed(climber::pivotTelescopingArmIn); var ramsetePrototype = new RamseteBuilder() @@ -415,13 +420,25 @@ public static RobotMap createRobotMap() { // Spit the preloaded ball, pick up another, come back and spit it out var topOneOneTraj = oneThenOneBallTraj( - drive, cargo, ramsetePrototype.name("topOneOne"), pose(7.11, 4.80, 159.25), pose(5.39, 5.91, 137.86)); + drive, + cargo, + ramsetePrototype.name("topOneOne"), + pose(7.11, 4.80, 159.25), + pose(5.39, 5.91, 137.86)); var midOneOneTraj = oneThenOneBallTraj( - drive, cargo, ramsetePrototype.name("midOneOne"), pose(7.56, 3.00, -111.80), pose(5.58, 2.00, -173.42)); + drive, + cargo, + ramsetePrototype.name("midOneOne"), + pose(7.56, 3.00, -111.80), + pose(5.58, 2.00, -173.42)); var bottomOneOneTraj = oneThenOneBallTraj( - drive, cargo, ramsetePrototype.name("bottomOneOne"), pose(8.01, 2.82, -111.80), pose(7.67, 0.77, -91.97)); + drive, + cargo, + ramsetePrototype.name("bottomOneOne"), + pose(8.01, 2.82, -111.80), + pose(7.67, 0.77, -91.97)); // Start at the edge at the top, collect the top ball, then come back and spit var topTwoBallTraj = From 059b8d8a7b619ce7b9755083a564162dfc5dd2e8 Mon Sep 17 00:00:00 2001 From: Teddy Date: Sat, 5 Mar 2022 13:01:40 -0500 Subject: [PATCH 04/25] Delete some unnecessary classes, move classes around --- .../team449/_2022robot/cargo/Cargo2022.java | 15 +-- .../_2022robot/climber/ClimberArm.java | 2 +- .../climber/PivotingTelescopingClimber.java | 8 +- .../frc/team449/{wrappers => ahrs}/AHRS.java | 8 +- .../{wrappers/simulated => ahrs}/AHRSSim.java | 41 ++++--- .../commands}/RamseteBuilder.java | 2 +- .../components/AHRSRumbleComponent.java | 2 +- .../DriveSettings.java | 2 +- .../DriveSettingsBuilder.java | 5 +- .../DriveUnidirectionalBase.java | 4 +- .../DriveUnidirectionalWithGyro.java | 6 +- .../DriveUnidirectionalWithGyroSim.java | 6 +- .../java/frc/team449/javaMaps/FullMap.java | 14 +-- .../frc/team449/javaMaps/IntakeTestMap.java | 10 +- .../team449/{wrappers => motor}/Encoder.java | 3 +- .../{wrappers => motor}/WrappedMotor.java | 4 +- .../builder}/MotorConfig.java | 10 +- .../builder}/SparkMaxConfig.java | 6 +- .../builder}/TalonConfig.java | 6 +- .../sim}/DummyMotorController.java | 2 +- .../sim}/SimulatedEncoder.java | 12 +- .../sim}/SimulatedMotor.java | 2 +- .../throttles}/ThrottlePolynomialBuilder.java | 3 +- .../frc/team449/other/DefaultCommand.java | 39 ------- .../java/frc/team449/other/FollowerUtils.java | 11 +- src/main/java/frc/team449/other/SimUtil.java | 103 ------------------ .../builders => other}/UsbCameraCreator.java | 2 +- .../builders => other}/VictorCreator.java | 10 +- src/main/java/frc/team449/other/Waypoint.java | 79 -------------- .../{simulated => }/JoystickSimulated.java | 4 +- .../team449/wrappers/RumbleableJoystick.java | 1 - .../frc/team449/wrappers/SimulationMode.java | 16 --- 32 files changed, 92 insertions(+), 346 deletions(-) rename src/main/java/frc/team449/{wrappers => ahrs}/AHRS.java (93%) rename src/main/java/frc/team449/{wrappers/simulated => ahrs}/AHRSSim.java (63%) rename src/main/java/frc/team449/{javaMaps/builders => auto/commands}/RamseteBuilder.java (99%) rename src/main/java/frc/team449/{generalInterfaces => drive}/DriveSettings.java (98%) rename src/main/java/frc/team449/{javaMaps/builders => drive}/DriveSettingsBuilder.java (97%) rename src/main/java/frc/team449/{wrappers => motor}/Encoder.java (99%) rename src/main/java/frc/team449/{wrappers => motor}/WrappedMotor.java (95%) rename src/main/java/frc/team449/{javaMaps/builders => motor/builder}/MotorConfig.java (96%) rename src/main/java/frc/team449/{javaMaps/builders => motor/builder}/SparkMaxConfig.java (97%) rename src/main/java/frc/team449/{javaMaps/builders => motor/builder}/TalonConfig.java (98%) rename src/main/java/frc/team449/{wrappers/simulated => motor/sim}/DummyMotorController.java (91%) rename src/main/java/frc/team449/{wrappers/simulated => motor/sim}/SimulatedEncoder.java (72%) rename src/main/java/frc/team449/{wrappers/simulated => motor/sim}/SimulatedMotor.java (98%) rename src/main/java/frc/team449/{javaMaps/builders => oi/throttles}/ThrottlePolynomialBuilder.java (94%) delete mode 100644 src/main/java/frc/team449/other/DefaultCommand.java delete mode 100644 src/main/java/frc/team449/other/SimUtil.java rename src/main/java/frc/team449/{javaMaps/builders => other}/UsbCameraCreator.java (97%) rename src/main/java/frc/team449/{javaMaps/builders => other}/VictorCreator.java (86%) delete mode 100644 src/main/java/frc/team449/other/Waypoint.java rename src/main/java/frc/team449/wrappers/{simulated => }/JoystickSimulated.java (99%) delete mode 100644 src/main/java/frc/team449/wrappers/SimulationMode.java diff --git a/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java b/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java index 9a837e6b..aa1fd90a 100644 --- a/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java +++ b/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.jetbrains.annotations.NotNull; public class Cargo2022 extends SubsystemBase { /** The leader motor for the intake */ @@ -17,9 +18,9 @@ public class Cargo2022 extends SubsystemBase { private final double spitterSpeed; public Cargo2022( - MotorController intakeMotor, - MotorController spitterMotor, - DoubleSolenoid deployIntake, + @NotNull MotorController intakeMotor, + @NotNull MotorController spitterMotor, + @NotNull DoubleSolenoid deployIntake, double intakeSpeed, double spitterSpeed) { this.intakeMotor = intakeMotor; @@ -35,8 +36,8 @@ public void runIntake() { } public void runIntakeReverse() { - intakeMotor.set(intakeSpeed); - spitterMotor.set(spitterSpeed); + intakeMotor.set(-intakeSpeed); + spitterMotor.set(-spitterSpeed); } public void spit() { @@ -50,10 +51,10 @@ public void stop() { } public void deployIntake() { - deployIntake.set(DoubleSolenoid.Value.kForward); + deployIntake.set(DoubleSolenoid.Value.kReverse); } public void retractIntake() { - deployIntake.set(DoubleSolenoid.Value.kReverse); + deployIntake.set(DoubleSolenoid.Value.kForward); } } diff --git a/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java b/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java index 20886492..9ee7c34e 100644 --- a/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java +++ b/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; -import frc.team449.wrappers.WrappedMotor; +import frc.team449.motor.WrappedMotor; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java index 612748b1..d82fe9cd 100644 --- a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java +++ b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java @@ -1,6 +1,5 @@ package frc.team449._2022robot.climber; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.shuffleboard.EventImportance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -14,6 +13,7 @@ public class PivotingTelescopingClimber extends SubsystemBase implements Loggable { /** Distance the climber can travel, in meters */ public final double distanceTopBottom; + private final @NotNull ClimberArm rightArm; private @NotNull ClimberState state; private final @NotNull ClimberArm leftArm; @@ -23,11 +23,11 @@ public class PivotingTelescopingClimber extends SubsystemBase implements Loggabl private double goal; /** - * * @param leftArm Climber's left arm * @param rightArm Climber's right arm * @param pivotPiston Piston used to pivot climber arms - * @param hallSensor The hall sensor that detects whether or not one climber arm's reached the bottom + * @param hallSensor The hall sensor that detects whether or not one climber arm's reached the + * bottom * @param distanceTopBottom How much the climber arms extend up */ public PivotingTelescopingClimber( @@ -107,7 +107,7 @@ public void stop() { this.rightArm.stop(); } - public boolean onTarget() { + public boolean atGoal() { return this.leftArm.getController().atGoal() && this.rightArm.getController().atGoal(); } diff --git a/src/main/java/frc/team449/wrappers/AHRS.java b/src/main/java/frc/team449/ahrs/AHRS.java similarity index 93% rename from src/main/java/frc/team449/wrappers/AHRS.java rename to src/main/java/frc/team449/ahrs/AHRS.java index dc878116..7e244f8a 100644 --- a/src/main/java/frc/team449/wrappers/AHRS.java +++ b/src/main/java/frc/team449/ahrs/AHRS.java @@ -1,15 +1,10 @@ -package frc.team449.wrappers; +package frc.team449.ahrs; -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonIdentityInfo; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.annotation.ObjectIdGenerators; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SerialPort; import frc.team449.generalInterfaces.updatable.Updatable; import frc.team449.other.Updater; -import frc.team449.wrappers.simulated.AHRSSim; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.Contract; @@ -18,7 +13,6 @@ import static com.kauailabs.navx.frc.AHRS.SerialDataType.kProcessedData; /** An invertible wrapper for the NavX. */ -@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public class AHRS implements Updatable, Loggable { /** The AHRS this class is a wrapper on. */ diff --git a/src/main/java/frc/team449/wrappers/simulated/AHRSSim.java b/src/main/java/frc/team449/ahrs/AHRSSim.java similarity index 63% rename from src/main/java/frc/team449/wrappers/simulated/AHRSSim.java rename to src/main/java/frc/team449/ahrs/AHRSSim.java index aefb54aa..20b10103 100644 --- a/src/main/java/frc/team449/wrappers/simulated/AHRSSim.java +++ b/src/main/java/frc/team449/ahrs/AHRSSim.java @@ -1,16 +1,15 @@ -package frc.team449.wrappers.simulated; +package frc.team449.ahrs; import edu.wpi.first.hal.SimBoolean; import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.hal.simulation.SimDeviceDataJNI; import edu.wpi.first.wpilibj.SerialPort; -import frc.team449.wrappers.AHRS; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; import org.jetbrains.annotations.NotNull; /** A simulated AHRS. See https://pdocs.kauailabs.com/navx-mxp/software/roborio-libraries/java/ */ public class AHRSSim extends AHRS { /** The default name used for the simulated NavX (probably the one you want */ - public static final String DEFAULT_NAVX_NAME = "navX-Sensor[0]"; + public static final String DEFAULT_NAVX_NAME = "navX-Sensor"; private final SimBoolean isConnected; private final SimDouble yaw; @@ -31,37 +30,35 @@ public class AHRSSim extends AHRS { * @param devName The name of the simulated device. If you don't know what this should be, try * {@link AHRSSim#AHRSSim(SerialPort.Port, boolean)}, where it defaults to {@link * AHRSSim#DEFAULT_NAVX_NAME} + * @param index The NavX index. If you don't know what this should be, try {@link + * AHRSSim#AHRSSim(SerialPort.Port, boolean)}, where it defaults to 0 */ - public AHRSSim(SerialPort.Port port, boolean invertYaw, @NotNull String devName) { + public AHRSSim(SerialPort.Port port, boolean invertYaw, @NotNull String devName, int index) { super(port, invertYaw); - int devHandle = SimDeviceDataJNI.getSimDeviceHandle(devName); - this.isConnected = new SimBoolean(SimDeviceDataJNI.getSimValueHandle(devHandle, "Connected")); - this.yaw = new SimDouble(SimDeviceDataJNI.getSimValueHandle(devHandle, "Yaw")); - this.pitch = new SimDouble(SimDeviceDataJNI.getSimValueHandle(devHandle, "Pitch")); - this.roll = new SimDouble(SimDeviceDataJNI.getSimValueHandle(devHandle, "Roll")); - this.compassHeading = - new SimDouble(SimDeviceDataJNI.getSimValueHandle(devHandle, "CompassHeading")); - this.fusedHeading = - new SimDouble(SimDeviceDataJNI.getSimValueHandle(devHandle, "FusedHeading")); - this.linearWorldAccelX = - new SimDouble(SimDeviceDataJNI.getSimValueHandle(devHandle, "LinearWorldAccelX")); - this.linearWorldAccelY = - new SimDouble(SimDeviceDataJNI.getSimValueHandle(devHandle, "LinearWorldAccelY")); - this.linearWorldAccelZ = - new SimDouble(SimDeviceDataJNI.getSimValueHandle(devHandle, "LinearWorldAccelZ")); + + var deviceSim = new SimDeviceSim(devName, index); + this.isConnected = deviceSim.getBoolean("Connected"); + this.yaw = deviceSim.getDouble("Yaw"); + this.pitch = deviceSim.getDouble("Pitch"); + this.roll = deviceSim.getDouble("Roll"); + this.compassHeading = deviceSim.getDouble("CompassHeading"); + this.fusedHeading = deviceSim.getDouble("FusedHeading"); + this.linearWorldAccelX = deviceSim.getDouble("LinearWorldAccelX"); + this.linearWorldAccelY = deviceSim.getDouble("LinearWorldAccelY"); + this.linearWorldAccelZ = deviceSim.getDouble("LinearWorldAccelZ"); setHeading(0); } /** * Alternative constructor where the device name is taken to be {@link AHRSSim#DEFAULT_NAVX_NAME}, - * which is probably what you want + * and the index is taken to be 0, which is probably what we want * * @param port The port the NavX is plugged into. It seems like only kMXP (the port on the RIO) * * works. * @param invertYaw Whether or not to invert the yaw axis. Defaults to true. */ public AHRSSim(SerialPort.Port port, boolean invertYaw) { - this(port, invertYaw, DEFAULT_NAVX_NAME); + this(port, invertYaw, DEFAULT_NAVX_NAME, 0); } @Override diff --git a/src/main/java/frc/team449/javaMaps/builders/RamseteBuilder.java b/src/main/java/frc/team449/auto/commands/RamseteBuilder.java similarity index 99% rename from src/main/java/frc/team449/javaMaps/builders/RamseteBuilder.java rename to src/main/java/frc/team449/auto/commands/RamseteBuilder.java index 4a7c7b98..0128bec6 100644 --- a/src/main/java/frc/team449/javaMaps/builders/RamseteBuilder.java +++ b/src/main/java/frc/team449/auto/commands/RamseteBuilder.java @@ -1,4 +1,4 @@ -package frc.team449.javaMaps.builders; +package frc.team449.auto.commands; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.RamseteController; diff --git a/src/main/java/frc/team449/components/AHRSRumbleComponent.java b/src/main/java/frc/team449/components/AHRSRumbleComponent.java index e1707e80..a2a566c1 100644 --- a/src/main/java/frc/team449/components/AHRSRumbleComponent.java +++ b/src/main/java/frc/team449/components/AHRSRumbleComponent.java @@ -5,7 +5,7 @@ import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.annotation.ObjectIdGenerators; import frc.team449.generalInterfaces.rumbleable.Rumbleable; -import frc.team449.wrappers.AHRS; +import frc.team449.ahrs.AHRS; import frc.team449.other.Clock; import java.util.List; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/generalInterfaces/DriveSettings.java b/src/main/java/frc/team449/drive/DriveSettings.java similarity index 98% rename from src/main/java/frc/team449/generalInterfaces/DriveSettings.java rename to src/main/java/frc/team449/drive/DriveSettings.java index b36f1688..ba95b42f 100644 --- a/src/main/java/frc/team449/generalInterfaces/DriveSettings.java +++ b/src/main/java/frc/team449/drive/DriveSettings.java @@ -1,4 +1,4 @@ -package frc.team449.generalInterfaces; +package frc.team449.drive; import com.fasterxml.jackson.annotation.JsonCreator; import edu.wpi.first.math.controller.PIDController; diff --git a/src/main/java/frc/team449/javaMaps/builders/DriveSettingsBuilder.java b/src/main/java/frc/team449/drive/DriveSettingsBuilder.java similarity index 97% rename from src/main/java/frc/team449/javaMaps/builders/DriveSettingsBuilder.java rename to src/main/java/frc/team449/drive/DriveSettingsBuilder.java index 6fc07ea7..4916a93e 100644 --- a/src/main/java/frc/team449/javaMaps/builders/DriveSettingsBuilder.java +++ b/src/main/java/frc/team449/drive/DriveSettingsBuilder.java @@ -1,8 +1,8 @@ -package frc.team449.javaMaps.builders; +package frc.team449.drive; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import frc.team449.generalInterfaces.DriveSettings; +import org.jetbrains.annotations.NotNull; public final class DriveSettingsBuilder { private Double fwdPeakOutputVoltage; @@ -37,6 +37,7 @@ public DriveSettingsBuilder copy() { return copy; } + @NotNull public DriveSettings build() { return new DriveSettings( feedforward, diff --git a/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalBase.java b/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalBase.java index 92439fc7..1926d8ea 100644 --- a/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalBase.java +++ b/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalBase.java @@ -2,9 +2,9 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.team449.generalInterfaces.DriveSettings; +import frc.team449.drive.DriveSettings; import frc.team449.other.Updater; -import frc.team449.wrappers.WrappedMotor; +import frc.team449.motor.WrappedMotor; import io.github.oblarg.oblog.Loggable; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyro.java b/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyro.java index c3e2fd21..a304d147 100644 --- a/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyro.java +++ b/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyro.java @@ -9,10 +9,10 @@ import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; import edu.wpi.first.wpilibj.simulation.EncoderSim; -import frc.team449.generalInterfaces.DriveSettings; +import frc.team449.drive.DriveSettings; import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; -import frc.team449.wrappers.AHRS; -import frc.team449.wrappers.WrappedMotor; +import frc.team449.ahrs.AHRS; +import frc.team449.motor.WrappedMotor; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyroSim.java b/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyroSim.java index a2ec42e7..799c83bf 100644 --- a/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyroSim.java +++ b/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyroSim.java @@ -5,11 +5,11 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; import edu.wpi.first.wpilibj.simulation.EncoderSim; -import frc.team449.generalInterfaces.DriveSettings; +import frc.team449.drive.DriveSettings; import frc.team449.generalInterfaces.updatable.Updatable; import frc.team449.other.Clock; -import frc.team449.wrappers.AHRS; -import frc.team449.wrappers.WrappedMotor; +import frc.team449.ahrs.AHRS; +import frc.team449.motor.WrappedMotor; import org.jetbrains.annotations.NotNull; public class DriveUnidirectionalWithGyroSim extends DriveUnidirectionalWithGyro implements Updatable { diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 48b83b89..ee0abe97 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -31,24 +31,24 @@ import frc.team449._2022robot.cargo.Cargo2022; import frc.team449._2022robot.climber.ClimberArm; import frc.team449._2022robot.climber.PivotingTelescopingClimber; +import frc.team449.ahrs.AHRS; import frc.team449.ahrs.PIDAngleControllerBuilder; +import frc.team449.auto.commands.RamseteBuilder; import frc.team449.components.RunningLinRegComponent; +import frc.team449.drive.DriveSettingsBuilder; import frc.team449.drive.unidirectional.DriveUnidirectionalWithGyro; import frc.team449.drive.unidirectional.commands.UnidirectionalNavXDefaultDrive; import frc.team449.generalInterfaces.doubleUnaryOperator.Polynomial; import frc.team449.generalInterfaces.doubleUnaryOperator.RampComponent; import frc.team449.generalInterfaces.limelight.Limelight; -import frc.team449.javaMaps.builders.DriveSettingsBuilder; -import frc.team449.javaMaps.builders.RamseteBuilder; -import frc.team449.javaMaps.builders.SparkMaxConfig; -import frc.team449.javaMaps.builders.ThrottlePolynomialBuilder; +import frc.team449.motor.builder.SparkMaxConfig; +import frc.team449.oi.throttles.ThrottlePolynomialBuilder; import frc.team449.oi.throttles.ThrottleSum; import frc.team449.oi.throttles.ThrottleWithRamp; import frc.team449.oi.unidirectional.arcade.OIArcadeWithDPad; import frc.team449.other.Debouncer; import frc.team449.other.FollowerUtils; import frc.team449.other.Updater; -import frc.team449.wrappers.AHRS; import frc.team449.wrappers.PDP; import frc.team449.wrappers.RumbleableJoystick; import org.jetbrains.annotations.Contract; @@ -354,7 +354,7 @@ public static RobotMap createRobotMap() { new JoystickButton(climberJoystick, XboxController.Button.kA.value) .whenPressed( new InstantCommand(() -> climber.setGoal(climber.distanceTopBottom), climber) - .andThen(new WaitUntilCommand(climber::onTarget))); + .andThen(new WaitUntilCommand(climber::atGoal))); // .whileActiveContinuous( // new WaitCommand(0.01) // .andThen(() -> climber.setGoal(climber.getGoal() + 0.01), climber)); @@ -362,7 +362,7 @@ public static RobotMap createRobotMap() { new JoystickButton(climberJoystick, XboxController.Button.kY.value) .whenPressed( new InstantCommand(() -> climber.setGoal(0), climber) - .andThen(new WaitUntilCommand(climber::onTarget)) + .andThen(new WaitUntilCommand(climber::atGoal)) .until(climber::hitBottom) .andThen( () -> { diff --git a/src/main/java/frc/team449/javaMaps/IntakeTestMap.java b/src/main/java/frc/team449/javaMaps/IntakeTestMap.java index 92f2b350..f66619d3 100644 --- a/src/main/java/frc/team449/javaMaps/IntakeTestMap.java +++ b/src/main/java/frc/team449/javaMaps/IntakeTestMap.java @@ -30,10 +30,10 @@ import frc.team449.generalInterfaces.doubleUnaryOperator.Polynomial; import frc.team449.generalInterfaces.doubleUnaryOperator.RampComponent; import frc.team449.generalInterfaces.limelight.Limelight; -import frc.team449.javaMaps.builders.DriveSettingsBuilder; -import frc.team449.javaMaps.builders.RamseteBuilder; -import frc.team449.javaMaps.builders.SparkMaxConfig; -import frc.team449.javaMaps.builders.ThrottlePolynomialBuilder; +import frc.team449.drive.DriveSettingsBuilder; +import frc.team449.auto.commands.RamseteBuilder; +import frc.team449.motor.builder.SparkMaxConfig; +import frc.team449.oi.throttles.ThrottlePolynomialBuilder; import frc.team449.oi.buttons.SimpleButton; import frc.team449.oi.throttles.ThrottleSum; import frc.team449.oi.throttles.ThrottleWithRamp; @@ -41,7 +41,7 @@ import frc.team449.other.Debouncer; import frc.team449.other.FollowerUtils; import frc.team449.other.Updater; -import frc.team449.wrappers.AHRS; +import frc.team449.ahrs.AHRS; import frc.team449.wrappers.PDP; import frc.team449.wrappers.RumbleableJoystick; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/wrappers/Encoder.java b/src/main/java/frc/team449/motor/Encoder.java similarity index 99% rename from src/main/java/frc/team449/wrappers/Encoder.java rename to src/main/java/frc/team449/motor/Encoder.java index 2a97be47..71389812 100644 --- a/src/main/java/frc/team449/wrappers/Encoder.java +++ b/src/main/java/frc/team449/motor/Encoder.java @@ -1,8 +1,7 @@ -package frc.team449.wrappers; +package frc.team449.motor; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.revrobotics.RelativeEncoder; -import edu.wpi.first.wpilibj.Timer; import frc.team449.other.Clock; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; diff --git a/src/main/java/frc/team449/wrappers/WrappedMotor.java b/src/main/java/frc/team449/motor/WrappedMotor.java similarity index 95% rename from src/main/java/frc/team449/wrappers/WrappedMotor.java rename to src/main/java/frc/team449/motor/WrappedMotor.java index c0966811..9782a67d 100644 --- a/src/main/java/frc/team449/wrappers/WrappedMotor.java +++ b/src/main/java/frc/team449/motor/WrappedMotor.java @@ -1,8 +1,8 @@ -package frc.team449.wrappers; +package frc.team449.motor; -import com.revrobotics.CANSparkMax; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import frc.team449.generalInterfaces.MotorContainer; +import frc.team449.motor.Encoder; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/javaMaps/builders/MotorConfig.java b/src/main/java/frc/team449/motor/builder/MotorConfig.java similarity index 96% rename from src/main/java/frc/team449/javaMaps/builders/MotorConfig.java rename to src/main/java/frc/team449/motor/builder/MotorConfig.java index 0f0a52a6..7618a1ba 100644 --- a/src/main/java/frc/team449/javaMaps/builders/MotorConfig.java +++ b/src/main/java/frc/team449/motor/builder/MotorConfig.java @@ -1,11 +1,11 @@ -package frc.team449.javaMaps.builders; +package frc.team449.motor.builder; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.simulation.EncoderSim; -import frc.team449.wrappers.WrappedMotor; -import frc.team449.wrappers.simulated.DummyMotorController; -import frc.team449.wrappers.simulated.SimulatedEncoder; +import frc.team449.motor.WrappedMotor; +import frc.team449.motor.sim.DummyMotorController; +import frc.team449.motor.sim.SimulatedEncoder; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; @@ -18,7 +18,7 @@ *

todo find a better way to make the subclass the return type than F-bounds * * @param The type of the "current" subclass of {@link MotorConfig} - * @see frc.team449.wrappers.WrappedMotor + * @see WrappedMotor */ @SuppressWarnings({"unchecked", "UnusedReturnValue"}) public abstract class MotorConfig> { diff --git a/src/main/java/frc/team449/javaMaps/builders/SparkMaxConfig.java b/src/main/java/frc/team449/motor/builder/SparkMaxConfig.java similarity index 97% rename from src/main/java/frc/team449/javaMaps/builders/SparkMaxConfig.java rename to src/main/java/frc/team449/motor/builder/SparkMaxConfig.java index 4ad36719..4ef15b83 100644 --- a/src/main/java/frc/team449/javaMaps/builders/SparkMaxConfig.java +++ b/src/main/java/frc/team449/motor/builder/SparkMaxConfig.java @@ -1,12 +1,12 @@ -package frc.team449.javaMaps.builders; +package frc.team449.motor.builder; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel; import com.revrobotics.REVLibError; import com.revrobotics.SparkMaxLimitSwitch; import frc.team449.other.FollowerUtils; -import frc.team449.wrappers.Encoder; -import frc.team449.wrappers.WrappedMotor; +import frc.team449.motor.Encoder; +import frc.team449.motor.WrappedMotor; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; diff --git a/src/main/java/frc/team449/javaMaps/builders/TalonConfig.java b/src/main/java/frc/team449/motor/builder/TalonConfig.java similarity index 98% rename from src/main/java/frc/team449/javaMaps/builders/TalonConfig.java rename to src/main/java/frc/team449/motor/builder/TalonConfig.java index 91e8306b..831f3234 100644 --- a/src/main/java/frc/team449/javaMaps/builders/TalonConfig.java +++ b/src/main/java/frc/team449/motor/builder/TalonConfig.java @@ -1,4 +1,4 @@ -package frc.team449.javaMaps.builders; +package frc.team449.motor.builder; import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.can.TalonSRX; @@ -6,8 +6,8 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod; import frc.team449.other.FollowerUtils; -import frc.team449.wrappers.Encoder; -import frc.team449.wrappers.WrappedMotor; +import frc.team449.motor.Encoder; +import frc.team449.motor.WrappedMotor; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; diff --git a/src/main/java/frc/team449/wrappers/simulated/DummyMotorController.java b/src/main/java/frc/team449/motor/sim/DummyMotorController.java similarity index 91% rename from src/main/java/frc/team449/wrappers/simulated/DummyMotorController.java rename to src/main/java/frc/team449/motor/sim/DummyMotorController.java index 4e0b864a..47f59600 100644 --- a/src/main/java/frc/team449/wrappers/simulated/DummyMotorController.java +++ b/src/main/java/frc/team449/motor/sim/DummyMotorController.java @@ -1,4 +1,4 @@ -package frc.team449.wrappers.simulated; +package frc.team449.motor.sim; import edu.wpi.first.wpilibj.motorcontrol.MotorController; diff --git a/src/main/java/frc/team449/wrappers/simulated/SimulatedEncoder.java b/src/main/java/frc/team449/motor/sim/SimulatedEncoder.java similarity index 72% rename from src/main/java/frc/team449/wrappers/simulated/SimulatedEncoder.java rename to src/main/java/frc/team449/motor/sim/SimulatedEncoder.java index 91415323..061ed268 100644 --- a/src/main/java/frc/team449/wrappers/simulated/SimulatedEncoder.java +++ b/src/main/java/frc/team449/motor/sim/SimulatedEncoder.java @@ -1,8 +1,7 @@ -package frc.team449.wrappers.simulated; +package frc.team449.motor.sim; -import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; import edu.wpi.first.wpilibj.simulation.EncoderSim; -import frc.team449.wrappers.Encoder; +import frc.team449.motor.Encoder; import org.jetbrains.annotations.NotNull; public final class SimulatedEncoder extends Encoder { @@ -18,13 +17,6 @@ public SimulatedEncoder( this.encSim = encSim; } - public SimulatedEncoder( - @NotNull String name, - @NotNull EncoderSim encSim, - @NotNull DifferentialDrivetrainSim driveSim) { - this(name, encSim, 1, 1, 1); - } - @Override public void resetPosition() { encSim.setReset(true); diff --git a/src/main/java/frc/team449/wrappers/simulated/SimulatedMotor.java b/src/main/java/frc/team449/motor/sim/SimulatedMotor.java similarity index 98% rename from src/main/java/frc/team449/wrappers/simulated/SimulatedMotor.java rename to src/main/java/frc/team449/motor/sim/SimulatedMotor.java index 1587a6c2..5a2f7e30 100644 --- a/src/main/java/frc/team449/wrappers/simulated/SimulatedMotor.java +++ b/src/main/java/frc/team449/motor/sim/SimulatedMotor.java @@ -1,4 +1,4 @@ -package frc.team449.wrappers.simulated; +package frc.team449.motor.sim; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; diff --git a/src/main/java/frc/team449/javaMaps/builders/ThrottlePolynomialBuilder.java b/src/main/java/frc/team449/oi/throttles/ThrottlePolynomialBuilder.java similarity index 94% rename from src/main/java/frc/team449/javaMaps/builders/ThrottlePolynomialBuilder.java rename to src/main/java/frc/team449/oi/throttles/ThrottlePolynomialBuilder.java index 8c1a4ef9..edb0b362 100644 --- a/src/main/java/frc/team449/javaMaps/builders/ThrottlePolynomialBuilder.java +++ b/src/main/java/frc/team449/oi/throttles/ThrottlePolynomialBuilder.java @@ -1,8 +1,7 @@ -package frc.team449.javaMaps.builders; +package frc.team449.oi.throttles; import edu.wpi.first.wpilibj.GenericHID; import frc.team449.generalInterfaces.doubleUnaryOperator.Polynomial; -import frc.team449.oi.throttles.ThrottlePolynomial; import org.jetbrains.annotations.NotNull; public class ThrottlePolynomialBuilder { diff --git a/src/main/java/frc/team449/other/DefaultCommand.java b/src/main/java/frc/team449/other/DefaultCommand.java deleted file mode 100644 index 5e06c8e1..00000000 --- a/src/main/java/frc/team449/other/DefaultCommand.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.team449.other; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonProperty; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; -import io.github.oblarg.oblog.Loggable; -import io.github.oblarg.oblog.annotations.Log; -import org.jetbrains.annotations.NotNull; - -/** A class that sets the default command for a subsystem when constructed. */ -public class DefaultCommand implements Loggable { - - /** The command of this defaultCommand; field to allow logging */ - @Log.Include private final Command command; - - /** - * Sets the given command as the default command for the given subsystem. - * - * @param subsystem The subsystem to set the default command for. - * @param command The command to set as the default. - */ - @JsonCreator - public DefaultCommand( - @NotNull @JsonProperty(required = true) final Subsystem subsystem, - @NotNull @JsonProperty(required = true) final Command command) { - // Check if it's an instant command and warn the user if it is - if (InstantCommand.class.isAssignableFrom(command.getClass())) { - System.out.println( - "You're trying to set an InstantCommand as a default command! This is a really bad " - + "idea!"); - System.out.println("Subsystem: " + subsystem.getClass().toString()); - System.out.println("Command: " + command.getClass().toString()); - } - subsystem.setDefaultCommand(command); - this.command = command; - } -} diff --git a/src/main/java/frc/team449/other/FollowerUtils.java b/src/main/java/frc/team449/other/FollowerUtils.java index 3dae605d..9439cb4e 100644 --- a/src/main/java/frc/team449/other/FollowerUtils.java +++ b/src/main/java/frc/team449/other/FollowerUtils.java @@ -3,7 +3,6 @@ import com.ctre.phoenix.motorcontrol.*; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.VictorSPX; -import com.fasterxml.jackson.annotation.JsonCreator; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel; import com.revrobotics.SparkMaxLimitSwitch; @@ -14,6 +13,7 @@ public final class FollowerUtils { private FollowerUtils() {} /** Create a Spark that will follow another Spark */ + @NotNull public static CANSparkMax createFollowerSpark(int port) { var slaveSpark = new CANSparkMax(port, CANSparkMaxLowLevel.MotorType.kBrushless); @@ -31,7 +31,8 @@ public static CANSparkMax createFollowerSpark(int port) { return slaveSpark; } - /** Make a Spark follow another Spark + /** + * Make a Spark follow another Spark * * @param follower The follower * @param leader The leader @@ -53,8 +54,8 @@ public static void setMasterForSpark( * @param invertType Whether or not to invert this Talon. Defaults to FollowMaster , but can be * changed to OpposeMaster. */ - @JsonCreator - public static TalonSRX createFollowerTalon(int port, InvertType invertType) { + @NotNull + public static TalonSRX createFollowerTalon(int port, @Nullable InvertType invertType) { var talonSRX = new TalonSRX(port); // Turn off features we don't want a slave to have @@ -136,7 +137,7 @@ public static void setMasterForTalon( * @param invertType Whether to invert this relative to the master. Defaults to not inverting * relative to master. */ - @JsonCreator + @NotNull public static VictorSPX createFollowerVictor(int port, InvertType invertType) { var victorSPX = new VictorSPX(port); victorSPX.setInverted(invertType == null ? InvertType.FollowMaster : invertType); diff --git a/src/main/java/frc/team449/other/SimUtil.java b/src/main/java/frc/team449/other/SimUtil.java deleted file mode 100644 index f48e73fa..00000000 --- a/src/main/java/frc/team449/other/SimUtil.java +++ /dev/null @@ -1,103 +0,0 @@ -package frc.team449.other; - -import edu.wpi.first.hal.HALValue; -import edu.wpi.first.hal.SimValue; -import java.util.Objects; -import java.util.function.Supplier; -import org.jetbrains.annotations.Contract; -import org.jetbrains.annotations.NotNull; -import org.jetbrains.annotations.Nullable; - -/** Helpers for using the {@link edu.wpi.first.hal.SimDevice} framework. */ -public class SimUtil { - /** - * Gets the value for a property based on either a simulation variable or the real implementation. - * - * @param useSimValue whether to use the value provided by {@code simValue} instead of that - * provided by {@code regularImplementation} - * @param setSimValue whether to set the value of {@code simValue} to the returned value when - * using {@code regularImplementation} - * @param simValue the simulation variable that appears as an input in the WPILib simulation GUI - * @param regularImplementation a reference to the regular getter of the property - * @param The type of the property. Used to cast the value obtained from {@code simValue}. - * @return a value for the property obtained from either a simulation input or the return value of - * the actual getter - */ - @Contract("true, _, null, _ -> fail") - public static T getWithSimHelper( - final boolean useSimValue, - final boolean setSimValue, - @Nullable final SimValue simValue, - @NotNull final Supplier regularImplementation) { - final T result; - - if (useSimValue) { - Objects.requireNonNull(simValue); - //noinspection unchecked - result = (T) unwrapHALValue(simValue.getValue()); - } else { - result = regularImplementation.get(); - if (simValue != null && setSimValue) simValue.setValue(makeHALValue(result)); - } - - return result; - } - - /** - * Gets the underlying value of the specified {@link HALValue} instance. - * - * @param value the object to retrieve the value from - * @return Depending on {@link HALValue#getType()}: - *

    - *
  • {@link HALValue#kUnassigned}: {@code null} - *
  • {@link HALValue#kBoolean}: {@link HALValue#getBoolean()} - *
  • {@link HALValue#kDouble}: {@link HALValue#getDouble()} - *
  • {@link HALValue#kInt}, {@link HALValue#kEnum}: {@code (int)}{@link - * HALValue#getLong()} - *
  • {@link HALValue#kLong}: {@link HALValue#getLong()} - *
- */ - @Nullable - private static Object unwrapHALValue(@NotNull final HALValue value) { - switch (value.getType()) { - case HALValue.kUnassigned: - return null; - case HALValue.kBoolean: - return value.getBoolean(); - case HALValue.kDouble: - return value.getDouble(); - case HALValue.kEnum: - case HALValue.kInt: - return (int) value.getLong(); - case HALValue.kLong: - return value.getLong(); - } - throw new IllegalStateException("value.getType() is invalid."); - } - - /** - * Creates an instance of {@link HALValue} from the specified value. - * - * @param value the object to retrieve the value from - * @return Depending on {@code value}: - *
    - *
  • {@code null}: {@link HALValue#makeUnassigned()} - *
  • {@link Boolean}: {@link HALValue#makeBoolean(boolean)} - *
  • {@link Double}: {@link HALValue#makeDouble(double)} - *
  • {@link Integer}: {@link HALValue#makeInt(int)} - *
  • {@link Enum}: {@link HALValue#makeInt(int)} with {@link Enum#ordinal()} - *
  • {@link Long}: {@link HALValue#makeLong(long)} - *
- */ - @NotNull - private static HALValue makeHALValue(@Nullable final Object value) { - if (value == null) return HALValue.makeUnassigned(); - if (Boolean.class.equals(value.getClass())) return HALValue.makeBoolean((Boolean) value); - if (Double.class.equals(value.getClass())) return HALValue.makeDouble((Double) value); - if (Integer.class.equals(value.getClass())) return HALValue.makeInt((Integer) value); - if (Enum.class.isAssignableFrom(value.getClass())) - return HALValue.makeEnum(((Enum) value).ordinal()); - if (Long.class.equals(value.getClass())) return HALValue.makeLong((Long) value); - throw new IllegalArgumentException("value must be Boolean, Double, Integer, Enum, or Long."); - } -} diff --git a/src/main/java/frc/team449/javaMaps/builders/UsbCameraCreator.java b/src/main/java/frc/team449/other/UsbCameraCreator.java similarity index 97% rename from src/main/java/frc/team449/javaMaps/builders/UsbCameraCreator.java rename to src/main/java/frc/team449/other/UsbCameraCreator.java index 98b9b2a4..2ac4d604 100644 --- a/src/main/java/frc/team449/javaMaps/builders/UsbCameraCreator.java +++ b/src/main/java/frc/team449/other/UsbCameraCreator.java @@ -1,4 +1,4 @@ -package frc.team449.javaMaps.builders; +package frc.team449.other; import edu.wpi.first.cscore.UsbCamera; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/javaMaps/builders/VictorCreator.java b/src/main/java/frc/team449/other/VictorCreator.java similarity index 86% rename from src/main/java/frc/team449/javaMaps/builders/VictorCreator.java rename to src/main/java/frc/team449/other/VictorCreator.java index ed855609..28d2cd32 100644 --- a/src/main/java/frc/team449/javaMaps/builders/VictorCreator.java +++ b/src/main/java/frc/team449/other/VictorCreator.java @@ -1,29 +1,29 @@ -package frc.team449.javaMaps.builders; +package frc.team449.other; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.VictorSPX; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; -import com.fasterxml.jackson.annotation.JsonIdentityInfo; -import com.fasterxml.jackson.annotation.ObjectIdGenerators; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.motorcontrol.VictorSP; import frc.team449.other.FollowerUtils; +import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; import java.util.List; -//todo turn this into a builder like SparkMaxConfig +// todo turn this into a builder like SparkMaxConfig /** A helper static class to create {@link VictorSP}'s and {@link WPI_VictorSPX}'s. */ -@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) public final class VictorCreator { private VictorCreator() {} + @NotNull public static MotorController createVictorSP(int port, boolean inverted) { var victorSP = new VictorSP(port); victorSP.setInverted(inverted); return victorSP; } + @NotNull public static MotorController createVictorSPX( int port, boolean brakeMode, diff --git a/src/main/java/frc/team449/other/Waypoint.java b/src/main/java/frc/team449/other/Waypoint.java deleted file mode 100644 index c6cb959e..00000000 --- a/src/main/java/frc/team449/other/Waypoint.java +++ /dev/null @@ -1,79 +0,0 @@ -package frc.team449.other; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonProperty; - -/** A waypoint to hit during a motion profile. */ -public class Waypoint { - - /** The X position to hit, in meters. */ - private double x; - - /** The Y position to hit, in meters. */ - private double y; - - /** The angle, in degrees, for the robot to be at when it arrives at (x, y). */ - private double theta; - - /** - * Default constructor. - * - * @param x The X position to hit, in meters. - * @param y The Y position to hit, in meters. - * @param theta The angle, in degrees, for the robot to be at when it arrives at (x, y). - */ - @JsonCreator - public Waypoint( - @JsonProperty(required = true) double x, - @JsonProperty(required = true) double y, - @JsonProperty(required = true) double theta) { - this.x = x; - this.y = y; - this.theta = theta; - } - - /** @return The X position to hit, in meters. */ - public double getX() { - return x; - } - - /** @param x The X position to hit, in meters. */ - public void setX(double x) { - this.x = x; - } - - /** @return The Y position to hit, in meters. */ - public double getY() { - return y; - } - - /** @param y The X position to hit, in meters. */ - public void setY(double y) { - this.y = y; - } - - /** @return The angle, in radians, for the robot to be at when it arrives at (x, y). */ - public double getThetaRadians() { - return Math.toRadians(theta); - } - - /** @param theta The angle, in radians, for the robot to be at when it arrives at (x, y). */ - public void setThetaRadians(double theta) { - this.theta = Math.toDegrees(theta); - } - - /** @return The angle, in degrees, for the robot to be at when it arrives at (x, y). */ - public double getThetaDegrees() { - return theta; - } - - /** @param theta The angle, in degrees, for the robot to be at when it arrives at (x, y). */ - public void setThetaDegrees(double theta) { - this.theta = theta; - } - - @Override - public String toString() { - return "X: " + getX() + ", Y: " + getY() + ", Theta: " + getThetaDegrees(); - } -} diff --git a/src/main/java/frc/team449/wrappers/simulated/JoystickSimulated.java b/src/main/java/frc/team449/wrappers/JoystickSimulated.java similarity index 99% rename from src/main/java/frc/team449/wrappers/simulated/JoystickSimulated.java rename to src/main/java/frc/team449/wrappers/JoystickSimulated.java index ab2267c9..91756931 100644 --- a/src/main/java/frc/team449/wrappers/simulated/JoystickSimulated.java +++ b/src/main/java/frc/team449/wrappers/JoystickSimulated.java @@ -1,7 +1,7 @@ -package frc.team449.wrappers.simulated; +package frc.team449.wrappers; import frc.team449.Robot; -import frc.team449.wrappers.RumbleableJoystick; + import java.awt.*; import java.awt.event.KeyEvent; import java.util.HashMap; diff --git a/src/main/java/frc/team449/wrappers/RumbleableJoystick.java b/src/main/java/frc/team449/wrappers/RumbleableJoystick.java index c5392c70..8f5b13f5 100644 --- a/src/main/java/frc/team449/wrappers/RumbleableJoystick.java +++ b/src/main/java/frc/team449/wrappers/RumbleableJoystick.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.RobotBase; import frc.team449.generalInterfaces.rumbleable.Rumbleable; -import frc.team449.wrappers.simulated.JoystickSimulated; import static frc.team449.other.Util.getLogPrefix; diff --git a/src/main/java/frc/team449/wrappers/SimulationMode.java b/src/main/java/frc/team449/wrappers/SimulationMode.java deleted file mode 100644 index 20e21f22..00000000 --- a/src/main/java/frc/team449/wrappers/SimulationMode.java +++ /dev/null @@ -1,16 +0,0 @@ -package frc.team449.wrappers; - -/** When to construct simulated hardware component instances. */ -public enum SimulationMode { - /** Simulate all instances of the hardware component. */ - ALWAYS, - /** Simulate all instances of the hardware component when the robot runs in a simulation. */ - WHEN_SIMULATING, - /** - * Attempt to determine whether the hardware component physically exists and only simulate - * components that can be proven not to exist. - */ - WHEN_NECESSARY, - /** Never simulate the hardware component. */ - NEVER -} From 659511f143acba90508eaf149368c9490a3642a2 Mon Sep 17 00:00:00 2001 From: Teddy Date: Sat, 5 Mar 2022 13:15:36 -0500 Subject: [PATCH 05/25] Make intake have two solenoids --- .../team449/_2022robot/cargo/Cargo2022.java | 22 ++++++++++++++----- .../java/frc/team449/javaMaps/FullMap.java | 14 +++++++++--- .../frc/team449/javaMaps/IntakeTestMap.java | 17 +++++++++----- 3 files changed, 39 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java b/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java index aa1fd90a..cd0f18f9 100644 --- a/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java +++ b/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java @@ -10,8 +10,10 @@ public class Cargo2022 extends SubsystemBase { public final MotorController intakeMotor; /** The top motor that lets balls be spit out */ public final MotorController spitterMotor; - /** Piston used to extend and retract intake */ - private final DoubleSolenoid deployIntake; + /** Piston used to extend and retract left part of intake */ + private final DoubleSolenoid deployIntakeLeft; + /** Piston used to extend and retract right part of intake */ + private final DoubleSolenoid deployIntakeRight; /** The speed when intaking */ private final double intakeSpeed; /** The speed of the spitter motor when spitting */ @@ -20,12 +22,14 @@ public class Cargo2022 extends SubsystemBase { public Cargo2022( @NotNull MotorController intakeMotor, @NotNull MotorController spitterMotor, - @NotNull DoubleSolenoid deployIntake, + @NotNull DoubleSolenoid deployIntakeLeft, + @NotNull DoubleSolenoid deployIntakeRight, double intakeSpeed, double spitterSpeed) { this.intakeMotor = intakeMotor; this.spitterMotor = spitterMotor; - this.deployIntake = deployIntake; + this.deployIntakeLeft = deployIntakeLeft; + this.deployIntakeRight = deployIntakeRight; this.intakeSpeed = intakeSpeed; this.spitterSpeed = spitterSpeed; } @@ -51,10 +55,16 @@ public void stop() { } public void deployIntake() { - deployIntake.set(DoubleSolenoid.Value.kReverse); + this.setIntake(DoubleSolenoid.Value.kReverse); } public void retractIntake() { - deployIntake.set(DoubleSolenoid.Value.kForward); + this.setIntake(DoubleSolenoid.Value.kForward); + } + + /** Set both intake solenoids to the same value to either extend or retract */ + private void setIntake(DoubleSolenoid.Value value) { + deployIntakeLeft.set(value); + deployIntakeRight.set(value); } } diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index ee0abe97..0429891b 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -114,7 +114,10 @@ public class FullMap { CLIMBER_FF_KA = 0, CLIMBER_FF_KG = 0; // Intake - public static final int INTAKE_PISTON_FWD_CHANNEL = 2, INTAKE_PISTON_REV_CHANNEL = 3; + public static final int INTAKE_PISTON_LEFT_FWD_CHANNEL = 2, + INTAKE_PISTON_LEFT_REV_CHANNEL = 3, + INTAKE_PISTON_RIGHT_FWD_CHANNEL = 2, + INTAKE_PISTON_RIGHT_REV_CHANNEL = 3; // todo find out what the channel numbers are private FullMap() {} @@ -263,8 +266,13 @@ public static RobotMap createRobotMap() { new DoubleSolenoid( PCM_MODULE, PneumaticsModuleType.CTREPCM, - INTAKE_PISTON_FWD_CHANNEL, - INTAKE_PISTON_REV_CHANNEL), + INTAKE_PISTON_LEFT_FWD_CHANNEL, + INTAKE_PISTON_LEFT_REV_CHANNEL), + new DoubleSolenoid( + PCM_MODULE, + PneumaticsModuleType.CTREPCM, + INTAKE_PISTON_RIGHT_FWD_CHANNEL, + INTAKE_PISTON_RIGHT_REV_CHANNEL), INTAKE_SPEED, SPITTER_SPEED); Supplier spit = diff --git a/src/main/java/frc/team449/javaMaps/IntakeTestMap.java b/src/main/java/frc/team449/javaMaps/IntakeTestMap.java index f66619d3..29168367 100644 --- a/src/main/java/frc/team449/javaMaps/IntakeTestMap.java +++ b/src/main/java/frc/team449/javaMaps/IntakeTestMap.java @@ -51,6 +51,8 @@ import java.util.Set; import java.util.function.Supplier; +import static frc.team449.javaMaps.FullMap.*; + public class IntakeTestMap { // Motor IDs public static final int RIGHT_LEADER_PORT = 1, @@ -235,11 +237,16 @@ public static RobotMap createRobotMap() { .setPort(SPITTER_PORT) .setEnableBrakeMode(false) .createReal(), - new DoubleSolenoid( - FullMap.PCM_MODULE, - PneumaticsModuleType.CTREPCM, - FullMap.INTAKE_PISTON_FWD_CHANNEL, - FullMap.INTAKE_PISTON_REV_CHANNEL), + new DoubleSolenoid( + PCM_MODULE, + PneumaticsModuleType.CTREPCM, + INTAKE_PISTON_LEFT_FWD_CHANNEL, + INTAKE_PISTON_LEFT_REV_CHANNEL), + new DoubleSolenoid( + PCM_MODULE, + PneumaticsModuleType.CTREPCM, + INTAKE_PISTON_RIGHT_FWD_CHANNEL, + INTAKE_PISTON_RIGHT_REV_CHANNEL), INTAKE_SPEED, SPITTER_SPEED); Supplier runIntake = From 6873566223398ff547b913451a9300f9f5116364 Mon Sep 17 00:00:00 2001 From: Teddy Date: Sat, 5 Mar 2022 13:24:37 -0500 Subject: [PATCH 06/25] Move ahrs stuff out of genIfaces.ahrs and into ahrs --- .../ahrs/commands => ahrs}/OverrideNavX.java | 3 +-- .../{generalInterfaces/ahrs/commands => ahrs}/SetHeading.java | 3 +-- .../team449/{generalInterfaces => }/ahrs/SubsystemAHRS.java | 2 +- .../ahrs/commands => ahrs}/ToggleOverrideNavX.java | 3 +-- .../drive/unidirectional/DriveUnidirectionalWithGyro.java | 2 +- .../drive/unidirectional/commands/AHRS/NavXDriveStraight.java | 2 +- .../drive/unidirectional/commands/AHRS/NavXTurnToAngle.java | 2 +- .../unidirectional/commands/AHRS/NavXTurnToAngleLimelight.java | 2 +- .../unidirectional/commands/AHRS/NavXTurnToAngleRelative.java | 2 +- .../commands/FieldOrientedUnidirectionalDriveCommand.java | 2 +- .../commands/UnidirectionalNavXDefaultDrive.java | 2 +- 11 files changed, 11 insertions(+), 14 deletions(-) rename src/main/java/frc/team449/{generalInterfaces/ahrs/commands => ahrs}/OverrideNavX.java (94%) rename src/main/java/frc/team449/{generalInterfaces/ahrs/commands => ahrs}/SetHeading.java (94%) rename src/main/java/frc/team449/{generalInterfaces => }/ahrs/SubsystemAHRS.java (97%) rename src/main/java/frc/team449/{generalInterfaces/ahrs/commands => ahrs}/ToggleOverrideNavX.java (94%) diff --git a/src/main/java/frc/team449/generalInterfaces/ahrs/commands/OverrideNavX.java b/src/main/java/frc/team449/ahrs/OverrideNavX.java similarity index 94% rename from src/main/java/frc/team449/generalInterfaces/ahrs/commands/OverrideNavX.java rename to src/main/java/frc/team449/ahrs/OverrideNavX.java index 67023c38..3f3aa205 100644 --- a/src/main/java/frc/team449/generalInterfaces/ahrs/commands/OverrideNavX.java +++ b/src/main/java/frc/team449/ahrs/OverrideNavX.java @@ -1,4 +1,4 @@ -package frc.team449.generalInterfaces.ahrs.commands; +package frc.team449.ahrs; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIdentityInfo; @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj.shuffleboard.EventImportance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/generalInterfaces/ahrs/commands/SetHeading.java b/src/main/java/frc/team449/ahrs/SetHeading.java similarity index 94% rename from src/main/java/frc/team449/generalInterfaces/ahrs/commands/SetHeading.java rename to src/main/java/frc/team449/ahrs/SetHeading.java index 33605e2b..7466c2fa 100644 --- a/src/main/java/frc/team449/generalInterfaces/ahrs/commands/SetHeading.java +++ b/src/main/java/frc/team449/ahrs/SetHeading.java @@ -1,4 +1,4 @@ -package frc.team449.generalInterfaces.ahrs.commands; +package frc.team449.ahrs; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIdentityInfo; @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj.shuffleboard.EventImportance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/generalInterfaces/ahrs/SubsystemAHRS.java b/src/main/java/frc/team449/ahrs/SubsystemAHRS.java similarity index 97% rename from src/main/java/frc/team449/generalInterfaces/ahrs/SubsystemAHRS.java rename to src/main/java/frc/team449/ahrs/SubsystemAHRS.java index 794ea2f8..f1991bd8 100644 --- a/src/main/java/frc/team449/generalInterfaces/ahrs/SubsystemAHRS.java +++ b/src/main/java/frc/team449/ahrs/SubsystemAHRS.java @@ -1,4 +1,4 @@ -package frc.team449.generalInterfaces.ahrs; +package frc.team449.ahrs; import com.fasterxml.jackson.annotation.JsonTypeInfo; diff --git a/src/main/java/frc/team449/generalInterfaces/ahrs/commands/ToggleOverrideNavX.java b/src/main/java/frc/team449/ahrs/ToggleOverrideNavX.java similarity index 94% rename from src/main/java/frc/team449/generalInterfaces/ahrs/commands/ToggleOverrideNavX.java rename to src/main/java/frc/team449/ahrs/ToggleOverrideNavX.java index 4ade51f8..ad61ce24 100644 --- a/src/main/java/frc/team449/generalInterfaces/ahrs/commands/ToggleOverrideNavX.java +++ b/src/main/java/frc/team449/ahrs/ToggleOverrideNavX.java @@ -1,4 +1,4 @@ -package frc.team449.generalInterfaces.ahrs.commands; +package frc.team449.ahrs; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIdentityInfo; @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj.shuffleboard.EventImportance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyro.java b/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyro.java index a304d147..560d6882 100644 --- a/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyro.java +++ b/src/main/java/frc/team449/drive/unidirectional/DriveUnidirectionalWithGyro.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; import edu.wpi.first.wpilibj.simulation.EncoderSim; import frc.team449.drive.DriveSettings; -import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; +import frc.team449.ahrs.SubsystemAHRS; import frc.team449.ahrs.AHRS; import frc.team449.motor.WrappedMotor; import io.github.oblarg.oblog.annotations.Log; diff --git a/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXDriveStraight.java b/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXDriveStraight.java index 1ad0c615..9c7b2695 100644 --- a/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXDriveStraight.java +++ b/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXDriveStraight.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import frc.team449.ahrs.PIDAngleController; import frc.team449.drive.unidirectional.DriveUnidirectional; -import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; +import frc.team449.ahrs.SubsystemAHRS; import frc.team449.oi.unidirectional.tank.OITank; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngle.java b/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngle.java index 8595a699..6e354d27 100644 --- a/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngle.java +++ b/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngle.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import frc.team449.ahrs.PIDAngleController; import frc.team449.drive.unidirectional.DriveUnidirectional; -import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; +import frc.team449.ahrs.SubsystemAHRS; import frc.team449.other.Clock; import frc.team449.other.Util; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngleLimelight.java b/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngleLimelight.java index 2b951259..8cbad771 100644 --- a/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngleLimelight.java +++ b/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngleLimelight.java @@ -6,7 +6,7 @@ import frc.team449.ahrs.PIDAngleController; import frc.team449.components.limelight.LimelightDistanceComponent; import frc.team449.drive.unidirectional.DriveUnidirectional; -import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; +import frc.team449.ahrs.SubsystemAHRS; import frc.team449.generalInterfaces.limelight.Limelight; import frc.team449.other.Clock; import frc.team449.other.Util; diff --git a/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngleRelative.java b/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngleRelative.java index f2993634..e787e220 100644 --- a/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngleRelative.java +++ b/src/main/java/frc/team449/drive/unidirectional/commands/AHRS/NavXTurnToAngleRelative.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import frc.team449.ahrs.PIDAngleController; import frc.team449.drive.unidirectional.DriveUnidirectional; -import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; +import frc.team449.ahrs.SubsystemAHRS; import frc.team449.other.Clock; import frc.team449.other.Util; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/drive/unidirectional/commands/FieldOrientedUnidirectionalDriveCommand.java b/src/main/java/frc/team449/drive/unidirectional/commands/FieldOrientedUnidirectionalDriveCommand.java index 2ebd07a0..8ca2a386 100644 --- a/src/main/java/frc/team449/drive/unidirectional/commands/FieldOrientedUnidirectionalDriveCommand.java +++ b/src/main/java/frc/team449/drive/unidirectional/commands/FieldOrientedUnidirectionalDriveCommand.java @@ -7,7 +7,7 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import frc.team449.ahrs.PIDAngleController; import frc.team449.drive.unidirectional.DriveUnidirectional; -import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; +import frc.team449.ahrs.SubsystemAHRS; import frc.team449.oi.fieldoriented.OIFieldOriented; import org.jetbrains.annotations.NotNull; diff --git a/src/main/java/frc/team449/drive/unidirectional/commands/UnidirectionalNavXDefaultDrive.java b/src/main/java/frc/team449/drive/unidirectional/commands/UnidirectionalNavXDefaultDrive.java index 688c9a6e..1c090e43 100644 --- a/src/main/java/frc/team449/drive/unidirectional/commands/UnidirectionalNavXDefaultDrive.java +++ b/src/main/java/frc/team449/drive/unidirectional/commands/UnidirectionalNavXDefaultDrive.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.Subsystem; import frc.team449.ahrs.PIDAngleController; import frc.team449.drive.unidirectional.DriveUnidirectional; -import frc.team449.generalInterfaces.ahrs.SubsystemAHRS; +import frc.team449.ahrs.SubsystemAHRS; import frc.team449.generalInterfaces.doubleUnaryOperator.RampComponent; import frc.team449.oi.unidirectional.OIUnidirectional; import frc.team449.other.Debouncer; From ed63cfdeb7ca2cf98c66d8481b029b70589c8a94 Mon Sep 17 00:00:00 2001 From: Teddy Date: Sat, 5 Mar 2022 13:29:59 -0500 Subject: [PATCH 07/25] More reorganizing --- src/main/java/frc/team449/RobotMap.java | 3 +-- src/main/java/frc/team449/generalInterfaces/SlaveMotor.java | 4 ---- .../team449/{generalInterfaces => motor}/MotorContainer.java | 2 +- src/main/java/frc/team449/motor/WrappedMotor.java | 2 -- 4 files changed, 2 insertions(+), 9 deletions(-) delete mode 100644 src/main/java/frc/team449/generalInterfaces/SlaveMotor.java rename src/main/java/frc/team449/{generalInterfaces => motor}/MotorContainer.java (96%) diff --git a/src/main/java/frc/team449/RobotMap.java b/src/main/java/frc/team449/RobotMap.java index 275fd7b8..c0bd839d 100644 --- a/src/main/java/frc/team449/RobotMap.java +++ b/src/main/java/frc/team449/RobotMap.java @@ -2,10 +2,9 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.team449.generalInterfaces.MotorContainer; +import frc.team449.motor.MotorContainer; import frc.team449.wrappers.PDP; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; diff --git a/src/main/java/frc/team449/generalInterfaces/SlaveMotor.java b/src/main/java/frc/team449/generalInterfaces/SlaveMotor.java deleted file mode 100644 index 28ccef52..00000000 --- a/src/main/java/frc/team449/generalInterfaces/SlaveMotor.java +++ /dev/null @@ -1,4 +0,0 @@ -package frc.team449.generalInterfaces; - -/** A marker interface for followers */ -public interface SlaveMotor {} diff --git a/src/main/java/frc/team449/generalInterfaces/MotorContainer.java b/src/main/java/frc/team449/motor/MotorContainer.java similarity index 96% rename from src/main/java/frc/team449/generalInterfaces/MotorContainer.java rename to src/main/java/frc/team449/motor/MotorContainer.java index 2db3de6d..f144172b 100644 --- a/src/main/java/frc/team449/generalInterfaces/MotorContainer.java +++ b/src/main/java/frc/team449/motor/MotorContainer.java @@ -1,4 +1,4 @@ -package frc.team449.generalInterfaces; +package frc.team449.motor; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.Logger; diff --git a/src/main/java/frc/team449/motor/WrappedMotor.java b/src/main/java/frc/team449/motor/WrappedMotor.java index 9782a67d..085ffeb8 100644 --- a/src/main/java/frc/team449/motor/WrappedMotor.java +++ b/src/main/java/frc/team449/motor/WrappedMotor.java @@ -1,8 +1,6 @@ package frc.team449.motor; import edu.wpi.first.wpilibj.motorcontrol.MotorController; -import frc.team449.generalInterfaces.MotorContainer; -import frc.team449.motor.Encoder; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.NotNull; From 94f0ab559e7d0caa0aa21a53da3e9c1bfa88aef5 Mon Sep 17 00:00:00 2001 From: Teddy Date: Sat, 5 Mar 2022 14:43:32 -0500 Subject: [PATCH 08/25] Let intake have only one solenoid, did stuffs --- .../team449/_2022robot/cargo/Cargo2022.java | 29 +++------ .../climber/PivotingTelescopingClimber.java | 5 +- .../java/frc/team449/javaMaps/FullMap.java | 38 +++++------ .../frc/team449/javaMaps/IntakeTestMap.java | 9 +-- .../AnalogMotorSingleSpeed.java | 51 --------------- .../multiSubsystem/SubsystemBinaryMotor.java | 23 ------- .../multiSubsystem/commands/LazyCommand.java | 48 -------------- .../commands/SetIntakeMode.java | 64 ------------------- .../commands/SetSolenoidPose.java | 28 -------- .../commands/ToggleSolenoid.java | 64 ------------------- .../multiSubsystem/commands/TurnMotorOff.java | 56 ---------------- .../multiSubsystem/commands/TurnMotorOn.java | 55 ---------------- 12 files changed, 35 insertions(+), 435 deletions(-) delete mode 100644 src/main/java/frc/team449/multiSubsystem/AnalogMotorSingleSpeed.java delete mode 100644 src/main/java/frc/team449/multiSubsystem/SubsystemBinaryMotor.java delete mode 100644 src/main/java/frc/team449/multiSubsystem/commands/LazyCommand.java delete mode 100644 src/main/java/frc/team449/multiSubsystem/commands/SetIntakeMode.java delete mode 100644 src/main/java/frc/team449/multiSubsystem/commands/SetSolenoidPose.java delete mode 100644 src/main/java/frc/team449/multiSubsystem/commands/ToggleSolenoid.java delete mode 100644 src/main/java/frc/team449/multiSubsystem/commands/TurnMotorOff.java delete mode 100644 src/main/java/frc/team449/multiSubsystem/commands/TurnMotorOn.java diff --git a/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java b/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java index cd0f18f9..dbf7ae5a 100644 --- a/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java +++ b/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java @@ -10,10 +10,8 @@ public class Cargo2022 extends SubsystemBase { public final MotorController intakeMotor; /** The top motor that lets balls be spit out */ public final MotorController spitterMotor; - /** Piston used to extend and retract left part of intake */ - private final DoubleSolenoid deployIntakeLeft; - /** Piston used to extend and retract right part of intake */ - private final DoubleSolenoid deployIntakeRight; + /** Piston used to extend and retract intake */ + private final DoubleSolenoid deployIntake; /** The speed when intaking */ private final double intakeSpeed; /** The speed of the spitter motor when spitting */ @@ -22,14 +20,12 @@ public class Cargo2022 extends SubsystemBase { public Cargo2022( @NotNull MotorController intakeMotor, @NotNull MotorController spitterMotor, - @NotNull DoubleSolenoid deployIntakeLeft, - @NotNull DoubleSolenoid deployIntakeRight, + @NotNull DoubleSolenoid deployIntake, double intakeSpeed, double spitterSpeed) { this.intakeMotor = intakeMotor; this.spitterMotor = spitterMotor; - this.deployIntakeLeft = deployIntakeLeft; - this.deployIntakeRight = deployIntakeRight; + this.deployIntake = deployIntake; this.intakeSpeed = intakeSpeed; this.spitterSpeed = spitterSpeed; } @@ -39,10 +35,10 @@ public void runIntake() { spitterMotor.set(-spitterSpeed); } - public void runIntakeReverse() { - intakeMotor.set(-intakeSpeed); - spitterMotor.set(-spitterSpeed); - } +// public void runIntakeReverse() { +// intakeMotor.set(-intakeSpeed); +// spitterMotor.set(-spitterSpeed); +// } public void spit() { intakeMotor.set(intakeSpeed); @@ -55,16 +51,11 @@ public void stop() { } public void deployIntake() { - this.setIntake(DoubleSolenoid.Value.kReverse); + this.deployIntake.set(DoubleSolenoid.Value.kReverse); } public void retractIntake() { - this.setIntake(DoubleSolenoid.Value.kForward); + this.deployIntake.set(DoubleSolenoid.Value.kForward); } - /** Set both intake solenoids to the same value to either extend or retract */ - private void setIntake(DoubleSolenoid.Value value) { - deployIntakeLeft.set(value); - deployIntakeRight.set(value); - } } diff --git a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java index d82fe9cd..57c44a4f 100644 --- a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java +++ b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj.shuffleboard.EventImportance; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.team449.multiSubsystem.BooleanSupplierUpdatable; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.NotNull; @@ -18,7 +19,7 @@ public class PivotingTelescopingClimber extends SubsystemBase implements Loggabl private @NotNull ClimberState state; private final @NotNull ClimberArm leftArm; private final @NotNull DoubleSolenoid pivotPiston; - private final @NotNull BooleanSupplier hallSensor; + private final @NotNull BooleanSupplierUpdatable hallSensor; /** The current goal */ private double goal; @@ -40,7 +41,7 @@ public PivotingTelescopingClimber( this.rightArm = rightArm; this.pivotPiston = pivotPiston; this.distanceTopBottom = distanceTopBottom; - this.hallSensor = hallSensor; + this.hallSensor = new BooleanSupplierUpdatable(hallSensor, null); // Start arm retracted this.state = ClimberState.RETRACTED; this.goal = 0; diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 0429891b..4a7d61f8 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -103,7 +103,7 @@ public class FullMap { public static final double MOMENT_OF_INERTIA = 7.5; public static final double MASS = 60; // Climber - public static final int CLIMBER_PISTON_FWD_CHANNEL = 0, CLIMBER_PISTON_REV_CHANNEL = 1; + public static final int CLIMBER_PISTON_FWD_CHANNEL = 4, CLIMBER_PISTON_REV_CHANNEL = 5; // todo find out what the channel numbers are public static final int CLIMBER_SENSOR_CHANNEL = 0; // todo find out what this really is public static final double CLIMBER_MAX_VEL = 0.1, CLIMBER_MAX_ACCEL = 0.1; @@ -114,10 +114,8 @@ public class FullMap { CLIMBER_FF_KA = 0, CLIMBER_FF_KG = 0; // Intake - public static final int INTAKE_PISTON_LEFT_FWD_CHANNEL = 2, - INTAKE_PISTON_LEFT_REV_CHANNEL = 3, - INTAKE_PISTON_RIGHT_FWD_CHANNEL = 2, - INTAKE_PISTON_RIGHT_REV_CHANNEL = 3; + public static final int INTAKE_PISTON_FWD_CHANNEL = 0, + INTAKE_PISTON_REV_CHANNEL = 1; // todo find out what the channel numbers are private FullMap() {} @@ -251,6 +249,12 @@ public static RobotMap createRobotMap() { () -> new InstantCommand(() -> drive.resetOdometry(new Pose2d()), drive); SmartDashboard.putData("Reset odometry", resetDriveOdometry.get()); + var deployIntake = new DoubleSolenoid( + PCM_MODULE, + PneumaticsModuleType.CTREPCM, + INTAKE_PISTON_FWD_CHANNEL, + INTAKE_PISTON_REV_CHANNEL); + var cargo = new Cargo2022( new SparkMaxConfig() @@ -263,16 +267,7 @@ public static RobotMap createRobotMap() { .setPort(SPITTER_PORT) .setEnableBrakeMode(false) .createReal(), - new DoubleSolenoid( - PCM_MODULE, - PneumaticsModuleType.CTREPCM, - INTAKE_PISTON_LEFT_FWD_CHANNEL, - INTAKE_PISTON_LEFT_REV_CHANNEL), - new DoubleSolenoid( - PCM_MODULE, - PneumaticsModuleType.CTREPCM, - INTAKE_PISTON_RIGHT_FWD_CHANNEL, - INTAKE_PISTON_RIGHT_REV_CHANNEL), + deployIntake, INTAKE_SPEED, SPITTER_SPEED); Supplier spit = @@ -331,11 +326,15 @@ public static RobotMap createRobotMap() { RobotBase.isReal() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get : () -> false, // todo this is janky af + // did teddy right this cuz he's the only person I know that says "janky" CLIMBER_DISTANCE); // PUT YOUR SUBSYSTEM IN HERE AFTER INITIALIZING IT var subsystems = List.of(drive, cargo, climber); + SmartDashboard.putData("Intake deploy piston: ", new InstantCommand(cargo::deployIntake)); + SmartDashboard.putData("Intake retract piston: ", new InstantCommand(cargo::retractIntake)); + Updater.subscribe(() -> field.setRobotPose(drive.getCurrentPose())); // Button bindings here @@ -354,9 +353,9 @@ public static RobotMap createRobotMap() { new JoystickButton(cargoJoystick, XboxController.Button.kB.value) .whenPressed(cargo::deployIntake); // Run intake in reverse to feed ball from top - new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) - .whileHeld(cargo::runIntakeReverse, cargo) - .whenReleased(cargo::stop, cargo); +// new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) +// .whileHeld(cargo::runIntakeReverse, cargo) +// .whenReleased(cargo::stop, cargo); // Move climber arm up new JoystickButton(climberJoystick, XboxController.Button.kA.value) @@ -488,6 +487,7 @@ public static RobotMap createRobotMap() { // cargo))) // .andThen(spit.get()); + //Auto List autoStartupCommands = List.of(resetDriveOdometry.get(), testPath); List robotStartupCommands = List.of(); @@ -614,3 +614,5 @@ private static Command twoBallTraj( return pose(pose.getX(), pose.getY(), 180 + pose.getRotation().getDegrees()); } } + +//Hi there! \ No newline at end of file diff --git a/src/main/java/frc/team449/javaMaps/IntakeTestMap.java b/src/main/java/frc/team449/javaMaps/IntakeTestMap.java index 29168367..a2f765ad 100644 --- a/src/main/java/frc/team449/javaMaps/IntakeTestMap.java +++ b/src/main/java/frc/team449/javaMaps/IntakeTestMap.java @@ -240,13 +240,8 @@ public static RobotMap createRobotMap() { new DoubleSolenoid( PCM_MODULE, PneumaticsModuleType.CTREPCM, - INTAKE_PISTON_LEFT_FWD_CHANNEL, - INTAKE_PISTON_LEFT_REV_CHANNEL), - new DoubleSolenoid( - PCM_MODULE, - PneumaticsModuleType.CTREPCM, - INTAKE_PISTON_RIGHT_FWD_CHANNEL, - INTAKE_PISTON_RIGHT_REV_CHANNEL), + INTAKE_PISTON_FWD_CHANNEL, + INTAKE_PISTON_REV_CHANNEL), INTAKE_SPEED, SPITTER_SPEED); Supplier runIntake = diff --git a/src/main/java/frc/team449/multiSubsystem/AnalogMotorSingleSpeed.java b/src/main/java/frc/team449/multiSubsystem/AnalogMotorSingleSpeed.java deleted file mode 100644 index cb02ccb5..00000000 --- a/src/main/java/frc/team449/multiSubsystem/AnalogMotorSingleSpeed.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.team449.multiSubsystem; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonIdentityInfo; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.annotation.ObjectIdGenerators; -import edu.wpi.first.wpilibj.motorcontrol.MotorController; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import io.github.oblarg.oblog.Loggable; -import io.github.oblarg.oblog.annotations.Log; -import org.jetbrains.annotations.NotNull; - -/** - * Adapts a {@link MotorController} to be a {@link SubsystemBinaryMotor} by running at a - * constructor-specified speed. - */ -@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) -public class AnalogMotorSingleSpeed extends SubsystemBase - implements SubsystemBinaryMotor, Loggable { - private final MotorController motor; - private final double speed; - @Log private boolean isMotorOn; - - @JsonCreator - public AnalogMotorSingleSpeed( - @NotNull @JsonProperty(required = true) final MotorController motor, - @JsonProperty(required = true) final double speed) { - this.motor = motor; - this.speed = speed; - } - - /** Turns the motor on, and sets it to a map-specified speed. */ - @Override - public void turnMotorOn() { - this.isMotorOn = true; - this.motor.set(this.speed); - } - - /** Turns the motor off. */ - @Override - public void turnMotorOff() { - this.isMotorOn = false; - this.motor.disable(); - } - - /** @return true if the motor is on, false otherwise. */ - @Override - public boolean isMotorOn() { - return this.isMotorOn; - } -} diff --git a/src/main/java/frc/team449/multiSubsystem/SubsystemBinaryMotor.java b/src/main/java/frc/team449/multiSubsystem/SubsystemBinaryMotor.java deleted file mode 100644 index 0b183775..00000000 --- a/src/main/java/frc/team449/multiSubsystem/SubsystemBinaryMotor.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.team449.multiSubsystem; - -import com.fasterxml.jackson.annotation.JsonTypeInfo; - -/** - * A subsystem with a motor that only needs to be run at one speed, e.g. a flywheel multiSubsystem - * or simple intake. - */ -@JsonTypeInfo( - use = JsonTypeInfo.Id.CLASS, - include = JsonTypeInfo.As.WRAPPER_OBJECT, - property = "@class") -public interface SubsystemBinaryMotor { - - /** Turns the motor on, and sets it to a map-specified speed. */ - void turnMotorOn(); - - /** Turns the motor off. */ - void turnMotorOff(); - - /** @return true if the motor is on, false otherwise. */ - boolean isMotorOn(); -} diff --git a/src/main/java/frc/team449/multiSubsystem/commands/LazyCommand.java b/src/main/java/frc/team449/multiSubsystem/commands/LazyCommand.java deleted file mode 100644 index 3d681d51..00000000 --- a/src/main/java/frc/team449/multiSubsystem/commands/LazyCommand.java +++ /dev/null @@ -1,48 +0,0 @@ -package frc.team449.multiSubsystem.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.Subsystem; -import org.jetbrains.annotations.NotNull; - -import java.util.function.Supplier; - -/** - * A Command that instantiates and initializes a wrapped command in its {@link - * LazyCommand#initialize()} method - */ -public class LazyCommand extends CommandBase { - private final @NotNull Supplier cmdCreator; - /** The actual command, which will be created lazily */ - private Command cmd; - - /** - * @param cmdCreator A Supplier to create the actual command later - * @param requirements The subsystems required by the wrapped command. DO NOT FORGET THIS! - */ - public LazyCommand(@NotNull Supplier cmdCreator, Subsystem... requirements) { - addRequirements(requirements); - this.cmdCreator = cmdCreator; - } - - @Override - public void initialize() { - this.cmd = this.cmdCreator.get(); - this.cmd.initialize(); - } - - @Override - public void execute() { - this.cmd.execute(); - } - - @Override - public void end(boolean interrupted) { - this.cmd.end(interrupted); - } - - @Override - public boolean isFinished() { - return this.cmd.isFinished(); - } -} diff --git a/src/main/java/frc/team449/multiSubsystem/commands/SetIntakeMode.java b/src/main/java/frc/team449/multiSubsystem/commands/SetIntakeMode.java deleted file mode 100644 index 58a983ba..00000000 --- a/src/main/java/frc/team449/multiSubsystem/commands/SetIntakeMode.java +++ /dev/null @@ -1,64 +0,0 @@ -package frc.team449.multiSubsystem.commands; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonIdentityInfo; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.annotation.ObjectIdGenerators; -import edu.wpi.first.wpilibj.shuffleboard.EventImportance; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; -import frc.team449.multiSubsystem.SubsystemIntake; -import io.github.oblarg.oblog.annotations.Log; -import org.jetbrains.annotations.NotNull; - -/** Sets the mode of the intake. */ -@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) -public class SetIntakeMode extends InstantCommand { - - /** The subsystem to execute this command on. */ - @NotNull @Log.Exclude private final T subsystem; - - /** The mode to set this subsystem to. */ - @NotNull private final SubsystemIntake.IntakeMode mode; - - /** - * Default constructor - * - * @param subsystem The subsystem to execute this command on. - * @param mode The mode to set the intake to. - */ - @JsonCreator - public SetIntakeMode( - @NotNull @JsonProperty(required = true) final T subsystem, - @NotNull @JsonProperty(required = true) final SubsystemIntake.IntakeMode mode) { - addRequirements(subsystem); - this.subsystem = subsystem; - this.mode = mode; - } - - /** Log when this command is initialized */ - @Override - public void initialize() { - Shuffleboard.addEventMarker( - "SetIntakeMode init.", this.getClass().getSimpleName(), EventImportance.kNormal); - // Logger.addEvent("SetIntakeMode init.", this.getClass()); - } - - /** Set the intake to the given mode. */ - @Override - public void execute() { - subsystem.setMode(mode); - } - - /** Log when this command ends */ - @Override - public void end(final boolean interrupted) { - if (interrupted) { - Shuffleboard.addEventMarker( - "SetIntakeMode Interrupted!", this.getClass().getSimpleName(), EventImportance.kNormal); - } - Shuffleboard.addEventMarker( - "SetIntakeMode end.", this.getClass().getSimpleName(), EventImportance.kNormal); - } -} diff --git a/src/main/java/frc/team449/multiSubsystem/commands/SetSolenoidPose.java b/src/main/java/frc/team449/multiSubsystem/commands/SetSolenoidPose.java deleted file mode 100644 index 47674208..00000000 --- a/src/main/java/frc/team449/multiSubsystem/commands/SetSolenoidPose.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.team449.multiSubsystem.commands; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonIdentityInfo; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.annotation.ObjectIdGenerators; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.team449.multiSubsystem.SolenoidSimple; -import org.jetbrains.annotations.NotNull; - -/** Sets the state of a provided solenoid to the correct state. */ -@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) -public class SetSolenoidPose extends InstantCommand { - - /** - * Default constructor - * - * @param subsystem The subsystem subsystem to run this command on. - * @param value The value to set the subsystem to. - */ - @JsonCreator - public SetSolenoidPose( - @NotNull @JsonProperty(required = true) final SolenoidSimple subsystem, - @NotNull @JsonProperty(required = true) final DoubleSolenoid.Value value) { - super(() -> subsystem.setSolenoid(value), subsystem); - } -} diff --git a/src/main/java/frc/team449/multiSubsystem/commands/ToggleSolenoid.java b/src/main/java/frc/team449/multiSubsystem/commands/ToggleSolenoid.java deleted file mode 100644 index 9f0640d9..00000000 --- a/src/main/java/frc/team449/multiSubsystem/commands/ToggleSolenoid.java +++ /dev/null @@ -1,64 +0,0 @@ -package frc.team449.multiSubsystem.commands; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonIdentityInfo; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.annotation.ObjectIdGenerators; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.shuffleboard.EventImportance; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.team449.multiSubsystem.SubsystemSolenoid; -import io.github.oblarg.oblog.annotations.Log; -import org.jetbrains.annotations.NotNull; - -/** - * A command that toggles the position of a piston. DO NOT USE IN COMPETITIONS!! Toggles are too - * hard for driver to track - */ -@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) -public class ToggleSolenoid extends InstantCommand { - - /** The subsystem to execute this command on. */ - @NotNull @Log.Exclude private final SubsystemSolenoid subsystem; - - /** - * Default constructor - * - * @param subsystem The solenoid subsystem to execute this command on. - */ - @JsonCreator - public ToggleSolenoid(@NotNull @JsonProperty(required = true) final SubsystemSolenoid subsystem) { - this.subsystem = subsystem; - } - - /** Log when this command is initialized */ - @Override - public void initialize() { - Shuffleboard.addEventMarker( - "ToggleSolenoid init.", this.getClass().getSimpleName(), EventImportance.kNormal); - // Logger.addEvent("ToggleSolenoid init.", this.getClass()); - } - - /** Toggle the state of the piston. */ - @Override - public void execute() { - if (subsystem.getSolenoidPosition().equals(DoubleSolenoid.Value.kForward)) { - subsystem.setSolenoid(DoubleSolenoid.Value.kReverse); - } else { - subsystem.setSolenoid(DoubleSolenoid.Value.kForward); - } - } - - /** Log when this command ends */ - @Override - public void end(final boolean interrupted) { - if (interrupted) { - Shuffleboard.addEventMarker( - "ToggleSolenoid Interrupted!", this.getClass().getSimpleName(), EventImportance.kNormal); - } - Shuffleboard.addEventMarker( - "ToggleSolenoid end.", this.getClass().getSimpleName(), EventImportance.kNormal); - // Logger.addEvent("ToggleSolenoid end.", this.getClass()); - } -} diff --git a/src/main/java/frc/team449/multiSubsystem/commands/TurnMotorOff.java b/src/main/java/frc/team449/multiSubsystem/commands/TurnMotorOff.java deleted file mode 100644 index 327b41e0..00000000 --- a/src/main/java/frc/team449/multiSubsystem/commands/TurnMotorOff.java +++ /dev/null @@ -1,56 +0,0 @@ -package frc.team449.multiSubsystem.commands; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonIdentityInfo; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.annotation.ObjectIdGenerators; -import edu.wpi.first.wpilibj.shuffleboard.EventImportance; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.team449.multiSubsystem.SubsystemBinaryMotor; -import io.github.oblarg.oblog.annotations.Log; -import org.jetbrains.annotations.NotNull; - -/** Turns off the motor of the specified subsystem. */ -@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) -public class TurnMotorOff extends InstantCommand { - - /** The subsystem to execute this command on. */ - @NotNull @Log.Exclude protected final SubsystemBinaryMotor subsystem; - - /** - * Default constructor - * - * @param subsystem The subsystem to execute this command on. - */ - @JsonCreator - public TurnMotorOff( - @NotNull @JsonProperty(required = true) final SubsystemBinaryMotor subsystem) { - this.subsystem = subsystem; - } - - /** Log when this command is initialized */ - @Override - public void initialize() { - Shuffleboard.addEventMarker( - "TurnMotorOff init.", this.getClass().getSimpleName(), EventImportance.kNormal); - // Logger.addEvent("TurnMotorOff init.", this.getClass()); - } - - /** Turn the motor off. */ - @Override - public void execute() { - subsystem.turnMotorOff(); - } - - /** Log when this command ends */ - @Override - public void end(final boolean interrupted) { - if (interrupted) { - Shuffleboard.addEventMarker( - "TurnMotorOff Interrupted!", this.getClass().getSimpleName(), EventImportance.kNormal); - } - Shuffleboard.addEventMarker( - "TurnMotorOff end.", this.getClass().getSimpleName(), EventImportance.kNormal); - } -} diff --git a/src/main/java/frc/team449/multiSubsystem/commands/TurnMotorOn.java b/src/main/java/frc/team449/multiSubsystem/commands/TurnMotorOn.java deleted file mode 100644 index 6fd534ba..00000000 --- a/src/main/java/frc/team449/multiSubsystem/commands/TurnMotorOn.java +++ /dev/null @@ -1,55 +0,0 @@ -package frc.team449.multiSubsystem.commands; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonIdentityInfo; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.annotation.ObjectIdGenerators; -import edu.wpi.first.wpilibj.shuffleboard.EventImportance; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.team449.multiSubsystem.SubsystemBinaryMotor; -import io.github.oblarg.oblog.annotations.Log; -import org.jetbrains.annotations.NotNull; - -/** Turns on the motor of the specified subsystem. */ -@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) -public class TurnMotorOn extends InstantCommand { - - /** The subsystem to execute this command on. */ - @NotNull @Log.Exclude private final SubsystemBinaryMotor subsystem; - - /** - * Default constructor - * - * @param subsystem The subsystem to execute this command on. - */ - @JsonCreator - public TurnMotorOn(@NotNull @JsonProperty(required = true) final SubsystemBinaryMotor subsystem) { - this.subsystem = subsystem; - } - - /** Log when this command is initialized */ - @Override - public void initialize() { - Shuffleboard.addEventMarker( - "TurnMotorOn init.", this.getClass().getSimpleName(), EventImportance.kNormal); - // Logger.addEvent("TurnMotorOn init.", this.getClass()); - } - - /** Turn the motor on. */ - @Override - public void execute() { - subsystem.turnMotorOn(); - } - - /** Log when this command ends */ - @Override - public void end(final boolean interrupted) { - if (interrupted) { - Shuffleboard.addEventMarker( - "TurnMotorOn Interrupted!", this.getClass().getSimpleName(), EventImportance.kNormal); - } - Shuffleboard.addEventMarker( - "TurnMotorOn end.", this.getClass().getSimpleName(), EventImportance.kNormal); - } -} From 4e6ab62226443cf36290a36e7cc34abbeda681d5 Mon Sep 17 00:00:00 2001 From: Teddy Date: Sat, 5 Mar 2022 15:10:29 -0500 Subject: [PATCH 09/25] Cleaned up a lot of stuffs --- build.gradle | 2 - .../components/ConditionTimingComponent.java | 5 +- .../java/frc/team449/javaMaps/FullMap.java | 2 +- .../team449/multiSubsystem/IntakeSimple.java | 3 - .../multiSubsystem/SolenoidSimple.java | 47 ------------ .../multiSubsystem/SubsystemConditional.java | 72 ------------------- .../other/InjectiveDependencyHelper.java | 64 ----------------- src/main/java/frc/team449/other/LogEvent.java | 51 ------------- .../other/RegistrationOrderIDUtil.java | 49 ------------- 9 files changed, 2 insertions(+), 293 deletions(-) delete mode 100644 src/main/java/frc/team449/multiSubsystem/SolenoidSimple.java delete mode 100644 src/main/java/frc/team449/multiSubsystem/SubsystemConditional.java delete mode 100644 src/main/java/frc/team449/other/InjectiveDependencyHelper.java delete mode 100644 src/main/java/frc/team449/other/LogEvent.java delete mode 100644 src/main/java/frc/team449/other/RegistrationOrderIDUtil.java diff --git a/build.gradle b/build.gradle index 68d9f6e6..f9fde359 100644 --- a/build.gradle +++ b/build.gradle @@ -38,8 +38,6 @@ deploy { def deployArtifact = deploy.targets.roborio.artifacts.frcJava dependencies { - implementation group: 'com.google.guava', name: 'guava', version: '31.0-jre' - def jacksonVersion = '2.12.5' implementation group: 'com.fasterxml.jackson.core', name: 'jackson-databind', version: jacksonVersion implementation group: 'com.fasterxml.jackson.core', name: 'jackson-core', version: jacksonVersion diff --git a/src/main/java/frc/team449/components/ConditionTimingComponent.java b/src/main/java/frc/team449/components/ConditionTimingComponent.java index 0851afcf..a12ee757 100644 --- a/src/main/java/frc/team449/components/ConditionTimingComponent.java +++ b/src/main/java/frc/team449/components/ConditionTimingComponent.java @@ -1,6 +1,5 @@ package frc.team449.components; -import frc.team449.other.RegistrationOrderIDUtil; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.Contract; @@ -20,8 +19,6 @@ public abstract class ConditionTimingComponent implements Loggable { protected ConditionTimingComponent(final boolean initialValue) { this.current = initialValue; - - RegistrationOrderIDUtil.registerInstance(this); } /** @@ -169,6 +166,6 @@ public boolean isTrue() { @Override @Contract(pure = true) public String configureLogName() { - return this.getClass().getSimpleName() + RegistrationOrderIDUtil.getExistingID(this); + return this.getClass().getSimpleName(); } } diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 4a7d61f8..77359427 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -370,7 +370,7 @@ public static RobotMap createRobotMap() { .whenPressed( new InstantCommand(() -> climber.setGoal(0), climber) .andThen(new WaitUntilCommand(climber::atGoal)) - .until(climber::hitBottom) + .withInterrupt(climber::hitBottom) .andThen( () -> { if (climber.hitBottom()) { diff --git a/src/main/java/frc/team449/multiSubsystem/IntakeSimple.java b/src/main/java/frc/team449/multiSubsystem/IntakeSimple.java index ab258b45..bf07e1ee 100644 --- a/src/main/java/frc/team449/multiSubsystem/IntakeSimple.java +++ b/src/main/java/frc/team449/multiSubsystem/IntakeSimple.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.team449.other.InjectiveDependencyHelper; import frc.team449.other.Util; import io.github.oblarg.oblog.Loggable; import java.util.Map; @@ -42,8 +41,6 @@ public IntakeSimple( @NotNull @JsonProperty(required = true) final MotorController motor, @NotNull @JsonProperty(required = true) final Map velocities) { - InjectiveDependencyHelper.assertInjective(this, motor); - this.motor = motor; this.velocities = velocities; diff --git a/src/main/java/frc/team449/multiSubsystem/SolenoidSimple.java b/src/main/java/frc/team449/multiSubsystem/SolenoidSimple.java deleted file mode 100644 index 3de268e7..00000000 --- a/src/main/java/frc/team449/multiSubsystem/SolenoidSimple.java +++ /dev/null @@ -1,47 +0,0 @@ -package frc.team449.multiSubsystem; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonIdentityInfo; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.annotation.ObjectIdGenerators; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import io.github.oblarg.oblog.Loggable; -import io.github.oblarg.oblog.annotations.Log; -import org.jetbrains.annotations.NotNull; - -/** A simple SubsystemSolenoid.java. */ -@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) -public class SolenoidSimple extends SubsystemBase implements SubsystemSolenoid, Loggable { - - /** Piston for pushing gears */ - @NotNull private final DoubleSolenoid piston; - - /** The piston's current position */ - @NotNull private DoubleSolenoid.Value pistonPos = DoubleSolenoid.Value.kOff; - - /** - * Default constructor - * - * @param piston The piston that comprises this subsystem. - */ - @JsonCreator - public SolenoidSimple(@NotNull @JsonProperty(required = true) final DoubleSolenoid piston) { - this.piston = piston; - } - - /** @param value The position to set the solenoid to. */ - @Override - public void setSolenoid(@NotNull final DoubleSolenoid.Value value) { - this.piston.set(value); - this.pistonPos = value; - } - - /** @return the current position of the solenoid. */ - @NotNull - @Override - @Log.ToString - public DoubleSolenoid.Value getSolenoidPosition() { - return this.pistonPos; - } -} diff --git a/src/main/java/frc/team449/multiSubsystem/SubsystemConditional.java b/src/main/java/frc/team449/multiSubsystem/SubsystemConditional.java deleted file mode 100644 index 490558cc..00000000 --- a/src/main/java/frc/team449/multiSubsystem/SubsystemConditional.java +++ /dev/null @@ -1,72 +0,0 @@ -package frc.team449.multiSubsystem; - -import com.fasterxml.jackson.annotation.JsonTypeInfo; -import frc.team449.generalInterfaces.updatable.Updatable; -import io.github.oblarg.oblog.Loggable; -import java.util.concurrent.ConcurrentHashMap; -import java.util.concurrent.ConcurrentMap; - -/** - * A subsystem with a condition that's sometimes met, e.g. a limit switch, a current/power limit, an - * IR sensor. - */ -@JsonTypeInfo( - use = JsonTypeInfo.Id.CLASS, - include = JsonTypeInfo.As.WRAPPER_OBJECT, - property = "@class") -public interface SubsystemConditional extends Updatable, Loggable { - - /** - * Computes the current state of the condition. - * - * @return {@code true} if the condition is met, {@code false} otherwise - */ - boolean isConditionTrue(); - - /** - * Gets the state of the condition when {@link SubsystemConditional#update()} was last called. - * - * @return {@code false} if the condition was met when cached, {@code false} otherwise - * @implNote See documentation for {@link SubsystemConditional#update()} - * @implSpec Both this method and {@link SubsystemConditional#update()} must be overridden - * together. - */ - default boolean isConditionTrueCached() { - return MixinImpl.isConditionTrueCached(this); - } - - /** - * Updates the cached value of the condition. - * - * @implNote The default implementation caches the return value of {@link - * SubsystemConditional#isConditionTrue()} in a {@link ConcurrentHashMap} of instances of this - * interface. - * @implSpec Both this method and {@link SubsystemConditional#isConditionTrueCached()} must be - * overridden together. - */ - @Override - default void update() { - MixinImpl.update(this); - } -} - -@SuppressWarnings("ClassNameDiffersFromFileName") -final class MixinImpl { - private static final ConcurrentMap cachedConditions = - new ConcurrentHashMap<>(); - - private MixinImpl() {} - - static boolean isConditionTrueCached(final SubsystemConditional self) { - final Boolean cached = cachedConditions.get(self); - if (cached != null) return cached; - - final boolean updated = self.isConditionTrue(); - cachedConditions.put(self, updated); - return updated; - } - - static void update(final SubsystemConditional self) { - cachedConditions.put(self, self.isConditionTrue()); - } -} diff --git a/src/main/java/frc/team449/other/InjectiveDependencyHelper.java b/src/main/java/frc/team449/other/InjectiveDependencyHelper.java deleted file mode 100644 index 0818b30d..00000000 --- a/src/main/java/frc/team449/other/InjectiveDependencyHelper.java +++ /dev/null @@ -1,64 +0,0 @@ -package frc.team449.other; - -import com.google.common.collect.Multimap; -import com.google.common.collect.Multimaps; -import java.util.Collection; -import java.util.HashMap; -import java.util.LinkedList; -import java.util.function.Predicate; -import org.jetbrains.annotations.NotNull; - -/** - * Ensures that depended object instances are not each depended on by more than one - * dependent object within a set of criteria specified on a per-dependency basis. - * Dependencies are registered by either dependent or depended instances. - * - *

In other words, asserts that the function mapping instances of depended objects to instances - * of dependent objects is injective for each intersection of the set of all dependent objects and - * the sets for which each dependency-specified predicate returns {@code true}. - */ -public final class InjectiveDependencyHelper { - private static final Multimap mappings = - Multimaps.newMultimap(new HashMap<>(), LinkedList::new); - - private InjectiveDependencyHelper() {} - - public static void assertInjective( - @NotNull final Object dependent, @NotNull final Object depended) - throws IllegalStateException { - assertInjective(dependent, depended, dependent.getClass()); - } - - public static void assertInjective( - @NotNull final Object dependent, - @NotNull final Object depended, - @NotNull final Class instanceOf) - throws IllegalStateException { - assertInjective(dependent, depended, instanceOf::isInstance); - } - - public static void assertInjective( - @NotNull final Object dependent, - @NotNull final Object depended, - @NotNull final Predicate dependentClass) - throws IllegalStateException { - if (dependentIsDuplicate(dependent, depended, dependentClass)) { - throw new IllegalStateException( - "Non-injective dependency of " - + dependent - + " on " - + depended - + " (multiple instances of former depend on one instance of latter)"); - } - } - - private static boolean dependentIsDuplicate( - @NotNull final Object dependent, - @NotNull final Object depended, - @NotNull final Predicate dependentClass) { - final Collection dependents = mappings.get(depended); - final boolean dependentIsDuplicate = dependents.stream().anyMatch(dependentClass); - dependents.add(dependent); - return dependentIsDuplicate; - } -} diff --git a/src/main/java/frc/team449/other/LogEvent.java b/src/main/java/frc/team449/other/LogEvent.java deleted file mode 100644 index 6a8fecbe..00000000 --- a/src/main/java/frc/team449/other/LogEvent.java +++ /dev/null @@ -1,51 +0,0 @@ -package frc.team449.other; - -import com.fasterxml.jackson.annotation.JsonCreator; -import com.fasterxml.jackson.annotation.JsonIdentityInfo; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.annotation.ObjectIdGenerators; -import org.jetbrains.annotations.NotNull; - -/** An logged event with a message, timestamp, and calling class. */ -@JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class) -public class LogEvent { - - /** The time, in milliseconds, at which this event was created. */ - private final long timeCalled; - - /** The message of this event. */ - @NotNull private final String message; - - /** The class that called this event. */ - @NotNull private final Class caller; - - /** - * Default constructor. - * - *

Note to future people: Don't rewrite this to get the calling class from the stack trace. - * It's possible, and makes the code cleaner than taking the calling class as an argument, but - * getting the stack trace actually takes Java a little while, and considering how often this - * constructor is called, that would significantly slow us down. - * - * @param message The message of this event. - * @param caller The calling class. Should pretty much always be this.getClass(). - */ - @JsonCreator - public LogEvent( - @NotNull @JsonProperty(required = true) String message, - @NotNull @JsonProperty(required = true) Class caller) { - timeCalled = Clock.currentTimeMillis(); - this.message = message; - this.caller = caller; - } - - /** - * Turn this event into a string for logging. - * - * @return The time called, calling class, and message, comma-separated and in that order. - */ - @NotNull - public String toString() { - return timeCalled + "," + caller + "," + message; - } -} diff --git a/src/main/java/frc/team449/other/RegistrationOrderIDUtil.java b/src/main/java/frc/team449/other/RegistrationOrderIDUtil.java deleted file mode 100644 index dc21d32a..00000000 --- a/src/main/java/frc/team449/other/RegistrationOrderIDUtil.java +++ /dev/null @@ -1,49 +0,0 @@ -package frc.team449.other; - -import java.util.IdentityHashMap; -import org.jetbrains.annotations.NotNull; - -/** - * Associates object instances ID based on order of registration. - * - * @implNote ID begins at {@code 1} and increments by {@code 1}. - */ -public final class RegistrationOrderIDUtil { - @NotNull private static final IdentityHashMap IDs = new IdentityHashMap<>(); - private static int currentId = 0; - - private RegistrationOrderIDUtil() { - throw new AssertionError("Utility class."); - } - - /** - * Associates an ID with the specified object. Does nothing if the specified object has already - * been registered. - * - * @param instance the object instance to register - * @return the ID associated with the object - */ - @SuppressWarnings("UnusedReturnValue") - public static int registerInstance(final Object instance) { - final Integer oldID; - if ((oldID = IDs.get(instance)) != null) return oldID; - - IDs.put(instance, ++currentId); - return currentId; - } - - /** - * Gets the ID associated with the specified object. - * - * @param instance the object instance to retrieve the ID of - * @return the ID associated with the object - * @throws IllegalStateException if the specified object has never been registered - */ - public static int getExistingID(final Object instance) throws IllegalStateException { - final Integer result = IDs.get(instance); - if (result == null) { - throw new IllegalStateException("Specified object not registered."); - } - return result; - } -} From a4b5375da0224b30200c23e6d01d8d1ee0fc0f0c Mon Sep 17 00:00:00 2001 From: Teddy Date: Sat, 5 Mar 2022 18:07:54 -0500 Subject: [PATCH 10/25] Start on 3 ball auto --- src/main/deploy/pathplanner/1ball blue 2.path | 2 +- .../team449/auto/commands/RamseteBuilder.java | 17 +- .../java/frc/team449/javaMaps/FullMap.java | 234 ++++++++++++------ 3 files changed, 164 insertions(+), 89 deletions(-) diff --git a/src/main/deploy/pathplanner/1ball blue 2.path b/src/main/deploy/pathplanner/1ball blue 2.path index 44ca257e..84a94d4a 100644 --- a/src/main/deploy/pathplanner/1ball blue 2.path +++ b/src/main/deploy/pathplanner/1ball blue 2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":7.558283470997322,"y":2.8936766734292965},"prevControl":null,"nextControl":{"x":7.255652260752695,"y":2.122455202160731},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.789777604871666,"y":0.7523559350916172},"prevControl":{"x":7.89394996511512,"y":1.0764477225156979},"nextControl":{"x":7.685605244628211,"y":0.4282641476675366},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.544284506290531,"y":1.6667577638952737},"prevControl":{"x":5.9493992405706315,"y":1.2269189095340214},"nextControl":{"x":5.13916977201043,"y":2.106596618256526},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.951823498583706,"y":2.8010790198795585},"prevControl":{"x":7.73190407140308,"y":2.3728148722120235},"nextControl":{"x":7.73190407140308,"y":2.3728148722120235},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.510584929933365,"y":1.3238518673916093},"prevControl":{"x":7.281886978562657,"y":2.14816412734927},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":2.0,"maxAcceleration":0.6,"isReversed":false} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":7.985559031614295,"y":2.811541566142419},"prevControl":null,"nextControl":{"x":7.6341163358463415,"y":1.8450741527805459},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.643878632948881,"y":0.6638362031207639},"prevControl":{"x":6.940993241412974,"y":0.9957543046793873},"nextControl":{"x":6.940993241412974,"y":0.9957543046793873},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.418074893085174,"y":1.7474511817386211},"prevControl":{"x":6.003812719365096,"y":1.9719840151459256},"nextControl":{"x":6.003812719365096,"y":1.9719840151459256},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.429108096646242,"y":2.8310661603564795},"prevControl":{"x":7.0971899950876205,"y":2.4796234645885256},"nextControl":{"x":7.761026198204864,"y":3.1825088561244335},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.36628861869412,"y":4.334459914474949},"prevControl":{"x":8.063657408449492,"y":3.758484385299691},"nextControl":{"x":8.66891982893875,"y":4.910435443650207},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.7207061033298,"y":1.9622217180412602},"prevControl":{"x":7.219218708895938,"y":3.1873900046767654},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.4,"isReversed":false} \ No newline at end of file diff --git a/src/main/java/frc/team449/auto/commands/RamseteBuilder.java b/src/main/java/frc/team449/auto/commands/RamseteBuilder.java index 0128bec6..de4956ca 100644 --- a/src/main/java/frc/team449/auto/commands/RamseteBuilder.java +++ b/src/main/java/frc/team449/auto/commands/RamseteBuilder.java @@ -52,9 +52,7 @@ public RamseteBuilder traj(Trajectory traj) { return this; } - /** - * Set the {@link Field2d} widget in Glass to display the trajectory on (optional) - */ + /** Set the {@link Field2d} widget in Glass to display the trajectory on (optional) */ public RamseteBuilder field(Field2d field) { this.field = field; return this; @@ -76,8 +74,8 @@ public RamseteBuilder b(double b) { } /** - * Set the zeta tuning parameter for Ramsete (0 rad-1 < zeta < 1 rad-1) for which larger - * values provide more damping in response. + * Set the zeta tuning parameter for Ramsete (0 rad-1 < zeta < 1 + * rad-1) for which larger values provide more damping in response. */ public RamseteBuilder zeta(double zeta) { this.zeta = zeta; @@ -154,9 +152,10 @@ public Command build() { return new InstantCommand(() -> drivetrain.resetOdometry(traj.getInitialPose())) .andThen(ramseteCmd) - .andThen(() -> drivetrain.setVoltage(0, 0)) - .andThen( - new NavXTurnToAngle<>( - lastPose.getRotation().getDegrees(), angleTimeout, drivetrain, pidAngleController)); +// .andThen(() -> drivetrain.setVoltage(0, 0)) +// .andThen( +// new NavXTurnToAngle<>( +// lastPose.getRotation().getDegrees(), angleTimeout, drivetrain, pidAngleController)) + .andThen(() -> drivetrain.setVoltage(0, 0)); } } diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 77359427..76122203 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -37,6 +37,7 @@ import frc.team449.components.RunningLinRegComponent; import frc.team449.drive.DriveSettingsBuilder; import frc.team449.drive.unidirectional.DriveUnidirectionalWithGyro; +import frc.team449.drive.unidirectional.commands.AHRS.NavXTurnToAngle; import frc.team449.drive.unidirectional.commands.UnidirectionalNavXDefaultDrive; import frc.team449.generalInterfaces.doubleUnaryOperator.Polynomial; import frc.team449.generalInterfaces.doubleUnaryOperator.RampComponent; @@ -103,9 +104,9 @@ public class FullMap { public static final double MOMENT_OF_INERTIA = 7.5; public static final double MASS = 60; // Climber - public static final int CLIMBER_PISTON_FWD_CHANNEL = 4, CLIMBER_PISTON_REV_CHANNEL = 5; + public static final int CLIMBER_PISTON_FWD_CHANNEL = 0, CLIMBER_PISTON_REV_CHANNEL = 1; // todo find out what the channel numbers are - public static final int CLIMBER_SENSOR_CHANNEL = 0; // todo find out what this really is + public static final int CLIMBER_SENSOR_CHANNEL = 6; // todo find out what this really is public static final double CLIMBER_MAX_VEL = 0.1, CLIMBER_MAX_ACCEL = 0.1; public static final double CLIMBER_DISTANCE = 0.5; public static final double CLIMBER_KP = 12; @@ -114,8 +115,7 @@ public class FullMap { CLIMBER_FF_KA = 0, CLIMBER_FF_KG = 0; // Intake - public static final int INTAKE_PISTON_FWD_CHANNEL = 0, - INTAKE_PISTON_REV_CHANNEL = 1; + public static final int INTAKE_PISTON_FWD_CHANNEL = 2, INTAKE_PISTON_REV_CHANNEL = 3; // todo find out what the channel numbers are private FullMap() {} @@ -227,29 +227,27 @@ public static RobotMap createRobotMap() { .3, false); + var pidAngleControllerPrototype = + new PIDAngleControllerBuilder() + .absoluteTolerance(0) + .onTargetBuffer(new Debouncer(1.5)) + .minimumOutput(0) + .maximumOutput(0.6) + .loopTimeMillis(null) + .deadband(2) + .inverted(false) + .pid(0.01, 0, 0.03); + var driveDefaultCmd = new UnidirectionalNavXDefaultDrive<>( - 3.0, - new Debouncer(0.15), - drive, - oi, - null, - new PIDAngleControllerBuilder() - .absoluteTolerance(0) - .onTargetBuffer(new Debouncer(1.5)) - .minimumOutput(0) - .maximumOutput(0.6) - .loopTimeMillis(null) - .deadband(2) - .inverted(false) - .pid(0.01, 0, 0.03) - .build()); + 3.0, new Debouncer(0.15), drive, oi, null, pidAngleControllerPrototype.build()); Supplier resetDriveOdometry = () -> new InstantCommand(() -> drive.resetOdometry(new Pose2d()), drive); SmartDashboard.putData("Reset odometry", resetDriveOdometry.get()); - var deployIntake = new DoubleSolenoid( + var deployIntake = + new DoubleSolenoid( PCM_MODULE, PneumaticsModuleType.CTREPCM, INTAKE_PISTON_FWD_CHANNEL, @@ -326,7 +324,7 @@ public static RobotMap createRobotMap() { RobotBase.isReal() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get : () -> false, // todo this is janky af - // did teddy right this cuz he's the only person I know that says "janky" + // did teddy right this cuz he's the only person I know that says "janky" CLIMBER_DISTANCE); // PUT YOUR SUBSYSTEM IN HERE AFTER INITIALIZING IT @@ -353,9 +351,9 @@ public static RobotMap createRobotMap() { new JoystickButton(cargoJoystick, XboxController.Button.kB.value) .whenPressed(cargo::deployIntake); // Run intake in reverse to feed ball from top -// new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) -// .whileHeld(cargo::runIntakeReverse, cargo) -// .whenReleased(cargo::stop, cargo); + // new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) + // .whileHeld(cargo::runIntakeReverse, cargo) + // .whenReleased(cargo::stop, cargo); // Move climber arm up new JoystickButton(climberJoystick, XboxController.Button.kA.value) @@ -414,63 +412,141 @@ public static RobotMap createRobotMap() { // (assume blue alliance) // Start at bottom next to hub, shoot preloaded ball, then get the two balls in that region and // score those - // var scoreThenGetTwoThenScore = - // new InstantCommand(cargo::runIntake, cargo) + // var scoreThenGetTwoThenScore = + // new InstantCommand(cargo::runIntake, cargo) + // .andThen( + // ramsetePrototype + // .copy() + // .name("1-2ballbluebottom") + // .traj(loadPathPlannerTraj("New Path")) + // .build()) + // .andThen(spit.get()); + + // Spit the preloaded ball, pick up another, come back and spit it out + // var topOneOneTraj = + // oneThenOneBallTraj( + // drive, + // cargo, + // ramsetePrototype.name("topOneOne"), + // pose(7.11, 4.80, 159.25), + // pose(5.39, 5.91, 137.86)); + // var midOneOneTraj = + // oneThenOneBallTraj( + // drive, + // cargo, + // ramsetePrototype.name("midOneOne"), + // pose(7.56, 3.00, -111.80), + // pose(5.58, 2.00, -173.42)); + // var bottomOneOneTraj = + // oneThenOneBallTraj( + // drive, + // cargo, + // ramsetePrototype.name("bottomOneOne"), + // pose(8.01, 2.82, -111.80), + // pose(7.67, 0.77, -91.97)); + // + // // Start at the edge at the top, collect the top ball, then come back and spit + // var topTwoBallTraj = + // twoBallTraj( + // drive, + // cargo, + // ramsetePrototype.name("topTwo"), + // pose(6.07, 5.12, 134.24), + // pose(5.34, 5.89, 130.31), + // reverseHeading(pose(7.04, 4.58, -22.25))); + // // Start at the edge at the bottom, collect the bottom ball, then come back and spit + // var bottomTwoBallTraj = + // twoBallTraj( + // drive, + // cargo, + // ramsetePrototype.name("bottomTwo"), + // pose(7.55, 1.83, -88.32), + // pose(7.63, 0.76, -86.19), + // reverseHeading(pose(8.01, 2.82, 68.63))); + var ballX = 7.65; + var ballY = 0.69; + var ballAngle = -92.44; + var turnAngle = -109.98; + var ballX2 = 5.42; + var ballY2 = 1.75; + var turnAngle2 = -159.03; + var hubPose = pose(7.43, 2.83, 46.64); + var threeBallAuto = + new InstantCommand(cargo::spit, cargo) + .andThen(new WaitCommand(1)) + .andThen(cargo::runIntake, cargo) + .andThen( + ramsetePrototype + .copy() + .name("threeBallAuto1") + .traj( + TrajectoryGenerator.generateTrajectory( + pose(7.99, 2.81, -109.98), + List.of(), + pose(ballX, ballY, ballAngle), + trajConfig(drive))) + .build()) + .andThen( + new NavXTurnToAngle<>( + -turnAngle, 6, drive, pidAngleControllerPrototype.pid(0.001, 0, 0).build())) + .andThen( + ramsetePrototype + .copy() + .name("threeBallAuto2") + .traj( + TrajectoryGenerator.generateTrajectory( + pose(ballX, ballY, turnAngle), + List.of(), + pose(ballX2, ballY2, 154.49), + trajConfig(drive))) + .build()); + // .andThen( + // new NavXTurnToAngle<>(turnAngle2, 1, drive, + // pidAngleControllerPrototype.build())) // .andThen( // ramsetePrototype - // .copy() - // .name("1-2ballbluebottom") - // .traj(loadPathPlannerTraj("New Path")) + // .name("threeBallAuto3") + // .traj( + // TrajectoryGenerator.generateTrajectory( + // pose(ballX2, ballY2, turnAngle2), + // List.of(), + // hubPose, + // trajConfig(drive))) // .build()) + // .andThen(cargo::spit,cargo) + // .andThen(new WaitCommand(2)) + // .andThen(ramsetePrototype + // .name("threeBallAuto4") + // .traj( + // TrajectoryGenerator.generateTrajectory( + // hubPose, + // List.of(), + // pose(5.70,1.98,-140.73), + // trajConfig(drive))) + // .build()); + + // var randomTestAuto = + // ramsetePrototype + // .name("randomTestAuto") + // .traj( + // TrajectoryGenerator.generateTrajectory( + // pose(7.56, 2.89, -38.40), + // List.of(new Translation2d(7.62, 0.30), new Translation2d(6.63, 2.27)), + // pose(5.17, 1.99, -56.21), + // trajConfig(drive)) + // .concatenate( + // TrajectoryGenerator.generateTrajectory( + // pose(5.17, 1.99, -56.21), + // List.of(new Translation2d(3.05, 1.80)), + // pose(4.97, 4.34, 1.04), + // trajConfig(drive)))) + // .build(); + + // var testPath = + // new InstantCommand(cargo::runIntake, cargo) + // .andThen(ramsetePrototype.copy().name("testtraj").traj(testTraj(drive)).build()) // .andThen(spit.get()); - // Spit the preloaded ball, pick up another, come back and spit it out - var topOneOneTraj = - oneThenOneBallTraj( - drive, - cargo, - ramsetePrototype.name("topOneOne"), - pose(7.11, 4.80, 159.25), - pose(5.39, 5.91, 137.86)); - var midOneOneTraj = - oneThenOneBallTraj( - drive, - cargo, - ramsetePrototype.name("midOneOne"), - pose(7.56, 3.00, -111.80), - pose(5.58, 2.00, -173.42)); - var bottomOneOneTraj = - oneThenOneBallTraj( - drive, - cargo, - ramsetePrototype.name("bottomOneOne"), - pose(8.01, 2.82, -111.80), - pose(7.67, 0.77, -91.97)); - - // Start at the edge at the top, collect the top ball, then come back and spit - var topTwoBallTraj = - twoBallTraj( - drive, - cargo, - ramsetePrototype.name("topTwo"), - pose(6.07, 5.12, 134.24), - pose(5.34, 5.89, 130.31), - reverseHeading(pose(7.04, 4.58, -22.25))); - // Start at the edge at the bottom, collect the bottom ball, then come back and spit - var bottomTwoBallTraj = - twoBallTraj( - drive, - cargo, - ramsetePrototype.name("bottomTwo"), - pose(7.55, 1.83, -88.32), - pose(7.63, 0.76, -86.19), - reverseHeading(pose(8.01, 2.82, 68.63))); - - var testPath = - new InstantCommand(cargo::runIntake, cargo) - .andThen(ramsetePrototype.copy().name("testtraj").traj(testTraj(drive)).build()) - .andThen(spit.get()); - // (assume blue alliance) // Start at bottom on edge of tape, get one ball, score that and the preloaded one, go back for // another ball, then score that @@ -487,8 +563,8 @@ public static RobotMap createRobotMap() { // cargo))) // .andThen(spit.get()); - //Auto - List autoStartupCommands = List.of(resetDriveOdometry.get(), testPath); + // Auto + List autoStartupCommands = List.of(resetDriveOdometry.get(), threeBallAuto); List robotStartupCommands = List.of(); @@ -615,4 +691,4 @@ private static Command twoBallTraj( } } -//Hi there! \ No newline at end of file +// Hi there! From 4b04982b7f755708f2799fa7881978dfb4869f65 Mon Sep 17 00:00:00 2001 From: ysthakur <45539777+ysthakur@users.noreply.github.com> Date: Sun, 6 Mar 2022 10:11:59 -0500 Subject: [PATCH 11/25] Make climber commands reset --- .../_2022robot/climber/PivotingTelescopingClimber.java | 6 ++++++ src/main/java/frc/team449/javaMaps/FullMap.java | 8 +++++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java index 57c44a4f..765f98f7 100644 --- a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java +++ b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java @@ -108,6 +108,12 @@ public void stop() { this.rightArm.stop(); } + /** Clear the previous goal and go to this one */ + public void reset(double goal) { + this.leftArm.getController().reset(goal); + this.rightArm.getController().reset(goal); + } + public boolean atGoal() { return this.leftArm.getController().atGoal() && this.rightArm.getController().atGoal(); } diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 76122203..a1fe118b 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -323,8 +323,10 @@ public static RobotMap createRobotMap() { pivotPiston, RobotBase.isReal() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get - : () -> false, // todo this is janky af + : () -> false, // todo this is not great, simulate the Hall effect some other way? // did teddy right this cuz he's the only person I know that says "janky" + // He didn't, don't besmirch his name. Jank is my specialty, although this wasn't + // actually that bad CLIMBER_DISTANCE); // PUT YOUR SUBSYSTEM IN HERE AFTER INITIALIZING IT @@ -358,7 +360,7 @@ public static RobotMap createRobotMap() { // Move climber arm up new JoystickButton(climberJoystick, XboxController.Button.kA.value) .whenPressed( - new InstantCommand(() -> climber.setGoal(climber.distanceTopBottom), climber) + new InstantCommand(() -> climber.reset(climber.distanceTopBottom), leftArm, rightArm) .andThen(new WaitUntilCommand(climber::atGoal))); // .whileActiveContinuous( // new WaitCommand(0.01) @@ -366,7 +368,7 @@ public static RobotMap createRobotMap() { // Move climber arm down new JoystickButton(climberJoystick, XboxController.Button.kY.value) .whenPressed( - new InstantCommand(() -> climber.setGoal(0), climber) + new InstantCommand(() -> climber.reset(0), leftArm, rightArm) .andThen(new WaitUntilCommand(climber::atGoal)) .withInterrupt(climber::hitBottom) .andThen( From f86b0199ca5a4353cdf3c6b36ff2af111e1549a3 Mon Sep 17 00:00:00 2001 From: ysthakur <45539777+ysthakur@users.noreply.github.com> Date: Sun, 6 Mar 2022 10:48:31 -0500 Subject: [PATCH 12/25] Correct angles for 3 ball auto --- .../team449/auto/commands/RamseteBuilder.java | 29 ++++++--- .../java/frc/team449/javaMaps/FullMap.java | 65 +++++++++---------- 2 files changed, 52 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/team449/auto/commands/RamseteBuilder.java b/src/main/java/frc/team449/auto/commands/RamseteBuilder.java index de4956ca..458d34b3 100644 --- a/src/main/java/frc/team449/auto/commands/RamseteBuilder.java +++ b/src/main/java/frc/team449/auto/commands/RamseteBuilder.java @@ -25,7 +25,7 @@ public final class RamseteBuilder { private @Nullable String name; private double b = 2; private double zeta = .7; - private PIDAngleController pidAngleController; + private @Nullable PIDAngleController pidAngleController; private double angleTimeout = 0; /** Set the drive subsystem which is to be controlled */ @@ -150,12 +150,25 @@ public Command build() { var lastPose = traj.getStates().get(traj.getStates().size() - 1).poseMeters; - return new InstantCommand(() -> drivetrain.resetOdometry(traj.getInitialPose())) - .andThen(ramseteCmd) -// .andThen(() -> drivetrain.setVoltage(0, 0)) -// .andThen( -// new NavXTurnToAngle<>( -// lastPose.getRotation().getDegrees(), angleTimeout, drivetrain, pidAngleController)) - .andThen(() -> drivetrain.setVoltage(0, 0)); + var cmd = + new InstantCommand(() -> drivetrain.resetOdometry(traj.getInitialPose())) + .andThen(ramseteCmd) + .andThen(() -> drivetrain.setVoltage(0, 0)); + + // If angleTimeout is nonzero, then we want to turn after the Ramsete command is over + // to ensure our heading is right + if (angleTimeout > 0) { + assert pidAngleController != null + : "PIDAngleController must not be null if turning after Ramsete"; + return cmd.andThen( + new NavXTurnToAngle<>( + lastPose.getRotation().getDegrees(), + angleTimeout, + drivetrain, + pidAngleController)) + .andThen(() -> drivetrain.setVoltage(0, 0)); + } else { + return cmd; + } } } diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index a1fe118b..68338ff8 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -271,7 +271,6 @@ public static RobotMap createRobotMap() { Supplier spit = () -> new InstantCommand(cargo::spit, cargo).andThen(new WaitCommand(2)).andThen(cargo::stop); - var stopCargo = new InstantCommand(cargo::stop, cargo); var armPrototype = driveMasterPrototype @@ -360,7 +359,7 @@ public static RobotMap createRobotMap() { // Move climber arm up new JoystickButton(climberJoystick, XboxController.Button.kA.value) .whenPressed( - new InstantCommand(() -> climber.reset(climber.distanceTopBottom), leftArm, rightArm) + new InstantCommand(() -> climber.reset(climber.distanceTopBottom), climber) .andThen(new WaitUntilCommand(climber::atGoal))); // .whileActiveContinuous( // new WaitCommand(0.01) @@ -368,7 +367,7 @@ public static RobotMap createRobotMap() { // Move climber arm down new JoystickButton(climberJoystick, XboxController.Button.kY.value) .whenPressed( - new InstantCommand(() -> climber.reset(0), leftArm, rightArm) + new InstantCommand(() -> climber.reset(0), climber) .andThen(new WaitUntilCommand(climber::atGoal)) .withInterrupt(climber::hitBottom) .andThen( @@ -468,11 +467,11 @@ public static RobotMap createRobotMap() { var ballX = 7.65; var ballY = 0.69; var ballAngle = -92.44; - var turnAngle = -109.98; - var ballX2 = 5.42; - var ballY2 = 1.75; - var turnAngle2 = -159.03; - var hubPose = pose(7.43, 2.83, 46.64); + var turnAngle = 130.0; + var ballX2 = 5.55; + var ballY2 = 1.85; + var turnAngle2 = 200; + var hubPose = pose(7.43, 2.83, -180 + 46.64); var threeBallAuto = new InstantCommand(cargo::spit, cargo) .andThen(new WaitCommand(1)) @@ -490,7 +489,7 @@ public static RobotMap createRobotMap() { .build()) .andThen( new NavXTurnToAngle<>( - -turnAngle, 6, drive, pidAngleControllerPrototype.pid(0.001, 0, 0).build())) + turnAngle, 4, drive, pidAngleControllerPrototype.pid(0.001, 0, 0).build())) .andThen( ramsetePrototype .copy() @@ -499,33 +498,31 @@ public static RobotMap createRobotMap() { TrajectoryGenerator.generateTrajectory( pose(ballX, ballY, turnAngle), List.of(), - pose(ballX2, ballY2, 154.49), + pose(ballX2, ballY2, 160), trajConfig(drive))) + .build()) + .andThen( + new NavXTurnToAngle<>(turnAngle2, 4, drive, pidAngleControllerPrototype.build())) + .andThen( + ramsetePrototype + .name("threeBallAuto3") + .traj( + TrajectoryGenerator.generateTrajectory( + pose(ballX2, ballY2, turnAngle2), + List.of(), + hubPose, + trajConfig(drive).setReversed(true))) + .build()) + .andThen(cargo::spit, cargo) + .andThen(new WaitCommand(2)) + .andThen( + ramsetePrototype + .copy() + .name("threeBallAuto4") + .traj( + TrajectoryGenerator.generateTrajectory( + hubPose, List.of(), pose(5.70, 1.98, -140.73), trajConfig(drive))) .build()); - // .andThen( - // new NavXTurnToAngle<>(turnAngle2, 1, drive, - // pidAngleControllerPrototype.build())) - // .andThen( - // ramsetePrototype - // .name("threeBallAuto3") - // .traj( - // TrajectoryGenerator.generateTrajectory( - // pose(ballX2, ballY2, turnAngle2), - // List.of(), - // hubPose, - // trajConfig(drive))) - // .build()) - // .andThen(cargo::spit,cargo) - // .andThen(new WaitCommand(2)) - // .andThen(ramsetePrototype - // .name("threeBallAuto4") - // .traj( - // TrajectoryGenerator.generateTrajectory( - // hubPose, - // List.of(), - // pose(5.70,1.98,-140.73), - // trajConfig(drive))) - // .build()); // var randomTestAuto = // ramsetePrototype From 19e7128b290000bfb26f1c7bafe4e3483ed2a672 Mon Sep 17 00:00:00 2001 From: Teddy Date: Mon, 7 Mar 2022 11:21:33 -0500 Subject: [PATCH 13/25] Testing climber --- src/main/deploy/pathplanner/1ball blue 2.path | 2 +- .../team449/_2022robot/cargo/Cargo2022.java | 4 +- .../_2022robot/climber/ClimberArm.java | 20 ++- .../climber/PivotingTelescopingClimber.java | 9 +- .../java/frc/team449/javaMaps/FullMap.java | 169 ++++++++++-------- 5 files changed, 127 insertions(+), 77 deletions(-) diff --git a/src/main/deploy/pathplanner/1ball blue 2.path b/src/main/deploy/pathplanner/1ball blue 2.path index 84a94d4a..600a6d61 100644 --- a/src/main/deploy/pathplanner/1ball blue 2.path +++ b/src/main/deploy/pathplanner/1ball blue 2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":7.985559031614295,"y":2.811541566142419},"prevControl":null,"nextControl":{"x":7.6341163358463415,"y":1.8450741527805459},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.643878632948881,"y":0.6638362031207639},"prevControl":{"x":6.940993241412974,"y":0.9957543046793873},"nextControl":{"x":6.940993241412974,"y":0.9957543046793873},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.418074893085174,"y":1.7474511817386211},"prevControl":{"x":6.003812719365096,"y":1.9719840151459256},"nextControl":{"x":6.003812719365096,"y":1.9719840151459256},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.429108096646242,"y":2.8310661603564795},"prevControl":{"x":7.0971899950876205,"y":2.4796234645885256},"nextControl":{"x":7.761026198204864,"y":3.1825088561244335},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.36628861869412,"y":4.334459914474949},"prevControl":{"x":8.063657408449492,"y":3.758484385299691},"nextControl":{"x":8.66891982893875,"y":4.910435443650207},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.7207061033298,"y":1.9622217180412602},"prevControl":{"x":7.219218708895938,"y":3.1873900046767654},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.4,"isReversed":false} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":7.985559031614295,"y":2.811541566142419},"prevControl":null,"nextControl":{"x":7.6341163358463415,"y":1.8450741527805459},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.643878632948881,"y":0.6638362031207639},"prevControl":{"x":6.940993241412974,"y":0.9957543046793873},"nextControl":{"x":6.940993241412974,"y":0.9957543046793873},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.418074893085174,"y":1.7474511817386211},"prevControl":{"x":6.003812719365096,"y":1.9719840151459256},"nextControl":{"x":6.003812719365096,"y":1.9719840151459256},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.429108096646242,"y":2.8310661603564795},"prevControl":{"x":7.0971899950876205,"y":2.4796234645885256},"nextControl":{"x":7.761026198204864,"y":3.1825088561244335},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.36628861869412,"y":4.334459914474949},"prevControl":{"x":8.063657408449492,"y":3.758484385299691},"nextControl":{"x":8.66891982893875,"y":4.910435443650207},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.7207061033298,"y":1.9622217180412602},"prevControl":{"x":7.219218708895938,"y":3.1873900046767654},"nextControl":{"x":4.222193497763661,"y":0.7370534314057551},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.379025704670627,"y":4.8713862552280265},"prevControl":{"x":4.95680635489179,"y":1.4667851399777314},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.4,"isReversed":false} \ No newline at end of file diff --git a/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java b/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java index dbf7ae5a..f8291185 100644 --- a/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java +++ b/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java @@ -32,7 +32,7 @@ public Cargo2022( public void runIntake() { intakeMotor.set(intakeSpeed); - spitterMotor.set(-spitterSpeed); + spitterMotor.set(spitterSpeed); } // public void runIntakeReverse() { @@ -42,7 +42,7 @@ public void runIntake() { public void spit() { intakeMotor.set(intakeSpeed); - spitterMotor.set(spitterSpeed); + spitterMotor.set(-spitterSpeed); } public void stop() { diff --git a/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java b/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java index 9ee7c34e..d91ad6b0 100644 --- a/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java +++ b/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java @@ -5,10 +5,11 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; import frc.team449.motor.WrappedMotor; +import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; import org.jetbrains.annotations.NotNull; -public class ClimberArm extends ProfiledPIDSubsystem { +public class ClimberArm extends ProfiledPIDSubsystem implements Loggable { private final WrappedMotor motor; private final ElevatorFeedforward feedforward; @@ -31,13 +32,23 @@ protected double getMeasurement() { return this.motor.getPositionUnits(); } + @Log + public double getGoal() { + return getController().getGoal().position; + } + + @Log + public double getSetpoint() { + return getController().getSetpoint().position; + } + @Log public double getError() { return this.getController().getPositionError(); } public void stop() { - this.getController().reset(0); + this.getController().reset(getMeasurement()); } /** @@ -47,4 +58,9 @@ public void stop() { public void set(double velocity) { this.motor.set(velocity); } + + @Override + public String configureLogName() { + return "ClimberArm" + motor.configureLogName(); + } } diff --git a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java index 765f98f7..ec85306f 100644 --- a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java +++ b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java @@ -110,8 +110,13 @@ public void stop() { /** Clear the previous goal and go to this one */ public void reset(double goal) { - this.leftArm.getController().reset(goal); - this.rightArm.getController().reset(goal); + this.leftArm.getController().reset(this.leftArm.getMeasurement()); + this.rightArm.getController().reset(this.rightArm.getMeasurement()); + this.leftArm.setGoal(goal); + this.rightArm.setGoal(goal); + + this.goal = goal; + System.out.println("resetting to goal " + goal); } public boolean atGoal() { diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 68338ff8..d044a5a6 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -94,9 +94,13 @@ public class FullMap { DRIVE_KD_VEL = 0, DRIVE_KP_POS = 45.269, DRIVE_KD_POS = 3264.2, - DRIVE_FF_KS = 0.15084, - DRIVE_FF_KV = 2.4303, - DRIVE_FF_KA = 0.5323; + DRIVE_FF_KS = 0.19993, + DRIVE_FF_KV = 2.3776, + DRIVE_FF_KA = 0.31082; + // todo actually use these feedforward values + public static final double DRIVE_ANGLE_FF_KS = 0.59239, + DRIVE_ANGLE_FF_KV = 25.315, + DRIVE_ANGLE_FF_KA = 145.68; // old value from measuring from the outside of the wheel: 0.6492875 // measuring from the inside of the wheel : .57785 public static final double DRIVE_TRACK_WIDTH = 0.6492875; @@ -115,7 +119,7 @@ public class FullMap { CLIMBER_FF_KA = 0, CLIMBER_FF_KG = 0; // Intake - public static final int INTAKE_PISTON_FWD_CHANNEL = 2, INTAKE_PISTON_REV_CHANNEL = 3; + public static final int INTAKE_PISTON_FWD_CHANNEL = 3, INTAKE_PISTON_REV_CHANNEL = 2; // todo find out what the channel numbers are private FullMap() {} @@ -268,9 +272,6 @@ public static RobotMap createRobotMap() { deployIntake, INTAKE_SPEED, SPITTER_SPEED); - Supplier spit = - () -> - new InstantCommand(cargo::spit, cargo).andThen(new WaitCommand(2)).andThen(cargo::stop); var armPrototype = driveMasterPrototype @@ -278,6 +279,7 @@ public static RobotMap createRobotMap() { .setRevSoftLimit(0.) .setFwdSoftLimit(CLIMBER_DISTANCE) .setUnitPerRotation(0.1949) + .setPostEncoderGearing(27) .setEnableBrakeMode(true); var leftArm = new ClimberArm( @@ -285,7 +287,8 @@ public static RobotMap createRobotMap() { .copy() .setName("climber_left") .setPort(LEFT_CLIMBER_MOTOR_PORT) - .setPostEncoderGearing(10) + .setUnitPerRotation(0.1778) + // checked 3/6/22 by Matthew N .setReverseOutput(true) .createReal(), new ProfiledPIDController( @@ -296,13 +299,23 @@ public static RobotMap createRobotMap() { new ElevatorFeedforward(CLIMBER_FF_KS, CLIMBER_FF_KG, CLIMBER_FF_KV, CLIMBER_FF_KA)); var rightArm = new ClimberArm( - driveMasterPrototype + armPrototype .copy() .setName("climber_right") .setPort(RIGHT_CLIMBER_MOTOR_PORT) - .setPostEncoderGearing(10) + .setUnitPerRotation(0.2286) + // checked 3/6/22 by Matthew N .setReverseOutput(false) .createReal(), + + // driveMasterPrototype + // .copy() + // .setName("climber_right") + // .setPort(RIGHT_CLIMBER_MOTOR_PORT) + // .setUnitPerRotation(0.2286) + // //checked 3/6/22 by Matthew N + // .setReverseOutput(false) + // .createReal(), new ProfiledPIDController( CLIMBER_KP, 0, @@ -357,15 +370,18 @@ public static RobotMap createRobotMap() { // .whenReleased(cargo::stop, cargo); // Move climber arm up - new JoystickButton(climberJoystick, XboxController.Button.kA.value) + new JoystickButton(climberJoystick, XboxController.Button.kY.value) .whenPressed( new InstantCommand(() -> climber.reset(climber.distanceTopBottom), climber) - .andThen(new WaitUntilCommand(climber::atGoal))); - // .whileActiveContinuous( - // new WaitCommand(0.01) - // .andThen(() -> climber.setGoal(climber.getGoal() + 0.01), climber)); + .andThen(new WaitUntilCommand(climber::atGoal)) + .andThen(climber::stop, climber)); + // .whileHeld( + // new WaitCommand(0.01) + // .andThen(() -> rightArm.set(0.1), + // climber))//climber.setGoal(climber.getGoal() + 0.01), climber)); + // .whenReleased(() -> rightArm.set(0), climber); // Move climber arm down - new JoystickButton(climberJoystick, XboxController.Button.kY.value) + new JoystickButton(climberJoystick, XboxController.Button.kA.value) .whenPressed( new InstantCommand(() -> climber.reset(0), climber) .andThen(new WaitUntilCommand(climber::atGoal)) @@ -377,9 +393,11 @@ public static RobotMap createRobotMap() { climber.setState(PivotingTelescopingClimber.ClimberState.RETRACTED); } })); - // .whileActiveContinuous( - // new WaitCommand(0.01) - // .andThen(() -> climber.setGoal(climber.getGoal() - 0.01), climber)); + // .whileActiveContinuous( + // new WaitCommand(0.01) + // .andThen(() -> climber.setGoal(climber.getGoal() - 0.01), climber)); + // .whileHeld(() -> rightArm.set(-0.1), climber) + // .whenReleased(() -> rightArm.set(0), climber); // Extend climber arm out new JoystickButton(climberJoystick, XboxController.Button.kB.value) .whenPressed( @@ -408,66 +426,73 @@ public static RobotMap createRobotMap() { .pid(0.002, 0, 0) .build()) .angleTimeout(4) - .field(field); + .field(null); + // .field(field); + + Supplier spit = + () -> + new InstantCommand(cargo::spit, cargo) + .andThen(new WaitCommand(1)) + .andThen(cargo::stop, cargo); // (assume blue alliance) // Start at bottom next to hub, shoot preloaded ball, then get the two balls in that region and // score those - // var scoreThenGetTwoThenScore = - // new InstantCommand(cargo::runIntake, cargo) - // .andThen( - // ramsetePrototype - // .copy() - // .name("1-2ballbluebottom") - // .traj(loadPathPlannerTraj("New Path")) - // .build()) - // .andThen(spit.get()); + var scoreThenGetTwoThenScore = + new InstantCommand(cargo::runIntake, cargo) + .andThen( + ramsetePrototype + .copy() + .name("1-2ballbluebottom") + .traj(loadPathPlannerTraj("New Path")) + .build()) + .andThen(spit.get()); // Spit the preloaded ball, pick up another, come back and spit it out - // var topOneOneTraj = - // oneThenOneBallTraj( - // drive, - // cargo, - // ramsetePrototype.name("topOneOne"), - // pose(7.11, 4.80, 159.25), - // pose(5.39, 5.91, 137.86)); - // var midOneOneTraj = - // oneThenOneBallTraj( - // drive, - // cargo, - // ramsetePrototype.name("midOneOne"), - // pose(7.56, 3.00, -111.80), - // pose(5.58, 2.00, -173.42)); - // var bottomOneOneTraj = - // oneThenOneBallTraj( - // drive, - // cargo, - // ramsetePrototype.name("bottomOneOne"), - // pose(8.01, 2.82, -111.80), - // pose(7.67, 0.77, -91.97)); - // + var topOneOneTraj = + oneThenOneBallTraj( + drive, + cargo, + ramsetePrototype.name("topOneOne"), + pose(7.11, 4.80, 159.25), + pose(5.39, 5.91, 137.86)); + var midOneOneTraj = + oneThenOneBallTraj( + drive, + cargo, + ramsetePrototype.name("midOneOne"), + pose(7.56, 3.00, -111.80), + pose(5.58, 2.00, -173.42)); + var bottomOneOneTraj = + oneThenOneBallTraj( + drive, + cargo, + ramsetePrototype.name("bottomOneOne"), + pose(8.01, 2.82, -111.80), + pose(7.67, 0.77, -91.97)); + // // Start at the edge at the top, collect the top ball, then come back and spit - // var topTwoBallTraj = - // twoBallTraj( - // drive, - // cargo, - // ramsetePrototype.name("topTwo"), - // pose(6.07, 5.12, 134.24), - // pose(5.34, 5.89, 130.31), - // reverseHeading(pose(7.04, 4.58, -22.25))); + var topTwoBallTraj = + twoBallTraj( + drive, + cargo, + ramsetePrototype.name("topTwo"), + pose(6.07, 5.12, 134.24), + pose(5.34, 5.89, 130.31), + reverseHeading(pose(7.04, 4.58, -22.25))); // // Start at the edge at the bottom, collect the bottom ball, then come back and spit - // var bottomTwoBallTraj = - // twoBallTraj( - // drive, - // cargo, - // ramsetePrototype.name("bottomTwo"), - // pose(7.55, 1.83, -88.32), - // pose(7.63, 0.76, -86.19), - // reverseHeading(pose(8.01, 2.82, 68.63))); + var bottomTwoBallTraj = + twoBallTraj( + drive, + cargo, + ramsetePrototype.name("bottomTwo"), + pose(7.55, 1.83, -88.32), + pose(7.63, 0.76, -86.19), + reverseHeading(pose(8.01, 2.82, 68.63))); var ballX = 7.65; var ballY = 0.69; var ballAngle = -92.44; - var turnAngle = 130.0; + var turnAngle = 170.0; var ballX2 = 5.55; var ballY2 = 1.85; var turnAngle2 = 200; @@ -480,6 +505,7 @@ public static RobotMap createRobotMap() { ramsetePrototype .copy() .name("threeBallAuto1") + .field(field) .traj( TrajectoryGenerator.generateTrajectory( pose(7.99, 2.81, -109.98), @@ -494,11 +520,12 @@ public static RobotMap createRobotMap() { ramsetePrototype .copy() .name("threeBallAuto2") + .field(field) .traj( TrajectoryGenerator.generateTrajectory( pose(ballX, ballY, turnAngle), List.of(), - pose(ballX2, ballY2, 160), + pose(ballX2, ballY2, 140), trajConfig(drive))) .build()) .andThen( @@ -506,6 +533,7 @@ public static RobotMap createRobotMap() { .andThen( ramsetePrototype .name("threeBallAuto3") + .field(field) .traj( TrajectoryGenerator.generateTrajectory( pose(ballX2, ballY2, turnAngle2), @@ -519,9 +547,10 @@ public static RobotMap createRobotMap() { ramsetePrototype .copy() .name("threeBallAuto4") + .field(field) .traj( TrajectoryGenerator.generateTrajectory( - hubPose, List.of(), pose(5.70, 1.98, -140.73), trajConfig(drive))) + hubPose, List.of(), pose(5.70, 1.98, -90), trajConfig(drive))) .build()); // var randomTestAuto = From 3f5052d48a8ce3abfbd24eb9ac06e9d865a2c13b Mon Sep 17 00:00:00 2001 From: Teddy Date: Mon, 7 Mar 2022 16:00:05 -0500 Subject: [PATCH 14/25] Added oneBallAuto and made constants for UPR --- src/main/deploy/pathplanner/1ball blue 2.path | 2 +- .../climber/PivotingTelescopingClimber.java | 3 ++- src/main/java/frc/team449/javaMaps/FullMap.java | 17 +++++++++++++---- 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/1ball blue 2.path b/src/main/deploy/pathplanner/1ball blue 2.path index 600a6d61..0bf32c47 100644 --- a/src/main/deploy/pathplanner/1ball blue 2.path +++ b/src/main/deploy/pathplanner/1ball blue 2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":7.985559031614295,"y":2.811541566142419},"prevControl":null,"nextControl":{"x":7.6341163358463415,"y":1.8450741527805459},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.643878632948881,"y":0.6638362031207639},"prevControl":{"x":6.940993241412974,"y":0.9957543046793873},"nextControl":{"x":6.940993241412974,"y":0.9957543046793873},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.418074893085174,"y":1.7474511817386211},"prevControl":{"x":6.003812719365096,"y":1.9719840151459256},"nextControl":{"x":6.003812719365096,"y":1.9719840151459256},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.429108096646242,"y":2.8310661603564795},"prevControl":{"x":7.0971899950876205,"y":2.4796234645885256},"nextControl":{"x":7.761026198204864,"y":3.1825088561244335},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.36628861869412,"y":4.334459914474949},"prevControl":{"x":8.063657408449492,"y":3.758484385299691},"nextControl":{"x":8.66891982893875,"y":4.910435443650207},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.7207061033298,"y":1.9622217180412602},"prevControl":{"x":7.219218708895938,"y":3.1873900046767654},"nextControl":{"x":4.222193497763661,"y":0.7370534314057551},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.379025704670627,"y":4.8713862552280265},"prevControl":{"x":4.95680635489179,"y":1.4667851399777314},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.4,"isReversed":false} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":7.985559031614295,"y":2.811541566142419},"prevControl":null,"nextControl":{"x":7.6341163358463415,"y":1.8450741527805459},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.643878632948881,"y":0.6638362031207639},"prevControl":{"x":6.940993241412974,"y":0.9957543046793873},"nextControl":{"x":6.940993241412974,"y":0.9957543046793873},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.418074893085174,"y":1.7474511817386211},"prevControl":{"x":6.003812719365096,"y":1.9719840151459256},"nextControl":{"x":6.003812719365096,"y":1.9719840151459256},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.429108096646242,"y":2.8310661603564795},"prevControl":{"x":7.0971899950876205,"y":2.4796234645885256},"nextControl":{"x":7.761026198204864,"y":3.1825088561244335},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.755053484790515,"y":2.8936766734292965},"prevControl":{"x":7.523934314387897,"y":2.28546833026451},"nextControl":{"x":7.974972911971131,"y":3.472412008115153},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.7207061033298,"y":1.9622217180412602},"prevControl":{"x":7.219218708895938,"y":3.1873900046767654},"nextControl":{"x":4.222193497763661,"y":0.7370534314057551},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.379025704670627,"y":4.8713862552280265},"prevControl":{"x":4.95680635489179,"y":1.4667851399777314},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.4,"isReversed":false} \ No newline at end of file diff --git a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java index ec85306f..a0b3443c 100644 --- a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java +++ b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java @@ -109,7 +109,8 @@ public void stop() { } /** Clear the previous goal and go to this one */ - public void reset(double goal) { + public void + reset(double goal) { this.leftArm.getController().reset(this.leftArm.getMeasurement()); this.rightArm.getController().reset(this.rightArm.getMeasurement()); this.leftArm.setGoal(goal); diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index d044a5a6..2e8813ae 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -38,6 +38,7 @@ import frc.team449.drive.DriveSettingsBuilder; import frc.team449.drive.unidirectional.DriveUnidirectionalWithGyro; import frc.team449.drive.unidirectional.commands.AHRS.NavXTurnToAngle; +import frc.team449.drive.unidirectional.commands.DriveAtSpeed; import frc.team449.drive.unidirectional.commands.UnidirectionalNavXDefaultDrive; import frc.team449.generalInterfaces.doubleUnaryOperator.Polynomial; import frc.team449.generalInterfaces.doubleUnaryOperator.RampComponent; @@ -83,7 +84,7 @@ public class FullMap { // Limelight public static final int DRIVER_PIPELINE = 0; // TODO find out what this is! // Speeds - public static final double INTAKE_SPEED = 0.4, SPITTER_SPEED = 0.5; + public static final double INTAKE_SPEED = 0.6, SPITTER_SPEED = 0.6; public static final double AUTO_MAX_SPEED = 1.9, AUTO_MAX_ACCEL = .4; // Drive constants public static final double DRIVE_WHEEL_RADIUS = Units.inchesToMeters(2); @@ -118,6 +119,9 @@ public class FullMap { CLIMBER_FF_KV = 0, CLIMBER_FF_KA = 0, CLIMBER_FF_KG = 0; + public static final double CLIMBER_LEFT_UPR = 0.1778, + CLIMBER_RIGHT_UPR = 0.2286; + // Intake public static final int INTAKE_PISTON_FWD_CHANNEL = 3, INTAKE_PISTON_REV_CHANNEL = 2; // todo find out what the channel numbers are @@ -287,7 +291,7 @@ public static RobotMap createRobotMap() { .copy() .setName("climber_left") .setPort(LEFT_CLIMBER_MOTOR_PORT) - .setUnitPerRotation(0.1778) + .setUnitPerRotation(CLIMBER_LEFT_UPR) // checked 3/6/22 by Matthew N .setReverseOutput(true) .createReal(), @@ -303,7 +307,7 @@ public static RobotMap createRobotMap() { .copy() .setName("climber_right") .setPort(RIGHT_CLIMBER_MOTOR_PORT) - .setUnitPerRotation(0.2286) + .setUnitPerRotation(CLIMBER_RIGHT_UPR) // checked 3/6/22 by Matthew N .setReverseOutput(false) .createReal(), @@ -552,6 +556,11 @@ public static RobotMap createRobotMap() { TrajectoryGenerator.generateTrajectory( hubPose, List.of(), pose(5.70, 1.98, -90), trajConfig(drive))) .build()); + var oneBallAuto = + new InstantCommand(cargo::spit, cargo) + .andThen(new WaitCommand(1)) + .andThen(cargo::stop,cargo) + .andThen(new DriveAtSpeed<>(drive,0.18,2)); // var randomTestAuto = // ramsetePrototype @@ -592,7 +601,7 @@ public static RobotMap createRobotMap() { // .andThen(spit.get()); // Auto - List autoStartupCommands = List.of(resetDriveOdometry.get(), threeBallAuto); + List autoStartupCommands = List.of(new InstantCommand(() -> drive.resetOdometry(pose(7.76, 2.89, 249.19)), drive).andThen(oneBallAuto)); List robotStartupCommands = List.of(); From cf35290ef637d551291e8e4dd287e4d805e7509a Mon Sep 17 00:00:00 2001 From: Teddy Date: Tue, 8 Mar 2022 11:52:59 -0500 Subject: [PATCH 15/25] Use bumpers for intake/spit --- src/main/java/frc/team449/javaMaps/FullMap.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 2e8813ae..c41701de 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -355,11 +355,11 @@ public static RobotMap createRobotMap() { // Button bindings here // Take in balls but don't shoot - new JoystickButton(cargoJoystick, XboxController.Button.kA.value) + new JoystickButton(cargoJoystick, XboxController.Button.kLeftBumper.value) .whileHeld(cargo::runIntake, cargo) .whenReleased(cargo::stop, cargo); // Run all motors in intake to spit balls out - new JoystickButton(cargoJoystick, XboxController.Button.kY.value) + new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) .whileHeld(cargo::spit, cargo) .whenReleased(cargo::stop, cargo); // Stow/retract intake From cacf4c946a7d467f770d9933fdd8e232c5a9b963 Mon Sep 17 00:00:00 2001 From: Teddy Date: Tue, 8 Mar 2022 18:05:27 -0500 Subject: [PATCH 16/25] Make climber use voltage control with PID for holding --- .../_2022robot/climber/ClimberArm.java | 2 +- .../climber/PivotingTelescopingClimber.java | 11 +++- .../java/frc/team449/javaMaps/FullMap.java | 62 +++++++++---------- 3 files changed, 41 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java b/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java index d91ad6b0..23421ce2 100644 --- a/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java +++ b/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java @@ -28,7 +28,7 @@ protected void useOutput(double output, TrapezoidProfile.State setpoint) { } @Override - protected double getMeasurement() { + public double getMeasurement() { return this.motor.getPositionUnits(); } diff --git a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java index a0b3443c..1b3b321d 100644 --- a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java +++ b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java @@ -78,6 +78,14 @@ public void setGoal(double goal) { this.rightArm.setGoal(goal); } + public void hold() { + leftArm.getController().reset(leftArm.getMeasurement()); + rightArm.getController().reset(rightArm.getMeasurement()); + leftArm.setGoal(leftArm.getMeasurement()); + rightArm.setGoal(rightArm.getMeasurement()); + this.enable(); + } + public void pivotTelescopingArmOut() { pivotPiston.set(DoubleSolenoid.Value.kReverse); } @@ -109,8 +117,7 @@ public void stop() { } /** Clear the previous goal and go to this one */ - public void - reset(double goal) { + public void reset(double goal) { this.leftArm.getController().reset(this.leftArm.getMeasurement()); this.rightArm.getController().reset(this.rightArm.getMeasurement()); this.leftArm.setGoal(goal); diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index c41701de..4ae2d2a0 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -113,14 +113,14 @@ public class FullMap { // todo find out what the channel numbers are public static final int CLIMBER_SENSOR_CHANNEL = 6; // todo find out what this really is public static final double CLIMBER_MAX_VEL = 0.1, CLIMBER_MAX_ACCEL = 0.1; - public static final double CLIMBER_DISTANCE = 0.5; - public static final double CLIMBER_KP = 12; + public static final double CLIMBER_DISTANCE = 0.7, CLIMBER_MID_DISTANCE = 0.6; + public static final double CLIMBER_KP = 30; //600; public static final double CLIMBER_FF_KS = 0, CLIMBER_FF_KV = 0, CLIMBER_FF_KA = 0, CLIMBER_FF_KG = 0; - public static final double CLIMBER_LEFT_UPR = 0.1778, - CLIMBER_RIGHT_UPR = 0.2286; + public static final double CLIMBER_LEFT_UPR = 0.239, // 0.1778, + CLIMBER_RIGHT_UPR = 0.239; // 0.2286; // Intake public static final int INTAKE_PISTON_FWD_CHANNEL = 3, INTAKE_PISTON_REV_CHANNEL = 2; @@ -337,12 +337,7 @@ public static RobotMap createRobotMap() { leftArm, rightArm, pivotPiston, - RobotBase.isReal() - ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get - : () -> false, // todo this is not great, simulate the Hall effect some other way? - // did teddy right this cuz he's the only person I know that says "janky" - // He didn't, don't besmirch his name. Jank is my specialty, although this wasn't - // actually that bad + RobotBase.isReal() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get : () -> false, CLIMBER_DISTANCE); // PUT YOUR SUBSYSTEM IN HERE AFTER INITIALIZING IT @@ -373,12 +368,14 @@ public static RobotMap createRobotMap() { // .whileHeld(cargo::runIntakeReverse, cargo) // .whenReleased(cargo::stop, cargo); - // Move climber arm up + // Move climber arms up enough for mid rung + // new JoystickButton(climberJoystick, XboxController.Button.kRightBumper.value) + // .whenPressed(() -> climber.reset(CLIMBER_MID_DISTANCE), climber); + // Move climber arm up enough for high rung new JoystickButton(climberJoystick, XboxController.Button.kY.value) - .whenPressed( - new InstantCommand(() -> climber.reset(climber.distanceTopBottom), climber) - .andThen(new WaitUntilCommand(climber::atGoal)) - .andThen(climber::stop, climber)); + .whenPressed(new InstantCommand(() -> climber.disable(), climber) + .andThen(new InstantCommand(() -> climber.set(.6), climber))) + .whenReleased(climber::hold, climber); // .whileHeld( // new WaitCommand(0.01) // .andThen(() -> rightArm.set(0.1), @@ -386,17 +383,9 @@ public static RobotMap createRobotMap() { // .whenReleased(() -> rightArm.set(0), climber); // Move climber arm down new JoystickButton(climberJoystick, XboxController.Button.kA.value) - .whenPressed( - new InstantCommand(() -> climber.reset(0), climber) - .andThen(new WaitUntilCommand(climber::atGoal)) - .withInterrupt(climber::hitBottom) - .andThen( - () -> { - if (climber.hitBottom()) { - climber.stop(); - climber.setState(PivotingTelescopingClimber.ClimberState.RETRACTED); - } - })); + .whenPressed(new InstantCommand(() -> climber.disable(), climber) + .andThen(new InstantCommand(() -> climber.set(-.6), climber))) + .whenReleased(climber::hold, climber); // .whileActiveContinuous( // new WaitCommand(0.01) // .andThen(() -> climber.setGoal(climber.getGoal() - 0.01), climber)); @@ -410,6 +399,14 @@ public static RobotMap createRobotMap() { // Retract climber arm in with piston new JoystickButton(climberJoystick, XboxController.Button.kX.value) .whenPressed(climber::pivotTelescopingArmIn); + // Stop climber + // new JoystickButton(climberJoystick, XboxController.Button.kRightBumper.value) + // .whenPressed(climber::stop, climber); + // Manual control to go down + new JoystickButton(climberJoystick, XboxController.Button.kStart.value) + .whenPressed(climber::disable, climber) + .whileHeld(() -> climber.set(-0.1), climber) + .whenReleased(() -> climber.set(0), climber); var ramsetePrototype = new RamseteBuilder() @@ -557,10 +554,10 @@ public static RobotMap createRobotMap() { hubPose, List.of(), pose(5.70, 1.98, -90), trajConfig(drive))) .build()); var oneBallAuto = - new InstantCommand(cargo::spit, cargo) - .andThen(new WaitCommand(1)) - .andThen(cargo::stop,cargo) - .andThen(new DriveAtSpeed<>(drive,0.18,2)); + new InstantCommand(cargo::spit, cargo) + .andThen(new WaitCommand(1)) + .andThen(cargo::stop, cargo) + .andThen(new DriveAtSpeed<>(drive, 0.18, 2)); // var randomTestAuto = // ramsetePrototype @@ -601,7 +598,10 @@ public static RobotMap createRobotMap() { // .andThen(spit.get()); // Auto - List autoStartupCommands = List.of(new InstantCommand(() -> drive.resetOdometry(pose(7.76, 2.89, 249.19)), drive).andThen(oneBallAuto)); + List autoStartupCommands = + List.of( + new InstantCommand(() -> drive.resetOdometry(pose(7.76, 2.89, 249.19)), drive) + .andThen(oneBallAuto)); List robotStartupCommands = List.of(); From f756e2721fd59ab75652fb64c4065fbba99af985 Mon Sep 17 00:00:00 2001 From: Teddy Date: Thu, 10 Mar 2022 15:27:47 -0500 Subject: [PATCH 17/25] Adjust ramping --- .../java/frc/team449/javaMaps/FullMap.java | 30 +++++++++++-------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 4ae2d2a0..9ffad7b7 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -114,7 +114,7 @@ public class FullMap { public static final int CLIMBER_SENSOR_CHANNEL = 6; // todo find out what this really is public static final double CLIMBER_MAX_VEL = 0.1, CLIMBER_MAX_ACCEL = 0.1; public static final double CLIMBER_DISTANCE = 0.7, CLIMBER_MID_DISTANCE = 0.6; - public static final double CLIMBER_KP = 30; //600; + public static final double CLIMBER_KP = 500; //600; public static final double CLIMBER_FF_KS = 0, CLIMBER_FF_KV = 0, CLIMBER_FF_KA = 0, @@ -221,7 +221,7 @@ public static RobotMap createRobotMap() { .axis(XboxController.Axis.kRightTrigger.value) .inverted(false) .build())), - new RampComponent(.6, .50)); + new RampComponent(1, 1.5)); var oi = new OIArcadeWithDPad( rotThrottle, @@ -373,9 +373,10 @@ public static RobotMap createRobotMap() { // .whenPressed(() -> climber.reset(CLIMBER_MID_DISTANCE), climber); // Move climber arm up enough for high rung new JoystickButton(climberJoystick, XboxController.Button.kY.value) - .whenPressed(new InstantCommand(() -> climber.disable(), climber) - .andThen(new InstantCommand(() -> climber.set(.6), climber))) - .whenReleased(climber::hold, climber); + .whenPressed( + new InstantCommand(() -> climber.reset(climber.distanceTopBottom), climber) + .andThen(new WaitUntilCommand(climber::atGoal)) + .andThen(climber::stop, climber)); // .whileHeld( // new WaitCommand(0.01) // .andThen(() -> rightArm.set(0.1), @@ -383,14 +384,17 @@ public static RobotMap createRobotMap() { // .whenReleased(() -> rightArm.set(0), climber); // Move climber arm down new JoystickButton(climberJoystick, XboxController.Button.kA.value) - .whenPressed(new InstantCommand(() -> climber.disable(), climber) - .andThen(new InstantCommand(() -> climber.set(-.6), climber))) - .whenReleased(climber::hold, climber); - // .whileActiveContinuous( - // new WaitCommand(0.01) - // .andThen(() -> climber.setGoal(climber.getGoal() - 0.01), climber)); - // .whileHeld(() -> rightArm.set(-0.1), climber) - // .whenReleased(() -> rightArm.set(0), climber); + .whenPressed( + new InstantCommand(() -> climber.reset(0), climber) + .andThen(new WaitUntilCommand(climber::atGoal)) + .withInterrupt(climber::hitBottom) + .andThen( + () -> { + if (climber.hitBottom()) { + climber.stop(); + climber.setState(PivotingTelescopingClimber.ClimberState.RETRACTED); + } + })); // Extend climber arm out new JoystickButton(climberJoystick, XboxController.Button.kB.value) .whenPressed( From 609ab94163fb4b5c8a2557e2f69390db88554318 Mon Sep 17 00:00:00 2001 From: Teddy Date: Thu, 10 Mar 2022 16:46:57 -0500 Subject: [PATCH 18/25] Set soft limit if climber stowed or hall sensor active --- .../_2022robot/climber/ClimberArm.java | 8 ++- .../climber/PivotingTelescopingClimber.java | 37 +++++++++++-- .../java/frc/team449/javaMaps/FullMap.java | 55 +++++++++++-------- 3 files changed, 70 insertions(+), 30 deletions(-) diff --git a/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java b/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java index 23421ce2..e46cf9e1 100644 --- a/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java +++ b/src/main/java/frc/team449/_2022robot/climber/ClimberArm.java @@ -50,11 +50,9 @@ public double getError() { public void stop() { this.getController().reset(getMeasurement()); } - /** - * Directly set the velocity. Only for testing/debugging + * Only for testing/debugging */ - @Deprecated public void set(double velocity) { this.motor.set(velocity); } @@ -63,4 +61,8 @@ public void set(double velocity) { public String configureLogName() { return "ClimberArm" + motor.configureLogName(); } + + public void sirswagger21() { + System.out.println("Sirswagger21 my fav yt"); + } } diff --git a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java index 1b3b321d..5959ce11 100644 --- a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java +++ b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java @@ -15,6 +15,8 @@ public class PivotingTelescopingClimber extends SubsystemBase implements Loggabl /** Distance the climber can travel, in meters */ public final double distanceTopBottom; + public final double midDistance; + private final @NotNull ClimberArm rightArm; private @NotNull ClimberState state; private final @NotNull ClimberArm leftArm; @@ -36,11 +38,13 @@ public PivotingTelescopingClimber( @NotNull ClimberArm rightArm, @NotNull DoubleSolenoid pivotPiston, @NotNull BooleanSupplier hallSensor, - double distanceTopBottom) { + double distanceTopBottom, + double midDistance) { this.leftArm = leftArm; this.rightArm = rightArm; this.pivotPiston = pivotPiston; this.distanceTopBottom = distanceTopBottom; + this.midDistance = midDistance; this.hallSensor = new BooleanSupplierUpdatable(hallSensor, null); // Start arm retracted this.state = ClimberState.RETRACTED; @@ -94,11 +98,34 @@ public void pivotTelescopingArmIn() { pivotPiston.set(DoubleSolenoid.Value.kForward); } - /** Only for testing/debugging */ - @Deprecated + /** Directly set velocity. This method must be called every loop if you're using climber. + * However, sets velocity to 0 if: + *

    + *
  • Hall sensor is active and velocity is negative
    • + *
  • Arms are stowed, velocity is positive, and is beyond {@link PivotingTelescopingClimber#midDistance}
    • + *
+ */ public void set(double velocity) { - leftArm.set(velocity); - rightArm.set(velocity); + double leftVel = velocity; + double rightVel = velocity; + if (velocity <= 0 && this.hitBottom()) { + leftVel = rightVel = 0; + } else if (this.isStowed()) { + if (leftArm.getMeasurement() > midDistance) { + leftVel = 0; + } + if (rightArm.getMeasurement() > midDistance) { + rightVel = 0; + } + } + + leftArm.set(leftVel); + rightArm.set(rightVel); + } + + /** Whether the arms are vertical */ + public boolean isStowed() { + return pivotPiston.get() == DoubleSolenoid.Value.kForward; } public void enable() { diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 9ffad7b7..04bf8d4c 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -24,7 +24,10 @@ import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.team449.CommandContainer; import frc.team449.RobotMap; @@ -113,8 +116,9 @@ public class FullMap { // todo find out what the channel numbers are public static final int CLIMBER_SENSOR_CHANNEL = 6; // todo find out what this really is public static final double CLIMBER_MAX_VEL = 0.1, CLIMBER_MAX_ACCEL = 0.1; + public static final double CLIMBER_EXTEND_VEL = 0.1, CLIMBER_RETRACT_VEL = -0.3; public static final double CLIMBER_DISTANCE = 0.7, CLIMBER_MID_DISTANCE = 0.6; - public static final double CLIMBER_KP = 500; //600; + public static final double CLIMBER_KP = 500; // 600; public static final double CLIMBER_FF_KS = 0, CLIMBER_FF_KV = 0, CLIMBER_FF_KA = 0, @@ -338,7 +342,8 @@ public static RobotMap createRobotMap() { rightArm, pivotPiston, RobotBase.isReal() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get : () -> false, - CLIMBER_DISTANCE); + CLIMBER_DISTANCE, + CLIMBER_MID_DISTANCE); // PUT YOUR SUBSYSTEM IN HERE AFTER INITIALIZING IT var subsystems = List.of(drive, cargo, climber); @@ -373,10 +378,15 @@ public static RobotMap createRobotMap() { // .whenPressed(() -> climber.reset(CLIMBER_MID_DISTANCE), climber); // Move climber arm up enough for high rung new JoystickButton(climberJoystick, XboxController.Button.kY.value) - .whenPressed( - new InstantCommand(() -> climber.reset(climber.distanceTopBottom), climber) - .andThen(new WaitUntilCommand(climber::atGoal)) - .andThen(climber::stop, climber)); + .whenPressed(climber::disable, climber) + .whileHeld(() -> climber.set(CLIMBER_EXTEND_VEL), climber) + .whenReleased(() -> climber.set(0), climber); + // new JoystickButton(climberJoystick, XboxController.Button.kY.value) + // .whenPressed( + // new InstantCommand(() -> climber.reset(climber.distanceTopBottom), + // climber) + // .andThen(new WaitUntilCommand(climber::atGoal)) + // .andThen(climber::stop, climber)); // .whileHeld( // new WaitCommand(0.01) // .andThen(() -> rightArm.set(0.1), @@ -384,17 +394,22 @@ public static RobotMap createRobotMap() { // .whenReleased(() -> rightArm.set(0), climber); // Move climber arm down new JoystickButton(climberJoystick, XboxController.Button.kA.value) - .whenPressed( - new InstantCommand(() -> climber.reset(0), climber) - .andThen(new WaitUntilCommand(climber::atGoal)) - .withInterrupt(climber::hitBottom) - .andThen( - () -> { - if (climber.hitBottom()) { - climber.stop(); - climber.setState(PivotingTelescopingClimber.ClimberState.RETRACTED); - } - })); + .whenPressed(climber::disable, climber) + .whileHeld(() -> climber.set(CLIMBER_RETRACT_VEL), climber) + .whenReleased(() -> climber.set(0), climber); + // new JoystickButton(climberJoystick, XboxController.Button.kA.value) + // .whenPressed( + // new InstantCommand(() -> climber.reset(0), climber) + // .andThen(new WaitUntilCommand(climber::atGoal)) + // .withInterrupt(climber::hitBottom) + // .andThen( + // () -> { + // if (climber.hitBottom()) { + // climber.stop(); + // + // climber.setState(PivotingTelescopingClimber.ClimberState.RETRACTED); + // } + // })); // Extend climber arm out new JoystickButton(climberJoystick, XboxController.Button.kB.value) .whenPressed( @@ -407,10 +422,6 @@ public static RobotMap createRobotMap() { // new JoystickButton(climberJoystick, XboxController.Button.kRightBumper.value) // .whenPressed(climber::stop, climber); // Manual control to go down - new JoystickButton(climberJoystick, XboxController.Button.kStart.value) - .whenPressed(climber::disable, climber) - .whileHeld(() -> climber.set(-0.1), climber) - .whenReleased(() -> climber.set(0), climber); var ramsetePrototype = new RamseteBuilder() From 50f65bd95a49480c376c4aa4a8fa0b8ee7064a1a Mon Sep 17 00:00:00 2001 From: Teddy Date: Thu, 10 Mar 2022 18:24:54 -0500 Subject: [PATCH 19/25] tuned ramping --- src/main/java/frc/team449/javaMaps/FullMap.java | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 04bf8d4c..32fee605 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -225,7 +225,7 @@ public static RobotMap createRobotMap() { .axis(XboxController.Axis.kRightTrigger.value) .inverted(false) .build())), - new RampComponent(1, 1.5)); + new RampComponent(.9, 1.8)); var oi = new OIArcadeWithDPad( rotThrottle, @@ -296,7 +296,6 @@ public static RobotMap createRobotMap() { .setName("climber_left") .setPort(LEFT_CLIMBER_MOTOR_PORT) .setUnitPerRotation(CLIMBER_LEFT_UPR) - // checked 3/6/22 by Matthew N .setReverseOutput(true) .createReal(), new ProfiledPIDController( @@ -341,7 +340,7 @@ public static RobotMap createRobotMap() { leftArm, rightArm, pivotPiston, - RobotBase.isReal() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get : () -> false, + RobotBase.isSimulation() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get : () -> false, CLIMBER_DISTANCE, CLIMBER_MID_DISTANCE); @@ -355,18 +354,18 @@ public static RobotMap createRobotMap() { // Button bindings here // Take in balls but don't shoot - new JoystickButton(cargoJoystick, XboxController.Button.kLeftBumper.value) + new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) .whileHeld(cargo::runIntake, cargo) .whenReleased(cargo::stop, cargo); // Run all motors in intake to spit balls out - new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) + new JoystickButton(cargoJoystick, XboxController.Button.kLeftBumper.value) .whileHeld(cargo::spit, cargo) .whenReleased(cargo::stop, cargo); // Stow/retract intake - new JoystickButton(cargoJoystick, XboxController.Button.kX.value) + new JoystickButton(cargoJoystick, XboxController.Button.kY.value) .whenPressed(cargo::retractIntake); // Deploy intake - new JoystickButton(cargoJoystick, XboxController.Button.kB.value) + new JoystickButton(cargoJoystick, XboxController.Button.kA.value) .whenPressed(cargo::deployIntake); // Run intake in reverse to feed ball from top // new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) From 6e0a6af8723a12012bb2e22dfce50ef2b140193b Mon Sep 17 00:00:00 2001 From: Teddy Date: Fri, 11 Mar 2022 13:44:33 -0500 Subject: [PATCH 20/25] Added external encoders --- Driver.vpr | 68 +++++++++++++++++++ src/main/deploy/pathplanner/1ball blue 2.path | 2 +- .../java/frc/team449/javaMaps/FullMap.java | 43 +++++++----- 3 files changed, 94 insertions(+), 19 deletions(-) create mode 100644 Driver.vpr diff --git a/Driver.vpr b/Driver.vpr new file mode 100644 index 00000000..d2f7c8d9 --- /dev/null +++ b/Driver.vpr @@ -0,0 +1,68 @@ +area_max:100 +area_min:0.0017850625000000004 +area_similarity:0 +aspect_max:20.000000 +aspect_min:0.000000 +black_level:0 +blue_balance:1975 +calibration_type:0 +contour_grouping:0 +contour_sort_final:0 +convexity_max:100 +convexity_min:10 +corner_approx:5.000000 +crop_x_max:1.000000 +crop_x_min:-1.000000 +crop_y_max:1.000000 +crop_y_min:-1.000000 +cross_a_a:1 +cross_a_x:0 +cross_a_y:0 +cross_b_a:1 +cross_b_x:0 +cross_b_y:0 +desc:Driver +desired_contour_region:0 +dilation_steps:0 +direction_filter:0 +dual_close_sort_origin:0 +erosion_steps:0 +exposure:177 +force_convex:1 +hue_max:86 +hue_min:42 +image_flip:1 +image_source:0 +img_to_show:0 +intersection_filter:0 +invert_hue:0 +multigroup_max:7 +multigroup_min:1 +multigroup_rejector:0 +pipeline_led_enabled:0 +pipeline_led_power:60 +pipeline_res:0 +pipeline_type:0 +red_balance:1200 +roi_x:0.000000 +roi_y:0.000000 +sat_max:255 +sat_min:70 +send_corners:0 +send_raw_contours:0 +solve3d:0 +solve3d_algo:0 +solve3d_bindtarget:1 +solve3d_conf:0.990000 +solve3d_error:8 +solve3d_guess:0 +solve3d_iterations:50 +solve3d_precies:1 +solve3d_precise:0 +solve3d_zoffset:0.000000 +val_max:255 +val_min:70 +x_outlier_miqr:1.5 +y_max:1.000000 +y_min:-1.000000 +y_outlier_miqr:1.5 diff --git a/src/main/deploy/pathplanner/1ball blue 2.path b/src/main/deploy/pathplanner/1ball blue 2.path index 0bf32c47..8241bba1 100644 --- a/src/main/deploy/pathplanner/1ball blue 2.path +++ b/src/main/deploy/pathplanner/1ball blue 2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":7.985559031614295,"y":2.811541566142419},"prevControl":null,"nextControl":{"x":7.6341163358463415,"y":1.8450741527805459},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.643878632948881,"y":0.6638362031207639},"prevControl":{"x":6.940993241412974,"y":0.9957543046793873},"nextControl":{"x":6.940993241412974,"y":0.9957543046793873},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.418074893085174,"y":1.7474511817386211},"prevControl":{"x":6.003812719365096,"y":1.9719840151459256},"nextControl":{"x":6.003812719365096,"y":1.9719840151459256},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.429108096646242,"y":2.8310661603564795},"prevControl":{"x":7.0971899950876205,"y":2.4796234645885256},"nextControl":{"x":7.761026198204864,"y":3.1825088561244335},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.755053484790515,"y":2.8936766734292965},"prevControl":{"x":7.523934314387897,"y":2.28546833026451},"nextControl":{"x":7.974972911971131,"y":3.472412008115153},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.7207061033298,"y":1.9622217180412602},"prevControl":{"x":7.219218708895938,"y":3.1873900046767654},"nextControl":{"x":4.222193497763661,"y":0.7370534314057551},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.379025704670627,"y":4.8713862552280265},"prevControl":{"x":4.95680635489179,"y":1.4667851399777314},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.4,"isReversed":false} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":6.099870427588956,"y":5.127595065316713},"prevControl":null,"nextControl":{"x":5.613732746452835,"y":5.590583333065401},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":true},{"anchorPoint":{"x":5.30121566572247,"y":5.9262498271832},"prevControl":{"x":5.771998133946636,"y":5.528700187349458},"nextControl":{"x":4.780353864505197,"y":6.366088681544453},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.037421669780049,"y":4.502560903855985},"prevControl":{"x":7.685605244628211,"y":4.247917356594206},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.4,"isReversed":false} \ No newline at end of file diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 32fee605..a65d2330 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -76,7 +76,11 @@ public class FullMap { INTAKE_FOLLOWER_PORT = 10, SPITTER_PORT = 9, RIGHT_CLIMBER_MOTOR_PORT = 6, - LEFT_CLIMBER_MOTOR_PORT = 5; + LEFT_CLIMBER_MOTOR_PORT = 5, + LEFT_EXTERNAL_FWD_PORT = 1, + LEFT_EXTERNAL_REV_PORT = 1, + RIGHT_EXTERNAL_FWD_PORT = 1, + RIGHT_EXTERNAL_REV_PORT = 1; // Other CAN IDs public static final int PDP_CAN = 1, PCM_MODULE = 0; @@ -91,7 +95,8 @@ public class FullMap { public static final double AUTO_MAX_SPEED = 1.9, AUTO_MAX_ACCEL = .4; // Drive constants public static final double DRIVE_WHEEL_RADIUS = Units.inchesToMeters(2); - public static final double DRIVE_GEARING = 5.86; + public static final double DRIVE_GEARING = 1; // 5.86; + public static final int DRIVE_ENCODER_CPR = 256; public static final int DRIVE_CURRENT_LIM = 50; public static final double DRIVE_KP_VEL = 27.2, DRIVE_KI_VEL = 0.0, @@ -154,6 +159,7 @@ public static RobotMap createRobotMap() { .setUnitPerRotation(2 * Math.PI * DRIVE_WHEEL_RADIUS) // = 0.3191858136 .setCurrentLimit(DRIVE_CURRENT_LIM) .setPostEncoderGearing(DRIVE_GEARING) + .setEncoderCPR(DRIVE_ENCODER_CPR) .setEnableVoltageComp(true); // todo use sysid gains to make this @@ -167,8 +173,10 @@ public static RobotMap createRobotMap() { DRIVE_TRACK_WIDTH, VecBuilder.fill(0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005)); - var leftEncSim = new EncoderSim(new Encoder(0, 1)); - var rightEncSim = new EncoderSim(new Encoder(2, 3)); + var leftExtEnc = new Encoder(LEFT_EXTERNAL_FWD_PORT, LEFT_EXTERNAL_REV_PORT); + var rightExtEnc = new Encoder(RIGHT_EXTERNAL_FWD_PORT, RIGHT_EXTERNAL_REV_PORT); + var leftEncSim = new EncoderSim(leftExtEnc); + var rightEncSim = new EncoderSim(rightExtEnc); var leftMaster = driveMasterPrototype @@ -176,6 +184,7 @@ public static RobotMap createRobotMap() { .setPort(LEFT_LEADER_PORT) .setName("left") .setReverseOutput(false) + .setExternalEncoder(leftExtEnc) .addSlaveSpark(FollowerUtils.createFollowerSpark(LEFT_LEADER_FOLLOWER_1_PORT), false) .addSlaveSpark(FollowerUtils.createFollowerSpark(LEFT_LEADER_FOLLOWER_2_PORT), false) .createRealOrSim(leftEncSim); @@ -185,6 +194,7 @@ public static RobotMap createRobotMap() { .setName("right") .setPort(RIGHT_LEADER_PORT) .setReverseOutput(true) + .setExternalEncoder(rightExtEnc) .addSlaveSpark(FollowerUtils.createFollowerSpark(RIGHT_LEADER_FOLLOWER_1_PORT), false) .addSlaveSpark(FollowerUtils.createFollowerSpark(RIGHT_LEADER_FOLLOWER_2_PORT), false) .createRealOrSim(rightEncSim); @@ -482,7 +492,7 @@ public static RobotMap createRobotMap() { oneThenOneBallTraj( drive, cargo, - ramsetePrototype.name("bottomOneOne"), + ramsetePrototype.copy().name("bottomOneOne"), pose(8.01, 2.82, -111.80), pose(7.67, 0.77, -91.97)); @@ -491,10 +501,10 @@ public static RobotMap createRobotMap() { twoBallTraj( drive, cargo, - ramsetePrototype.name("topTwo"), - pose(6.07, 5.12, 134.24), - pose(5.34, 5.89, 130.31), - reverseHeading(pose(7.04, 4.58, -22.25))); + ramsetePrototype.copy().name("topTwo").field(null), + pose(6.07, 5.12, 140.24), // was 134.24 + pose(5.35, 5.75, 140.24), // degrees was 130.31 + reverseHeading(pose(6.94, 4.4, -22.25))); // x was 7.04, y was 4.58 // // Start at the edge at the bottom, collect the bottom ball, then come back and spit var bottomTwoBallTraj = twoBallTraj( @@ -520,7 +530,7 @@ public static RobotMap createRobotMap() { ramsetePrototype .copy() .name("threeBallAuto1") - .field(field) + .field(null) .traj( TrajectoryGenerator.generateTrajectory( pose(7.99, 2.81, -109.98), @@ -535,7 +545,7 @@ public static RobotMap createRobotMap() { ramsetePrototype .copy() .name("threeBallAuto2") - .field(field) + .field(null) .traj( TrajectoryGenerator.generateTrajectory( pose(ballX, ballY, turnAngle), @@ -548,7 +558,7 @@ public static RobotMap createRobotMap() { .andThen( ramsetePrototype .name("threeBallAuto3") - .field(field) + .field(null) .traj( TrajectoryGenerator.generateTrajectory( pose(ballX2, ballY2, turnAngle2), @@ -562,7 +572,7 @@ public static RobotMap createRobotMap() { ramsetePrototype .copy() .name("threeBallAuto4") - .field(field) + .field(null) .traj( TrajectoryGenerator.generateTrajectory( hubPose, List.of(), pose(5.70, 1.98, -90), trajConfig(drive))) @@ -612,16 +622,13 @@ public static RobotMap createRobotMap() { // .andThen(spit.get()); // Auto - List autoStartupCommands = - List.of( - new InstantCommand(() -> drive.resetOdometry(pose(7.76, 2.89, 249.19)), drive) - .andThen(oneBallAuto)); + List autoStartupCommands = List.of(topTwoBallTraj); List robotStartupCommands = List.of(); List teleopStartupCommands = List.of( - new InstantCommand(climber::enable), + new InstantCommand(climber::disable), new InstantCommand(() -> drive.setDefaultCommand(driveDefaultCmd)), new InstantCommand(cargo::stop)); From 3e9ad6384378a9285b93ea2b4077f1bdedb5015b Mon Sep 17 00:00:00 2001 From: Teddy Date: Fri, 11 Mar 2022 16:39:19 -0500 Subject: [PATCH 21/25] Tune ramping --- .../cargo/IntakeLimelightCommand.java | 50 +++++++++++++++ .../team449/auto/commands/RamseteBuilder.java | 25 +++++--- .../java/frc/team449/javaMaps/FullMap.java | 63 +++++++++++-------- .../team449/motor/builder/SparkMaxConfig.java | 6 +- src/main/java/frc/team449/wrappers/PDP.java | 5 ++ 5 files changed, 112 insertions(+), 37 deletions(-) create mode 100644 src/main/java/frc/team449/_2022robot/cargo/IntakeLimelightCommand.java diff --git a/src/main/java/frc/team449/_2022robot/cargo/IntakeLimelightCommand.java b/src/main/java/frc/team449/_2022robot/cargo/IntakeLimelightCommand.java new file mode 100644 index 00000000..a0d4764a --- /dev/null +++ b/src/main/java/frc/team449/_2022robot/cargo/IntakeLimelightCommand.java @@ -0,0 +1,50 @@ +package frc.team449._2022robot.cargo; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.team449.generalInterfaces.limelight.Limelight; + +public class IntakeLimelightCommand extends CommandBase { + private final Cargo2022 cargo; + private final Limelight limelight; + private final int bluePipeline; + private final int redPipeline; + + /** + * + * @param cargo + * @param limelight + * @param bluePipeline Pipeline for detecting blue balls + * @param redPipeline Pipeline for detecting red balls + */ + public IntakeLimelightCommand(Cargo2022 cargo, Limelight limelight, int bluePipeline, int redPipeline) { + addRequirements(cargo); + this.cargo = cargo; + this.limelight = limelight; + this.bluePipeline = bluePipeline; + this.redPipeline = redPipeline; + } + + @Override + public void initialize() { + var isBlue = DriverStation.getAlliance() == DriverStation.Alliance.Blue; + if (isBlue) { + limelight.setPipeline(redPipeline); + } else { + limelight.setPipeline(bluePipeline); + } + } + + @Override + public void execute() { + if (limelight.hasTarget()) { + cargo.stop(); + } else { + cargo.runIntake(); + } + } + + @Override + public void end(boolean interrupted) { + } +} diff --git a/src/main/java/frc/team449/auto/commands/RamseteBuilder.java b/src/main/java/frc/team449/auto/commands/RamseteBuilder.java index 458d34b3..d31b3a32 100644 --- a/src/main/java/frc/team449/auto/commands/RamseteBuilder.java +++ b/src/main/java/frc/team449/auto/commands/RamseteBuilder.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RamseteCommand; import frc.team449.ahrs.PIDAngleController; import frc.team449.drive.unidirectional.DriveUnidirectionalWithGyro; @@ -153,22 +154,26 @@ public Command build() { var cmd = new InstantCommand(() -> drivetrain.resetOdometry(traj.getInitialPose())) .andThen(ramseteCmd) - .andThen(() -> drivetrain.setVoltage(0, 0)); + .andThen(drivetrain::fullStop, drivetrain) + .andThen(new PrintCommand("Stopped")) + .andThen(() -> System.out.println(drivetrain.getWheelSpeeds())); + cmd.setName("Ramsete command"); // If angleTimeout is nonzero, then we want to turn after the Ramsete command is over // to ensure our heading is right if (angleTimeout > 0) { assert pidAngleController != null : "PIDAngleController must not be null if turning after Ramsete"; - return cmd.andThen( - new NavXTurnToAngle<>( - lastPose.getRotation().getDegrees(), - angleTimeout, - drivetrain, - pidAngleController)) - .andThen(() -> drivetrain.setVoltage(0, 0)); - } else { - return cmd; + cmd = + cmd.andThen( + new NavXTurnToAngle<>( + lastPose.getRotation().getDegrees(), + angleTimeout, + drivetrain, + pidAngleController)) + .andThen(drivetrain::fullStop, drivetrain); + cmd.setName("Ramsete command with NavX"); } + return cmd; } } diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index a65d2330..73d02694 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -24,10 +24,7 @@ import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.*; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.team449.CommandContainer; import frc.team449.RobotMap; @@ -78,9 +75,9 @@ public class FullMap { RIGHT_CLIMBER_MOTOR_PORT = 6, LEFT_CLIMBER_MOTOR_PORT = 5, LEFT_EXTERNAL_FWD_PORT = 1, - LEFT_EXTERNAL_REV_PORT = 1, - RIGHT_EXTERNAL_FWD_PORT = 1, - RIGHT_EXTERNAL_REV_PORT = 1; + LEFT_EXTERNAL_REV_PORT = 2, + RIGHT_EXTERNAL_FWD_PORT = 3, + RIGHT_EXTERNAL_REV_PORT = 4; // Other CAN IDs public static final int PDP_CAN = 1, PCM_MODULE = 0; @@ -97,7 +94,7 @@ public class FullMap { public static final double DRIVE_WHEEL_RADIUS = Units.inchesToMeters(2); public static final double DRIVE_GEARING = 1; // 5.86; public static final int DRIVE_ENCODER_CPR = 256; - public static final int DRIVE_CURRENT_LIM = 50; + public static final int DRIVE_CURRENT_LIM = 40; public static final double DRIVE_KP_VEL = 27.2, DRIVE_KI_VEL = 0.0, DRIVE_KD_VEL = 0, @@ -133,7 +130,11 @@ public class FullMap { // Intake public static final int INTAKE_PISTON_FWD_CHANNEL = 3, INTAKE_PISTON_REV_CHANNEL = 2; - // todo find out what the channel numbers are + public static final int INTAKE_CURR_LIM = 20; + + // Ramping + public static final double RAMP_INCREASE = 0.9, RAMP_DECREASE = 0.7; + public static final int BLUE_PIPELINE = 1, RED_PIPELINE = 2; private FullMap() {} @@ -235,7 +236,7 @@ public static RobotMap createRobotMap() { .axis(XboxController.Axis.kRightTrigger.value) .inverted(false) .build())), - new RampComponent(.9, 1.8)); + new RampComponent(RAMP_INCREASE, RAMP_DECREASE)); var oi = new OIArcadeWithDPad( rotThrottle, @@ -280,6 +281,7 @@ public static RobotMap createRobotMap() { new SparkMaxConfig() .setName("intakeMotor") .setPort(INTAKE_LEADER_PORT) + .setCurrentLimit(INTAKE_CURR_LIM) .addSlaveSpark(FollowerUtils.createFollowerSpark(INTAKE_FOLLOWER_PORT), true) .createReal(), new SparkMaxConfig() @@ -350,7 +352,7 @@ public static RobotMap createRobotMap() { leftArm, rightArm, pivotPiston, - RobotBase.isSimulation() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get : () -> false, + RobotBase.isReal() ? new DigitalInput(CLIMBER_SENSOR_CHANNEL)::get : () -> false, CLIMBER_DISTANCE, CLIMBER_MID_DISTANCE); @@ -387,7 +389,7 @@ public static RobotMap createRobotMap() { // .whenPressed(() -> climber.reset(CLIMBER_MID_DISTANCE), climber); // Move climber arm up enough for high rung new JoystickButton(climberJoystick, XboxController.Button.kY.value) - .whenPressed(climber::disable, climber) + // .whenPressed(climber::disable, climber) .whileHeld(() -> climber.set(CLIMBER_EXTEND_VEL), climber) .whenReleased(() -> climber.set(0), climber); // new JoystickButton(climberJoystick, XboxController.Button.kY.value) @@ -403,7 +405,7 @@ public static RobotMap createRobotMap() { // .whenReleased(() -> rightArm.set(0), climber); // Move climber arm down new JoystickButton(climberJoystick, XboxController.Button.kA.value) - .whenPressed(climber::disable, climber) + // .whenPressed(climber::disable, climber) .whileHeld(() -> climber.set(CLIMBER_RETRACT_VEL), climber) .whenReleased(() -> climber.set(0), climber); // new JoystickButton(climberJoystick, XboxController.Button.kA.value) @@ -501,10 +503,10 @@ public static RobotMap createRobotMap() { twoBallTraj( drive, cargo, - ramsetePrototype.copy().name("topTwo").field(null), - pose(6.07, 5.12, 140.24), // was 134.24 - pose(5.35, 5.75, 140.24), // degrees was 130.31 - reverseHeading(pose(6.94, 4.4, -22.25))); // x was 7.04, y was 4.58 + ramsetePrototype.copy().name("topTwo").field(null).angleTimeout(0), + pose(6.07, 5.12, 140.24), + pose(5.35, 5.75, 140.24), + pose(6.94, 4.4, 140.24)); // // Start at the edge at the bottom, collect the bottom ball, then come back and spit var bottomTwoBallTraj = twoBallTraj( @@ -622,7 +624,15 @@ public static RobotMap createRobotMap() { // .andThen(spit.get()); // Auto - List autoStartupCommands = List.of(topTwoBallTraj); + List autoStartupCommands = + List.of( + topTwoBallTraj, + new InstantCommand( + () -> { + var cmd = CommandScheduler.getInstance().requiring(drive); + if (cmd != null) System.out.println("current drive cmd: " + cmd.getName()); + }) + .perpetually()); List robotStartupCommands = List.of(); @@ -631,6 +641,8 @@ public static RobotMap createRobotMap() { new InstantCommand(climber::disable), new InstantCommand(() -> drive.setDefaultCommand(driveDefaultCmd)), new InstantCommand(cargo::stop)); + // new IntakeLimelightCommand(cargo, limelight, BLUE_PIPELINE, RED_PIPELINE) + // .perpetually()); List testStartupCommands = List.of(); var allCommands = @@ -728,13 +740,14 @@ private static Command twoBallTraj( var fromBall = TrajectoryGenerator.generateTrajectory( ballPose, List.of(), endingPose, trajConfig(drive).setReversed(true)); - return new InstantCommand(cargo::runIntake, cargo) - .andThen(ramsetePrototype.copy().traj(toBall).build()) - .andThen(new WaitCommand(1)) - .andThen(ramsetePrototype.copy().traj(fromBall).build()) - .andThen(cargo::spit, cargo) - .andThen(new WaitCommand(1)) - .andThen(cargo::stop, cargo); + return ramsetePrototype.copy().traj(toBall).build(); + // return new InstantCommand(cargo::runIntake, cargo) + // .andThen(ramsetePrototype.copy().traj(toBall).build()) + // .andThen(new WaitCommand(1)) + // .andThen(ramsetePrototype.copy().traj(fromBall).build()) + // .andThen(cargo::spit, cargo) + // .andThen(new WaitCommand(1)) + // .andThen(cargo::stop, cargo); } /** Little helper because the verbosity of the Pose2d constructor is tiring */ diff --git a/src/main/java/frc/team449/motor/builder/SparkMaxConfig.java b/src/main/java/frc/team449/motor/builder/SparkMaxConfig.java index 4ef15b83..3440999d 100644 --- a/src/main/java/frc/team449/motor/builder/SparkMaxConfig.java +++ b/src/main/java/frc/team449/motor/builder/SparkMaxConfig.java @@ -4,9 +4,9 @@ import com.revrobotics.CANSparkMaxLowLevel; import com.revrobotics.REVLibError; import com.revrobotics.SparkMaxLimitSwitch; -import frc.team449.other.FollowerUtils; import frc.team449.motor.Encoder; import frc.team449.motor.WrappedMotor; +import frc.team449.other.FollowerUtils; import org.jetbrains.annotations.Contract; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; @@ -138,7 +138,9 @@ public WrappedMotor createReal() { // Set the current limit if it was given if (this.getCurrentLimit() != null) { - motor.setSmartCurrentLimit(this.getCurrentLimit()); + var limit = this.getCurrentLimit(); + motor.setSmartCurrentLimit(limit); + this.slaveSparks.forEach((slave, __) -> slave.setSmartCurrentLimit(limit)); } if (this.isEnableVoltageComp()) { diff --git a/src/main/java/frc/team449/wrappers/PDP.java b/src/main/java/frc/team449/wrappers/PDP.java index 856251c3..8b60d977 100644 --- a/src/main/java/frc/team449/wrappers/PDP.java +++ b/src/main/java/frc/team449/wrappers/PDP.java @@ -104,6 +104,11 @@ public Double getUnloadedVoltage() { return voltagePerCurrentLinReg == null ? null : unloadedVoltage; } + @Log + public double channel0Current() { + return this.PDP.getCurrent(0); + } + /** Updates all cached values with current ones. */ @Override public void update() { From d280b6efb78a6096bd9b467c87421e12d0089491 Mon Sep 17 00:00:00 2001 From: Teddy Date: Fri, 11 Mar 2022 18:12:49 -0500 Subject: [PATCH 22/25] Added external encoders and tuned drive --- src/main/deploy/pathplanner/1ball blue 2.path | 2 +- .../team449/_2022robot/cargo/Cargo2022.java | 12 +-- .../climber/PivotingTelescopingClimber.java | 31 ++++---- .../java/frc/team449/javaMaps/FullMap.java | 78 +++++++++---------- 4 files changed, 63 insertions(+), 60 deletions(-) diff --git a/src/main/deploy/pathplanner/1ball blue 2.path b/src/main/deploy/pathplanner/1ball blue 2.path index 8241bba1..d2c2574e 100644 --- a/src/main/deploy/pathplanner/1ball blue 2.path +++ b/src/main/deploy/pathplanner/1ball blue 2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":6.099870427588956,"y":5.127595065316713},"prevControl":null,"nextControl":{"x":5.613732746452835,"y":5.590583333065401},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":true},{"anchorPoint":{"x":5.30121566572247,"y":5.9262498271832},"prevControl":{"x":5.771998133946636,"y":5.528700187349458},"nextControl":{"x":4.780353864505197,"y":6.366088681544453},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.037421669780049,"y":4.502560903855985},"prevControl":{"x":7.685605244628211,"y":4.247917356594206},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.4,"isReversed":false} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":6.099870427588956,"y":5.127595065316713},"prevControl":null,"nextControl":{"x":5.613732746452835,"y":5.590583333065401},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":true},{"anchorPoint":{"x":5.30121566572247,"y":5.9262498271832},"prevControl":{"x":5.6947556933088554,"y":5.5442845062905315},"nextControl":{"x":5.6947556933088554,"y":5.5442845062905315},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.0026975496988975,"y":4.444687370387398},"prevControl":{"x":6.423962215013037,"y":4.687756210955459},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.4,"isReversed":false} \ No newline at end of file diff --git a/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java b/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java index f8291185..9d940fbc 100644 --- a/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java +++ b/src/main/java/frc/team449/_2022robot/cargo/Cargo2022.java @@ -32,17 +32,17 @@ public Cargo2022( public void runIntake() { intakeMotor.set(intakeSpeed); - spitterMotor.set(spitterSpeed); + spitterMotor.set(-spitterSpeed); } -// public void runIntakeReverse() { -// intakeMotor.set(-intakeSpeed); -// spitterMotor.set(-spitterSpeed); -// } + public void runIntakeReverse() { + intakeMotor.set(-intakeSpeed); + spitterMotor.set(-spitterSpeed); + } public void spit() { intakeMotor.set(intakeSpeed); - spitterMotor.set(-spitterSpeed); + spitterMotor.set(spitterSpeed); } public void stop() { diff --git a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java index 5959ce11..f1f770ab 100644 --- a/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java +++ b/src/main/java/frc/team449/_2022robot/climber/PivotingTelescopingClimber.java @@ -98,26 +98,29 @@ public void pivotTelescopingArmIn() { pivotPiston.set(DoubleSolenoid.Value.kForward); } - /** Directly set velocity. This method must be called every loop if you're using climber. - * However, sets velocity to 0 if: + /** + * Directly set velocity. This method must be called every loop if you're using climber. However, + * sets velocity to 0 if: + * *
    - *
  • Hall sensor is active and velocity is negative
    • - *
  • Arms are stowed, velocity is positive, and is beyond {@link PivotingTelescopingClimber#midDistance}
    • + *
  • Hall sensor is active and velocity is negative + *
  • Arms are stowed, velocity is positive, and is beyond {@link + * PivotingTelescopingClimber#midDistance} *
*/ public void set(double velocity) { double leftVel = velocity; double rightVel = velocity; - if (velocity <= 0 && this.hitBottom()) { - leftVel = rightVel = 0; - } else if (this.isStowed()) { - if (leftArm.getMeasurement() > midDistance) { - leftVel = 0; - } - if (rightArm.getMeasurement() > midDistance) { - rightVel = 0; - } - } +// if (velocity <= 0 && this.hitBottom()) { +// leftVel = rightVel = 0; +// } else if (this.isStowed()) { +// if (leftArm.getMeasurement() > midDistance) { +// leftVel = 0; +// } +// if (rightArm.getMeasurement() > midDistance) { +// rightVel = 0; +// } +// } leftArm.set(leftVel); rightArm.set(rightVel); diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 73d02694..ae7fb4d1 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -24,7 +24,10 @@ import edu.wpi.first.wpilibj.simulation.EncoderSim; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.team449.CommandContainer; import frc.team449.RobotMap; @@ -74,10 +77,10 @@ public class FullMap { SPITTER_PORT = 9, RIGHT_CLIMBER_MOTOR_PORT = 6, LEFT_CLIMBER_MOTOR_PORT = 5, - LEFT_EXTERNAL_FWD_PORT = 1, - LEFT_EXTERNAL_REV_PORT = 2, - RIGHT_EXTERNAL_FWD_PORT = 3, - RIGHT_EXTERNAL_REV_PORT = 4; + LEFT_EXTERNAL_FWD_PORT = 6, + LEFT_EXTERNAL_REV_PORT = 7, + RIGHT_EXTERNAL_FWD_PORT = 8, + RIGHT_EXTERNAL_REV_PORT = 9; // Other CAN IDs public static final int PDP_CAN = 1, PCM_MODULE = 0; @@ -88,14 +91,14 @@ public class FullMap { // Limelight public static final int DRIVER_PIPELINE = 0; // TODO find out what this is! // Speeds - public static final double INTAKE_SPEED = 0.6, SPITTER_SPEED = 0.6; + public static final double INTAKE_SPEED = 0.8, SPITTER_SPEED = 0.6; public static final double AUTO_MAX_SPEED = 1.9, AUTO_MAX_ACCEL = .4; // Drive constants public static final double DRIVE_WHEEL_RADIUS = Units.inchesToMeters(2); public static final double DRIVE_GEARING = 1; // 5.86; public static final int DRIVE_ENCODER_CPR = 256; - public static final int DRIVE_CURRENT_LIM = 40; - public static final double DRIVE_KP_VEL = 27.2, + public static final int DRIVE_CURRENT_LIM = 30; + public static final double DRIVE_KP_VEL = 0.02, // 27.2, DRIVE_KI_VEL = 0.0, DRIVE_KD_VEL = 0, DRIVE_KP_POS = 45.269, @@ -116,10 +119,10 @@ public class FullMap { // Climber public static final int CLIMBER_PISTON_FWD_CHANNEL = 0, CLIMBER_PISTON_REV_CHANNEL = 1; // todo find out what the channel numbers are - public static final int CLIMBER_SENSOR_CHANNEL = 6; // todo find out what this really is + public static final int CLIMBER_SENSOR_CHANNEL = 0; // todo find out what this really is public static final double CLIMBER_MAX_VEL = 0.1, CLIMBER_MAX_ACCEL = 0.1; - public static final double CLIMBER_EXTEND_VEL = 0.1, CLIMBER_RETRACT_VEL = -0.3; - public static final double CLIMBER_DISTANCE = 0.7, CLIMBER_MID_DISTANCE = 0.6; + public static final double CLIMBER_EXTEND_VEL = 0.2, CLIMBER_RETRACT_VEL = -0.3; + public static final double CLIMBER_DISTANCE = 0.7, CLIMBER_MID_DISTANCE = 0.5; public static final double CLIMBER_KP = 500; // 600; public static final double CLIMBER_FF_KS = 0, CLIMBER_FF_KV = 0, @@ -176,6 +179,7 @@ public static RobotMap createRobotMap() { var leftExtEnc = new Encoder(LEFT_EXTERNAL_FWD_PORT, LEFT_EXTERNAL_REV_PORT); var rightExtEnc = new Encoder(RIGHT_EXTERNAL_FWD_PORT, RIGHT_EXTERNAL_REV_PORT); + rightExtEnc.setReverseDirection(true); var leftEncSim = new EncoderSim(leftExtEnc); var rightEncSim = new EncoderSim(rightExtEnc); @@ -252,14 +256,14 @@ public static RobotMap createRobotMap() { var pidAngleControllerPrototype = new PIDAngleControllerBuilder() - .absoluteTolerance(0) + .absoluteTolerance(0.001) .onTargetBuffer(new Debouncer(1.5)) .minimumOutput(0) .maximumOutput(0.6) .loopTimeMillis(null) .deadband(2) .inverted(false) - .pid(0.01, 0, 0.03); + .pid(0.006, 0, 0.03); var driveDefaultCmd = new UnidirectionalNavXDefaultDrive<>( @@ -296,8 +300,9 @@ public static RobotMap createRobotMap() { var armPrototype = driveMasterPrototype .copy() - .setRevSoftLimit(0.) - .setFwdSoftLimit(CLIMBER_DISTANCE) + .setCurrentLimit(null) + // .setRevSoftLimit(0.) + // .setFwdSoftLimit(CLIMBER_DISTANCE) .setUnitPerRotation(0.1949) .setPostEncoderGearing(27) .setEnableBrakeMode(true); @@ -366,11 +371,11 @@ public static RobotMap createRobotMap() { // Button bindings here // Take in balls but don't shoot - new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) + new JoystickButton(cargoJoystick, XboxController.Button.kLeftBumper.value) .whileHeld(cargo::runIntake, cargo) .whenReleased(cargo::stop, cargo); // Run all motors in intake to spit balls out - new JoystickButton(cargoJoystick, XboxController.Button.kLeftBumper.value) + new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) .whileHeld(cargo::spit, cargo) .whenReleased(cargo::stop, cargo); // Stow/retract intake @@ -378,7 +383,11 @@ public static RobotMap createRobotMap() { .whenPressed(cargo::retractIntake); // Deploy intake new JoystickButton(cargoJoystick, XboxController.Button.kA.value) - .whenPressed(cargo::deployIntake); + .whileHeld(cargo::deployIntake, cargo) + .whenReleased(cargo::stop, cargo); + new JoystickButton(cargoJoystick, XboxController.Button.kB.value) + .whileHeld(cargo::runIntakeReverse, cargo) + .whenReleased(cargo::stop); // Run intake in reverse to feed ball from top // new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) // .whileHeld(cargo::runIntakeReverse, cargo) @@ -503,10 +512,10 @@ public static RobotMap createRobotMap() { twoBallTraj( drive, cargo, - ramsetePrototype.copy().name("topTwo").field(null).angleTimeout(0), - pose(6.07, 5.12, 140.24), - pose(5.35, 5.75, 140.24), - pose(6.94, 4.4, 140.24)); + ramsetePrototype.copy().name("topTwo").field(field).angleTimeout(0), + pose(6.10, 5.12, 136.40), + pose(5.3, 5.93, 135.86), + pose(7, 4.4, 180 - 22.78)); // // Start at the edge at the bottom, collect the bottom ball, then come back and spit var bottomTwoBallTraj = twoBallTraj( @@ -624,15 +633,7 @@ public static RobotMap createRobotMap() { // .andThen(spit.get()); // Auto - List autoStartupCommands = - List.of( - topTwoBallTraj, - new InstantCommand( - () -> { - var cmd = CommandScheduler.getInstance().requiring(drive); - if (cmd != null) System.out.println("current drive cmd: " + cmd.getName()); - }) - .perpetually()); + List autoStartupCommands = List.of(topTwoBallTraj); List robotStartupCommands = List.of(); @@ -740,14 +741,13 @@ private static Command twoBallTraj( var fromBall = TrajectoryGenerator.generateTrajectory( ballPose, List.of(), endingPose, trajConfig(drive).setReversed(true)); - return ramsetePrototype.copy().traj(toBall).build(); - // return new InstantCommand(cargo::runIntake, cargo) - // .andThen(ramsetePrototype.copy().traj(toBall).build()) - // .andThen(new WaitCommand(1)) - // .andThen(ramsetePrototype.copy().traj(fromBall).build()) - // .andThen(cargo::spit, cargo) - // .andThen(new WaitCommand(1)) - // .andThen(cargo::stop, cargo); + return new InstantCommand(cargo::runIntake, cargo) + .andThen(ramsetePrototype.copy().traj(toBall).build()) + .andThen(new WaitCommand(1)) + .andThen(ramsetePrototype.copy().traj(fromBall).build()) + .andThen(cargo::spit, cargo) + .andThen(new WaitCommand(1)) + .andThen(cargo::stop, cargo); } /** Little helper because the verbosity of the Pose2d constructor is tiring */ From 814b6706b588ee8b852907e7053791e0d0af457e Mon Sep 17 00:00:00 2001 From: Teddy Date: Sat, 12 Mar 2022 17:55:55 -0500 Subject: [PATCH 23/25] one ball and two ball auto done --- src/main/deploy/pathplanner/1ball blue 2.path | 2 +- .../java/frc/team449/javaMaps/FullMap.java | 34 +++++++++++-------- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/main/deploy/pathplanner/1ball blue 2.path b/src/main/deploy/pathplanner/1ball blue 2.path index d2c2574e..b216d7ae 100644 --- a/src/main/deploy/pathplanner/1ball blue 2.path +++ b/src/main/deploy/pathplanner/1ball blue 2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":6.099870427588956,"y":5.127595065316713},"prevControl":null,"nextControl":{"x":5.613732746452835,"y":5.590583333065401},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":true},{"anchorPoint":{"x":5.30121566572247,"y":5.9262498271832},"prevControl":{"x":5.6947556933088554,"y":5.5442845062905315},"nextControl":{"x":5.6947556933088554,"y":5.5442845062905315},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.0026975496988975,"y":4.444687370387398},"prevControl":{"x":6.423962215013037,"y":4.687756210955459},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.4,"isReversed":false} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":6.062386501997203,"y":5.134968277053992},"prevControl":null,"nextControl":{"x":5.769517588857243,"y":5.427837190193953},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.330214219147299,"y":5.867140559903895},"prevControl":{"x":5.6426077264965935,"y":5.525460161240607},"nextControl":{"x":5.6426077264965935,"y":5.525460161240607},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.872657161684431,"y":4.051353298436134},"prevControl":{"x":6.257632444090514,"y":4.295410726052769},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.2,"isReversed":false} \ No newline at end of file diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index ae7fb4d1..b6186631 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -40,8 +40,10 @@ import frc.team449.components.RunningLinRegComponent; import frc.team449.drive.DriveSettingsBuilder; import frc.team449.drive.unidirectional.DriveUnidirectionalWithGyro; +import frc.team449.drive.unidirectional.commands.AHRS.NavXDriveStraight; import frc.team449.drive.unidirectional.commands.AHRS.NavXTurnToAngle; import frc.team449.drive.unidirectional.commands.DriveAtSpeed; +import frc.team449.drive.unidirectional.commands.DriveStraight; import frc.team449.drive.unidirectional.commands.UnidirectionalNavXDefaultDrive; import frc.team449.generalInterfaces.doubleUnaryOperator.Polynomial; import frc.team449.generalInterfaces.doubleUnaryOperator.RampComponent; @@ -92,13 +94,13 @@ public class FullMap { public static final int DRIVER_PIPELINE = 0; // TODO find out what this is! // Speeds public static final double INTAKE_SPEED = 0.8, SPITTER_SPEED = 0.6; - public static final double AUTO_MAX_SPEED = 1.9, AUTO_MAX_ACCEL = .4; + public static final double AUTO_MAX_SPEED = 1.9, AUTO_MAX_ACCEL = .2; // Drive constants public static final double DRIVE_WHEEL_RADIUS = Units.inchesToMeters(2); public static final double DRIVE_GEARING = 1; // 5.86; public static final int DRIVE_ENCODER_CPR = 256; - public static final int DRIVE_CURRENT_LIM = 30; - public static final double DRIVE_KP_VEL = 0.02, // 27.2, + public static final int DRIVE_CURRENT_LIM = 40; + public static final double DRIVE_KP_VEL = 0.005, // 27.2, DRIVE_KI_VEL = 0.0, DRIVE_KD_VEL = 0, DRIVE_KP_POS = 45.269, @@ -508,20 +510,20 @@ public static RobotMap createRobotMap() { pose(7.67, 0.77, -91.97)); // // Start at the edge at the top, collect the top ball, then come back and spit - var topTwoBallTraj = + var hangarTwoBallTraj = twoBallTraj( drive, cargo, - ramsetePrototype.copy().name("topTwo").field(field).angleTimeout(0), - pose(6.10, 5.12, 136.40), - pose(5.3, 5.93, 135.86), - pose(7, 4.4, 180 - 22.78)); + ramsetePrototype.copy().name("hangar_two_ball_auto").field(field).angleTimeout(0), + pose(6.06, 5.13, 136.40), + pose(5.33, 5.87, 134.37), + pose(6.76, 4.05, 180 - 41)); // // Start at the edge at the bottom, collect the bottom ball, then come back and spit - var bottomTwoBallTraj = + var stationTwoBallTraj = twoBallTraj( drive, cargo, - ramsetePrototype.name("bottomTwo"), + ramsetePrototype.name("station_two_ball_auto"), pose(7.55, 1.83, -88.32), pose(7.63, 0.76, -86.19), reverseHeading(pose(8.01, 2.82, 68.63))); @@ -592,7 +594,7 @@ public static RobotMap createRobotMap() { new InstantCommand(cargo::spit, cargo) .andThen(new WaitCommand(1)) .andThen(cargo::stop, cargo) - .andThen(new DriveAtSpeed<>(drive, 0.18, 2)); + .andThen(new DriveAtSpeed<>(drive, 0.13, 5)); // var randomTestAuto = // ramsetePrototype @@ -633,7 +635,7 @@ public static RobotMap createRobotMap() { // .andThen(spit.get()); // Auto - List autoStartupCommands = List.of(topTwoBallTraj); + List autoStartupCommands = List.of(hangarTwoBallTraj); List robotStartupCommands = List.of(); @@ -741,13 +743,15 @@ private static Command twoBallTraj( var fromBall = TrajectoryGenerator.generateTrajectory( ballPose, List.of(), endingPose, trajConfig(drive).setReversed(true)); - return new InstantCommand(cargo::runIntake, cargo) + return //new InstantCommand(cargo::runIntake, cargo) + /*.andThen(*/new InstantCommand(cargo::deployIntake, cargo)/*)*/ + .andThen(new WaitCommand(1)) .andThen(ramsetePrototype.copy().traj(toBall).build()) .andThen(new WaitCommand(1)) .andThen(ramsetePrototype.copy().traj(fromBall).build()) - .andThen(cargo::spit, cargo) + /*.andThen(cargo::spit, cargo) .andThen(new WaitCommand(1)) - .andThen(cargo::stop, cargo); + .andThen(cargo::stop, cargo)*/; } /** Little helper because the verbosity of the Pose2d constructor is tiring */ From 7c9a892ae06cdc813bd9f144af117ba6c71bad32 Mon Sep 17 00:00:00 2001 From: Teddy Date: Sun, 13 Mar 2022 13:07:24 -0400 Subject: [PATCH 24/25] one ball and two ball auto fixed --- src/main/deploy/pathplanner/1ball blue 2.path | 2 +- src/main/java/frc/team449/Robot.java | 1 - .../team449/auto/commands/RamseteBuilder.java | 4 +- .../java/frc/team449/javaMaps/FullMap.java | 106 ++--- .../frc/team449/javaMaps/IntakeTestMap.java | 412 ------------------ 5 files changed, 35 insertions(+), 490 deletions(-) delete mode 100644 src/main/java/frc/team449/javaMaps/IntakeTestMap.java diff --git a/src/main/deploy/pathplanner/1ball blue 2.path b/src/main/deploy/pathplanner/1ball blue 2.path index b216d7ae..3f3453c3 100644 --- a/src/main/deploy/pathplanner/1ball blue 2.path +++ b/src/main/deploy/pathplanner/1ball blue 2.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":6.062386501997203,"y":5.134968277053992},"prevControl":null,"nextControl":{"x":5.769517588857243,"y":5.427837190193953},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.330214219147299,"y":5.867140559903895},"prevControl":{"x":5.6426077264965935,"y":5.525460161240607},"nextControl":{"x":5.6426077264965935,"y":5.525460161240607},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.872657161684431,"y":4.051353298436134},"prevControl":{"x":6.257632444090514,"y":4.295410726052769},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.2,"isReversed":false} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":6.130722581729864,"y":5.24235354520531},"prevControl":null,"nextControl":{"x":5.857378262799233,"y":5.496173269926609},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.359501110461299,"y":5.935476639636551},"prevControl":{"x":5.683246458675133,"y":5.604532312332696},"nextControl":{"x":5.683246458675133,"y":5.604532312332696},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.038616212463745,"y":4.510181262355406},"prevControl":{"x":7.584761353049779,"y":3.990094273727063},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.9,"maxAcceleration":0.2,"isReversed":false} \ No newline at end of file diff --git a/src/main/java/frc/team449/Robot.java b/src/main/java/frc/team449/Robot.java index 1c254a52..f18b3a96 100644 --- a/src/main/java/frc/team449/Robot.java +++ b/src/main/java/frc/team449/Robot.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.team449.javaMaps.FullMap; -import frc.team449.javaMaps.IntakeTestMap; import frc.team449.other.Clock; import frc.team449.other.Updater; import io.github.oblarg.oblog.Logger; diff --git a/src/main/java/frc/team449/auto/commands/RamseteBuilder.java b/src/main/java/frc/team449/auto/commands/RamseteBuilder.java index d31b3a32..fdebbe4f 100644 --- a/src/main/java/frc/team449/auto/commands/RamseteBuilder.java +++ b/src/main/java/frc/team449/auto/commands/RamseteBuilder.java @@ -154,9 +154,7 @@ public Command build() { var cmd = new InstantCommand(() -> drivetrain.resetOdometry(traj.getInitialPose())) .andThen(ramseteCmd) - .andThen(drivetrain::fullStop, drivetrain) - .andThen(new PrintCommand("Stopped")) - .andThen(() -> System.out.println(drivetrain.getWheelSpeeds())); + .andThen(drivetrain::fullStop, drivetrain); cmd.setName("Ramsete command"); // If angleTimeout is nonzero, then we want to turn after the Ramsete command is over diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index b6186631..2f39538c 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -14,10 +14,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint; import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; -import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint; -import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim; @@ -40,10 +37,8 @@ import frc.team449.components.RunningLinRegComponent; import frc.team449.drive.DriveSettingsBuilder; import frc.team449.drive.unidirectional.DriveUnidirectionalWithGyro; -import frc.team449.drive.unidirectional.commands.AHRS.NavXDriveStraight; import frc.team449.drive.unidirectional.commands.AHRS.NavXTurnToAngle; import frc.team449.drive.unidirectional.commands.DriveAtSpeed; -import frc.team449.drive.unidirectional.commands.DriveStraight; import frc.team449.drive.unidirectional.commands.UnidirectionalNavXDefaultDrive; import frc.team449.generalInterfaces.doubleUnaryOperator.Polynomial; import frc.team449.generalInterfaces.doubleUnaryOperator.RampComponent; @@ -93,7 +88,7 @@ public class FullMap { // Limelight public static final int DRIVER_PIPELINE = 0; // TODO find out what this is! // Speeds - public static final double INTAKE_SPEED = 0.8, SPITTER_SPEED = 0.6; + public static final double INTAKE_SPEED = 0.75, SPITTER_SPEED = 0.45; public static final double AUTO_MAX_SPEED = 1.9, AUTO_MAX_ACCEL = .2; // Drive constants public static final double DRIVE_WHEEL_RADIUS = Units.inchesToMeters(2); @@ -162,7 +157,7 @@ public static RobotMap createRobotMap() { var driveMasterPrototype = new SparkMaxConfig() .setEnableBrakeMode(true) - .setUnitPerRotation(2 * Math.PI * DRIVE_WHEEL_RADIUS) // = 0.3191858136 + .setUnitPerRotation(0.3192) // * Math.PI * DRIVE_WHEEL_RADIUS) // = 0.3191858136 .setCurrentLimit(DRIVE_CURRENT_LIM) .setPostEncoderGearing(DRIVE_GEARING) .setEncoderCPR(DRIVE_ENCODER_CPR) @@ -381,19 +376,16 @@ public static RobotMap createRobotMap() { .whileHeld(cargo::spit, cargo) .whenReleased(cargo::stop, cargo); // Stow/retract intake - new JoystickButton(cargoJoystick, XboxController.Button.kY.value) + new JoystickButton(cargoJoystick, XboxController.Button.kX.value) .whenPressed(cargo::retractIntake); // Deploy intake new JoystickButton(cargoJoystick, XboxController.Button.kA.value) .whileHeld(cargo::deployIntake, cargo) .whenReleased(cargo::stop, cargo); + // Run intake backwards new JoystickButton(cargoJoystick, XboxController.Button.kB.value) .whileHeld(cargo::runIntakeReverse, cargo) .whenReleased(cargo::stop); - // Run intake in reverse to feed ball from top - // new JoystickButton(cargoJoystick, XboxController.Button.kRightBumper.value) - // .whileHeld(cargo::runIntakeReverse, cargo) - // .whenReleased(cargo::stop, cargo); // Move climber arms up enough for mid rung // new JoystickButton(climberJoystick, XboxController.Button.kRightBumper.value) @@ -510,6 +502,14 @@ public static RobotMap createRobotMap() { pose(7.67, 0.77, -91.97)); // // Start at the edge at the top, collect the top ball, then come back and spit + // var hangarTwoBallTraj = + // twoBallTraj( + // drive, + // cargo, + // ramsetePrototype.copy().name("hangar_two_ball_auto").field(field).angleTimeout(0), + // pose(6.06, 5.13, 136.40), + // pose(5.33, 5.87, 134.37), + // pose(6.76, 4.05, 180 - 41)); var hangarTwoBallTraj = twoBallTraj( drive, @@ -526,7 +526,7 @@ public static RobotMap createRobotMap() { ramsetePrototype.name("station_two_ball_auto"), pose(7.55, 1.83, -88.32), pose(7.63, 0.76, -86.19), - reverseHeading(pose(8.01, 2.82, 68.63))); + pose(7.83, 2.91, 180 + 68.63)); var ballX = 7.65; var ballY = 0.69; var ballAngle = -92.44; @@ -595,29 +595,18 @@ public static RobotMap createRobotMap() { .andThen(new WaitCommand(1)) .andThen(cargo::stop, cargo) .andThen(new DriveAtSpeed<>(drive, 0.13, 5)); - - // var randomTestAuto = - // ramsetePrototype - // .name("randomTestAuto") - // .traj( - // TrajectoryGenerator.generateTrajectory( - // pose(7.56, 2.89, -38.40), - // List.of(new Translation2d(7.62, 0.30), new Translation2d(6.63, 2.27)), - // pose(5.17, 1.99, -56.21), - // trajConfig(drive)) - // .concatenate( - // TrajectoryGenerator.generateTrajectory( - // pose(5.17, 1.99, -56.21), - // List.of(new Translation2d(3.05, 1.80)), - // pose(4.97, 4.34, 1.04), - // trajConfig(drive)))) - // .build(); - - // var testPath = - // new InstantCommand(cargo::runIntake, cargo) - // .andThen(ramsetePrototype.copy().name("testtraj").traj(testTraj(drive)).build()) - // .andThen(spit.get()); - + var twoBallAuto = + new InstantCommand(cargo::runIntake, cargo) + .andThen(() -> drive.resetOdometry(pose(6.06, 5.13, 136.40)), drive) + .andThen(cargo::deployIntake, cargo) + .andThen(new WaitCommand(.4)) + .andThen(new DriveAtSpeed<>(drive, .13, 1.5)) + .andThen(new WaitCommand(.4)) + .andThen(new DriveAtSpeed<>(drive, -.13, 4)) + .andThen(cargo::spit, cargo) + .andThen(new WaitCommand(2)) + .andThen(cargo::stop, cargo) + .andThen(drive::fullStop, drive); // (assume blue alliance) // Start at bottom on edge of tape, get one ball, score that and the preloaded one, go back for // another ball, then score that @@ -635,7 +624,7 @@ public static RobotMap createRobotMap() { // .andThen(spit.get()); // Auto - List autoStartupCommands = List.of(hangarTwoBallTraj); + List autoStartupCommands = List.of(twoBallAuto); List robotStartupCommands = List.of(); @@ -644,8 +633,6 @@ public static RobotMap createRobotMap() { new InstantCommand(climber::disable), new InstantCommand(() -> drive.setDefaultCommand(driveDefaultCmd)), new InstantCommand(cargo::stop)); - // new IntakeLimelightCommand(cargo, limelight, BLUE_PIPELINE, RED_PIPELINE) - // .perpetually()); List testStartupCommands = List.of(); var allCommands = @@ -655,33 +642,6 @@ public static RobotMap createRobotMap() { return new RobotMap(subsystems, pdp, allCommands, false); } - private static Trajectory testTraj(@NotNull DriveUnidirectionalWithGyro drive) { - var ballPos = new Translation2d(2.5, 4); - var speedConstraint = - new EllipticalRegionConstraint( - ballPos, 0.5, 0.5, Rotation2d.fromDegrees(0), new MaxVelocityConstraint(1.0)); - var toBall = - TrajectoryGenerator.generateTrajectory( - new Pose2d(new Translation2d(1, 3), Rotation2d.fromDegrees(0)), - List.of(), - new Pose2d(ballPos, Rotation2d.fromDegrees(90)), - trajConfig(drive) - // .addConstraint(speedConstraint) - ); - var toEnd = - TrajectoryGenerator.generateTrajectory( - new Pose2d(ballPos, Rotation2d.fromDegrees(90)), - List.of(), - new Pose2d(new Translation2d(5, 3), Rotation2d.fromDegrees(180)), - trajConfig(drive) - .setEndVelocity(0.0) - // .addConstraint(speedConstraint) - .setReversed(true)); - return toBall.concatenate(toEnd); - // return toBall; - // return toEnd; - } - @NotNull private static TrajectoryConfig trajConfig(@NotNull DriveUnidirectionalWithGyro drive) { return new TrajectoryConfig(AUTO_MAX_SPEED, AUTO_MAX_ACCEL) @@ -690,8 +650,8 @@ private static TrajectoryConfig trajConfig(@NotNull DriveUnidirectionalWithGyro new DifferentialDriveVoltageConstraint( drive.getFeedforward(), drive.getDriveKinematics(), - RobotController.getBatteryVoltage())) - .addConstraint(new CentripetalAccelerationConstraint(0.5)); + RobotController.getBatteryVoltage())); + // .addConstraint(new CentripetalAccelerationConstraint(0.5)); } @NotNull @@ -743,15 +703,15 @@ private static Command twoBallTraj( var fromBall = TrajectoryGenerator.generateTrajectory( ballPose, List.of(), endingPose, trajConfig(drive).setReversed(true)); - return //new InstantCommand(cargo::runIntake, cargo) - /*.andThen(*/new InstantCommand(cargo::deployIntake, cargo)/*)*/ - .andThen(new WaitCommand(1)) + return new InstantCommand(cargo::runIntake, cargo) + .andThen(new InstantCommand(cargo::deployIntake, cargo)) + .andThen(new WaitCommand(.4)) .andThen(ramsetePrototype.copy().traj(toBall).build()) .andThen(new WaitCommand(1)) .andThen(ramsetePrototype.copy().traj(fromBall).build()) - /*.andThen(cargo::spit, cargo) + .andThen(cargo::spit, cargo) .andThen(new WaitCommand(1)) - .andThen(cargo::stop, cargo)*/; + .andThen(cargo::stop, cargo); } /** Little helper because the verbosity of the Pose2d constructor is tiring */ diff --git a/src/main/java/frc/team449/javaMaps/IntakeTestMap.java b/src/main/java/frc/team449/javaMaps/IntakeTestMap.java deleted file mode 100644 index a2f765ad..00000000 --- a/src/main/java/frc/team449/javaMaps/IntakeTestMap.java +++ /dev/null @@ -1,412 +0,0 @@ -package frc.team449.javaMaps; - -import com.pathplanner.lib.PathPlanner; -import edu.wpi.first.hal.SimDouble; -import edu.wpi.first.hal.simulation.SimDeviceDataJNI; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.*; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.team449.CommandContainer; -import frc.team449.RobotMap; -import frc.team449._2022robot.cargo.Cargo2022; -import frc.team449.ahrs.PIDAngleControllerBuilder; -import frc.team449.components.RunningLinRegComponent; -import frc.team449.drive.unidirectional.DriveUnidirectionalWithGyro; -import frc.team449.drive.unidirectional.commands.UnidirectionalNavXDefaultDrive; -import frc.team449.generalInterfaces.doubleUnaryOperator.Polynomial; -import frc.team449.generalInterfaces.doubleUnaryOperator.RampComponent; -import frc.team449.generalInterfaces.limelight.Limelight; -import frc.team449.drive.DriveSettingsBuilder; -import frc.team449.auto.commands.RamseteBuilder; -import frc.team449.motor.builder.SparkMaxConfig; -import frc.team449.oi.throttles.ThrottlePolynomialBuilder; -import frc.team449.oi.buttons.SimpleButton; -import frc.team449.oi.throttles.ThrottleSum; -import frc.team449.oi.throttles.ThrottleWithRamp; -import frc.team449.oi.unidirectional.arcade.OIArcadeWithDPad; -import frc.team449.other.Debouncer; -import frc.team449.other.FollowerUtils; -import frc.team449.other.Updater; -import frc.team449.ahrs.AHRS; -import frc.team449.wrappers.PDP; -import frc.team449.wrappers.RumbleableJoystick; -import org.jetbrains.annotations.NotNull; - -import java.util.List; -import java.util.Map; -import java.util.Set; -import java.util.function.Supplier; - -import static frc.team449.javaMaps.FullMap.*; - -public class IntakeTestMap { - // Motor IDs - public static final int RIGHT_LEADER_PORT = 1, - RIGHT_LEADER_FOLLOWER_1_PORT = 11, - RIGHT_LEADER_FOLLOWER_2_PORT = 7, - LEFT_LEADER_PORT = 2, - LEFT_LEADER_FOLLOWER_1_PORT = 4, - LEFT_LEADER_FOLLOWER_2_PORT = 3, - INTAKE_LEADER_PORT = 8, - INTAKE_FOLLOWER_PORT = 10, - SPITTER_PORT = 9, - RIGHT_CLIMBER_MOTOR_PORT = 6, - LEFT_CLIMBER_MOTOR_PORT = 5; - /* - public int chVEL = 1; - public void changeVel() { - if (chVEL == 1) { - DRIVE_KP_VEL = 0.0001; - chVEL = 2; - } - else { - DRIVE_KP_VEL = 0.001; - chVEL = 1; - } - } - - - */ - // Other CAN IDs - public static final int PDP_CAN = 1; - // Controller ports - public static final int MECHANISMS_JOYSTICK_PORT = 0, DRIVE_JOYSTICK_PORT = 1; - // Limelight - public static final int DRIVER_PIPELINE = 0; // TODO find out what this is! - // Speeds - public static final double INTAKE_SPEED = 0.4, SPITTER_SPEED = 0.5; - public static final double AUTO_MAX_SPEED = 1.8; //0.05 for testing - public static final double AUTO_MAX_ACCEL = .35; //0.05 for testing - // Drive constants - public static final double DRIVE_WHEEL_RADIUS = Units.inchesToMeters(2); - public static final double DRIVE_GEARING = 5.86; - public static final int DRIVE_CURRENT_LIM = 50; - // old value from measuring from the outside of the wheel: 0.6492875 - // measuring from the inside of the wheel : .57785 - public static final double DRIVE_TRACK_WIDTH = 0.6492875; - - // Other constants - public static final double CLIMBER_DISTANCE = 0.5; - public static final double DRIVE_KP_VEL = 0.001, - DRIVE_KD_VEL = 0, //0.00005, - DRIVE_KP_POS = 45.269, - DRIVE_KD_POS = 3264.2, - DRIVE_FF_KS = 0.15084, - DRIVE_FF_KV = 2.4303, - DRIVE_FF_KA = 0.5323; - //if changeVEL being used change from constant to changable variable - private IntakeTestMap() {} - - @NotNull - public static RobotMap createRobotMap() { - var pdp = new PDP(PDP_CAN, new RunningLinRegComponent(250, 0.75), PowerDistribution.ModuleType.kCTRE); - var mechanismsJoystick = new RumbleableJoystick(MECHANISMS_JOYSTICK_PORT); - var driveJoystick = new RumbleableJoystick(DRIVE_JOYSTICK_PORT); - - var navx = new AHRS(SerialPort.Port.kMXP, true); - - // Widget to show robot pose+trajectory in Glass - var field = new Field2d(); - SmartDashboard.putData(field); - - var limelight = new Limelight(DRIVER_PIPELINE); - int dev = SimDeviceDataJNI.getSimDeviceHandle("navX-Sensor[0]"); - SimDouble angle = new SimDouble(SimDeviceDataJNI.getSimValueHandle(dev, "Yaw")); - // NavX expects clockwise positive, but sim outputs clockwise negative - angle.set(180); - - var driveMasterPrototype = - new SparkMaxConfig() - .setEnableBrakeMode(true) - .setUnitPerRotation(2 * Math.PI * DRIVE_WHEEL_RADIUS) // = 0.3191858136 - .setCurrentLimit(DRIVE_CURRENT_LIM) - .setPostEncoderGearing(DRIVE_GEARING) - .setEnableVoltageComp(true) - .setCalculateVel(true); // TODO remove this as soon as possible - var leftMaster = - driveMasterPrototype - .copy() - .setPort(LEFT_LEADER_PORT) - .setName("left") - .setReverseOutput(false) - .addSlaveSpark(FollowerUtils.createFollowerSpark(LEFT_LEADER_FOLLOWER_1_PORT), false) - .addSlaveSpark(FollowerUtils.createFollowerSpark(LEFT_LEADER_FOLLOWER_2_PORT), false) - .createReal(); - var rightMaster = - driveMasterPrototype - .copy() - .setName("right") - .setPort(RIGHT_LEADER_PORT) - .setReverseOutput(true) - .addSlaveSpark(FollowerUtils.createFollowerSpark(RIGHT_LEADER_FOLLOWER_1_PORT), false) - .addSlaveSpark(FollowerUtils.createFollowerSpark(RIGHT_LEADER_FOLLOWER_2_PORT), false) - .createReal(); - - var drive = - new DriveUnidirectionalWithGyro( - leftMaster, - rightMaster, - navx, - new DriveSettingsBuilder() - .feedforward(new SimpleMotorFeedforward(DRIVE_FF_KS, DRIVE_FF_KV, DRIVE_FF_KA)) - .trackWidth(DRIVE_TRACK_WIDTH) - .build()); - - var throttlePrototype = - new ThrottlePolynomialBuilder().stick(driveJoystick).smoothingTimeSecs(0.06); - var rotThrottle = - throttlePrototype - .axis(XboxController.Axis.kLeftX.value) - .deadband(0.05) - .inverted(false) - .polynomial(new Polynomial(Map.of(1., 1.), null)) - .build(); - var fwdThrottle = - new ThrottleWithRamp( - new ThrottleSum( - Set.of( - throttlePrototype - .axis(XboxController.Axis.kLeftTrigger.value) - .deadband(0.05) - .inverted(true) - .polynomial(new Polynomial(Map.of(1., 1.), null)) - .build(), - throttlePrototype - .axis(XboxController.Axis.kRightTrigger.value) - .inverted(false) - .build())), - new RampComponent(.6, .5)); - var oi = - new OIArcadeWithDPad( - rotThrottle, - fwdThrottle, - 0.1, - false, - driveJoystick, - new Polynomial( - Map.of(1., 1.), // Curvature - null), - .3, - false); - - var driveDefaultCmd = - new UnidirectionalNavXDefaultDrive<>( - 3.0, - new Debouncer(0.15), - drive, - oi, - null, - new PIDAngleControllerBuilder() - .absoluteTolerance(0) - .onTargetBuffer(new Debouncer(1.5)) - .minimumOutput(0) - .maximumOutput(0.6) - .loopTimeMillis(null) - .deadband(2) - .inverted(false) - .pid(0.01, 0, 0.03) - .build()); - - Supplier resetDriveOdometry = - () -> new InstantCommand(() -> drive.resetOdometry(new Pose2d()), drive); - SmartDashboard.putData("Reset odometry", resetDriveOdometry.get()); - - var cargo = - new Cargo2022( - new SparkMaxConfig() - .setName("intakeMotor") - .setPort(INTAKE_LEADER_PORT) - .addSlaveSpark(FollowerUtils.createFollowerSpark(INTAKE_FOLLOWER_PORT), true) - .createReal(), - new SparkMaxConfig() - .setName("spitterMotor") - .setPort(SPITTER_PORT) - .setEnableBrakeMode(false) - .createReal(), - new DoubleSolenoid( - PCM_MODULE, - PneumaticsModuleType.CTREPCM, - INTAKE_PISTON_FWD_CHANNEL, - INTAKE_PISTON_REV_CHANNEL), - INTAKE_SPEED, - SPITTER_SPEED); - Supplier runIntake = - () -> - new InstantCommand(cargo::runIntake, cargo) - .andThen(new WaitCommand(3)) - .andThen(cargo::stop); - Supplier spit = - () -> - new InstantCommand(cargo::spit, cargo).andThen(new WaitCommand(2)).andThen(cargo::stop); -// var armPrototype = -// driveMasterPrototype -// .copy() -// .setRevSoftLimit(0.) -// .setFwdSoftLimit(CLIMBER_DISTANCE) -// .setUnitPerRotation(0.1949) -// .setEnableBrakeMode(true); -// var leftArm = -// new ClimberArm( -// armPrototype -// .copy() -// .setName("climber_left") -// .setPort(LEFT_CLIMBER_MOTOR_PORT) -// .setPostEncoderGearing(10) -// .setReverseOutput(true) -// .createReal(), -// new PIDController(12, 0, 0), -// new ElevatorFeedforward(0, 0, 0)); -// var rightArm = -// new ClimberArm( -// driveMasterPrototype -// .copy() -// .setName("climber_right") -// .setPort(RIGHT_CLIMBER_MOTOR_PORT) -// .setPostEncoderGearing(10) -// .setReverseOutput(false) -// .createReal(), -// new PIDController(12, 0, 0), -// new ElevatorFeedforward(0, 0, 0)); -// var climber = new PivotingTelescopingClimber(leftArm, rightArm, CLIMBER_DISTANCE); - - // PUT YOUR SUBSYSTEM IN HERE AFTER INITIALIZING IT - var subsystems = List.of(drive, cargo /*, climber*/); - - Updater.subscribe(() -> field.setRobotPose(drive.getCurrentPose())); - -// Button bindings here -// Take in balls but don't shoot - new SimpleButton(mechanismsJoystick, XboxController.Button.kLeftBumper.value) - .whileHeld(cargo::runIntake, cargo) - .whenReleased(cargo::stop, cargo); -// Run all motors in intake to spit balls out - new SimpleButton(mechanismsJoystick, XboxController.Button.kRightBumper.value) - .whileHeld(cargo::spit, cargo) - .whenReleased(cargo::stop, cargo); - - // TODO BUTTON BINDINGS HERE - // new JoystickButton(mechanismsJoystick, XboxController.Button.kA.value) - // .whileActiveContinuous( - // new WaitCommand(0.01) - // .andThen(() -> climber.setSetpoint(climber.getSetpoint() + 0.01), - // climber)); - // new JoystickButton(mechanismsJoystick, XboxController.Button.kB.value) - // .whileActiveContinuous( - // new WaitCommand(0.01) - // .andThen(() -> climber.setSetpoint(climber.getSetpoint() - 0.01), - // climber)); - - var ramsetePrototype = - new RamseteBuilder() - .drivetrain(drive) - .leftPid(new PIDController(DRIVE_KP_VEL, 0, DRIVE_KD_VEL)) - .rightPid(new PIDController(DRIVE_KP_VEL, 0, DRIVE_KD_VEL)) - .field(field); - // - // var sCurve = - // spit.get() - // .andThen(cargo::runIntake, cargo) - // - // .andThen(ramsetePrototype.copy().name("scurvetraj").traj(sCurveTraj(drive)).build()) - // .andThen(spit.get()); - - // (assume blue alliance) - // Start at bottom next to hub, shoot preloaded ball, then get the two balls in that region and - // score those - var scoreThenGetTwoThenScore = - new InstantCommand(cargo::runIntake, cargo) - .andThen( - ramsetePrototype - .copy() - .name("1-2ballbluebottom") - .traj(loadPathPlannerTraj("New Path")) - .build()) - .andThen(spit.get()); - - // (assume blue alliance) - // Start at bottom on edge of tape, get one ball, score that and the preloaded one, go back for - // another ball, then score that - // var getOneThenScoreThenGetAnotherThenScore = - // new InstantCommand(cargo::runIntake, cargo) - // .andThen( - // ramsetePrototype - // .copy() - // .name("2-1ballbluebottom") - // .traj(loadPathPlannerTraj("2-1ball blue bottom")) - // .build() - // .alongWith( - // new WaitCommand(10).andThen(spit.get()).andThen(cargo::runIntake, - // cargo))) - // .andThen(spit.get()); - - List autoStartupCommands = List.of(resetDriveOdometry.get(), scoreThenGetTwoThenScore); - - List robotStartupCommands = List.of(); - - List teleopStartupCommands = - List.of( - /*new InstantCommand(climber::enable),*/ - new InstantCommand(() -> drive.setDefaultCommand(driveDefaultCmd)), - new InstantCommand(cargo::stop)); - - List testStartupCommands = List.of(); - var allCommands = - new CommandContainer( - robotStartupCommands, autoStartupCommands, teleopStartupCommands, testStartupCommands); - - return new RobotMap(subsystems, pdp, allCommands, false); - } - - /** Generate a trajectory for the S-shaped curve we're using to test */ - @NotNull - private static Trajectory sCurveTraj(@NotNull DriveUnidirectionalWithGyro drive) { - var ballPos = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(0)); - var fwdTraj = - TrajectoryGenerator.generateTrajectory( - new Pose2d(), List.of(), ballPos, trajConfig(drive).setReversed(false)); - var revTraj = - TrajectoryGenerator.generateTrajectory( - ballPos, List.of(), new Pose2d(), trajConfig(drive).setReversed(true)); - return fwdTraj.concatenate(revTraj); - } - - private static Trajectory testTraj(@NotNull DriveUnidirectionalWithGyro drive) { - return TrajectoryGenerator.generateTrajectory( - new Pose2d(new Translation2d(7.68, 2.82), Rotation2d.fromDegrees(-111.63)), - List.of(), - new Pose2d(new Translation2d(6.4, 1.37), Rotation2d.fromDegrees(-143.13)), - trajConfig(drive)); - } - - @NotNull - private static TrajectoryConfig trajConfig(@NotNull DriveUnidirectionalWithGyro drive) { - return new TrajectoryConfig(AUTO_MAX_SPEED, AUTO_MAX_ACCEL) - .setKinematics(drive.getDriveKinematics()) - .addConstraint( - new DifferentialDriveVoltageConstraint( - drive.getFeedforward(), drive.getDriveKinematics(), 12)); - } - - @NotNull - private static Trajectory loadPathPlannerTraj(@NotNull String trajName) { - var traj = PathPlanner.loadPath(trajName, AUTO_MAX_SPEED, AUTO_MAX_ACCEL, false); - if (traj == null) { - throw new Error("Trajectory not found: " + trajName); - } - return traj; - } -} From 5cf0c396ca02f1ad8be6842b80bae77bf4169c36 Mon Sep 17 00:00:00 2001 From: Teddy Date: Sun, 13 Mar 2022 17:01:47 -0400 Subject: [PATCH 25/25] Won DC Week 2 Day 2! --- src/main/java/frc/team449/javaMaps/FullMap.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team449/javaMaps/FullMap.java b/src/main/java/frc/team449/javaMaps/FullMap.java index 2f39538c..6d601bcf 100644 --- a/src/main/java/frc/team449/javaMaps/FullMap.java +++ b/src/main/java/frc/team449/javaMaps/FullMap.java @@ -600,9 +600,9 @@ public static RobotMap createRobotMap() { .andThen(() -> drive.resetOdometry(pose(6.06, 5.13, 136.40)), drive) .andThen(cargo::deployIntake, cargo) .andThen(new WaitCommand(.4)) - .andThen(new DriveAtSpeed<>(drive, .13, 1.5)) + .andThen(new DriveAtSpeed<>(drive, .13, 2)) .andThen(new WaitCommand(.4)) - .andThen(new DriveAtSpeed<>(drive, -.13, 4)) + .andThen(new DriveAtSpeed<>(drive, -.13, 4.4)) .andThen(cargo::spit, cargo) .andThen(new WaitCommand(2)) .andThen(cargo::stop, cargo)