diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/RobotContainer.java b/comp/swerve100/src/main/java/org/team100/frc2024/RobotContainer.java index d9dd937dd..432d18689 100644 --- a/comp/swerve100/src/main/java/org/team100/frc2024/RobotContainer.java +++ b/comp/swerve100/src/main/java/org/team100/frc2024/RobotContainer.java @@ -1,26 +1,10 @@ package org.team100.frc2024; import java.io.IOException; -import java.util.Map; import java.util.function.BooleanSupplier; -import org.team100.frc2024.commands.AutonCommand; -import org.team100.frc2024.commands.Feed; -import org.team100.frc2024.commands.Lob; -import org.team100.frc2024.commands.drivetrain.manual.ManualWithAmpLock; -import org.team100.frc2024.commands.drivetrain.manual.ManualWithShooterLock; -import org.team100.frc2024.config.AutonChooser; -import org.team100.frc2024.motion.AutoMaker; -import org.team100.frc2024.motion.FeederSubsystem; -import org.team100.frc2024.motion.ShootSmartWithRotation; -import org.team100.frc2024.motion.drivetrain.manual.AmpLockCommand; -import org.team100.frc2024.motion.intake.Intake; -import org.team100.frc2024.motion.shooter.DrumShooter; -import org.team100.frc2024.motion.shooter.Ramp; -import org.team100.frc2024.motion.shooter.TestShoot; import org.team100.lib.async.Async; import org.team100.lib.async.AsyncFactory; -import org.team100.lib.commands.AllianceCommand; import org.team100.lib.commands.drivetrain.DriveToPoseSimple; import org.team100.lib.commands.drivetrain.DriveWithProfileRotation; import org.team100.lib.commands.drivetrain.FancyTrajectory; @@ -34,7 +18,6 @@ import org.team100.lib.commands.drivetrain.manual.ManualWithFullStateHeading; import org.team100.lib.commands.drivetrain.manual.ManualWithNoteRotation; import org.team100.lib.commands.drivetrain.manual.ManualWithProfiledHeading; -import org.team100.lib.commands.drivetrain.manual.ManualWithTargetLock; import org.team100.lib.commands.drivetrain.manual.SimpleManualModuleStates; import org.team100.lib.config.Identity; import org.team100.lib.controller.drivetrain.FullStateDriveController; @@ -52,7 +35,6 @@ import org.team100.lib.hid.DriverControlProxy; import org.team100.lib.hid.OperatorControl; import org.team100.lib.hid.OperatorControlProxy; -import org.team100.lib.indicator.LEDIndicator; import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation; import org.team100.lib.localization.NotePosition24ArrayListener; import org.team100.lib.localization.SwerveDrivePoseEstimator100; @@ -77,12 +59,9 @@ import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -99,8 +78,7 @@ public class RobotContainer implements Glassy { private static final double kDriveStatorLimit = 100; private final SwerveModuleCollection m_modules; - private final Command m_auton; - private final DrumShooter m_shooter; + private final Command m_auton = null; final SwerveDriveSubsystem m_drive; public RobotContainer(TimedRobot100 robot) throws IOException { @@ -184,30 +162,6 @@ public RobotContainer(TimedRobot100 robot) throws IOException { final LoggerFactory sysLog = logger.child("Subsystems"); - final FeederSubsystem feeder = new FeederSubsystem(sysLog, m_sensors); - - final Intake intake = new Intake(sysLog, m_sensors); - - m_shooter = new DrumShooter(sysLog, 3, 13, 27, 58, 100); - - // final ClimberSubsystem climber = new ClimberSubsystem(sysLog, 60, 61); - - // final AmpFeeder m_ampFeeder = new AmpFeeder(sysLog); - // final AmpPivot m_ampPivot = new AmpPivot(sysLog); - - /////////////////////////// - // - // LEDS - // - - final LEDIndicator ledIndicator = new LEDIndicator(0); - // has no default command, registers its own periodic. - new LEDSubsystem( - ledIndicator, - m_sensors, - m_shooter, - visionDataProvider); - // m_ampFeeder = new AmpFeeder(sysLog); // m_ampPivot = new AmpPivot(sysLog); @@ -253,11 +207,12 @@ public RobotContainer(TimedRobot100 robot) throws IOException { final DriveTrajectoryFollower drivePID = driveControllerFactory.goodPIDF(PIDFlog); whileTrue(driverControl::driveToNote, - new ParallelDeadlineGroup(new DriveWithProfileRotation( + new DriveWithProfileRotation( + fieldLog, noteListener::getClosestTranslation2d, m_drive, holonomicController, - swerveKinodynamics), intake.run(intake::intakeSmart))); + swerveKinodynamics)); // try the new mintime controller final HolonomicFieldRelativeController minTimeController = new MinTimeDriveController(comLog, hlog); whileTrue(driverControl::actualCircle, @@ -269,18 +224,6 @@ public RobotContainer(TimedRobot100 robot) throws IOException { // whileTrue(driverControl::actualCircle, // new DriveInACircle(comLog, m_drive, controller, -1, viz)); - // whileTrue(driverControl::driveToAmp, - // new DriveToAmp( - // m_drive, - // holonomicController, - // swerveKinodynamics, - // m_ampPivot, - // m_ampFeeder, - // intake, - // m_shooter, - // feeder)); - - /////////////////////// // // for testing odometry @@ -306,44 +249,6 @@ public RobotContainer(TimedRobot100 robot) throws IOException { new OscillateProfile(m_drive, hp, hcontroller, 1), new OscillateProfile(m_drive, hp, hcontroller, -1)))); - // new RepeatCommand( - // new SequentialCommandGroup( - // new OscillatePosition(driveLog, m_drive, maker, controller, 1, viz), - // new OscillatePosition(driveLog, m_drive, maker, controller, -1, viz)))); - - // whileTrue(driverControl::fullCycle, new OscillateDirect(comLog, m_drive)); - // new Oscillate(comLog, m_drive)); - // new RepeatCommand( - // new FullCycle(comLog, m_drive, controller, viz))); - - // whileTrue(operatorControl::intake, - // new RunIntakeAndAmpFeeder(intake, feeder, m_ampFeeder)); - - // whileTrue(operatorControl::outtake, - // new OuttakeCommand(intake, m_shooter, m_ampFeeder, feeder)); - - whileTrue(operatorControl::ramp, new Ramp(m_shooter, m_drive)); - - whileTrue(operatorControl::feed, new Feed(intake, feeder)); - - // fast, then slow. - // whileTrue(operatorControl::pivotToAmpPosition, - // new AmpFastThenSlow(m_ampPivot, 1.7, 1.8)); - - // whileTrue(operatorControl::feedToAmp, - // new FeedToAmp(intake, m_shooter, m_ampFeeder, feeder)); - - whileTrue(operatorControl::testShoot, new TestShoot(m_shooter)); - - // whileTrue(operatorControl::outtakeFromAmp, m_ampFeeder.run(m_ampFeeder::outtake)); - - whileTrue(operatorControl::never, new Lob(m_shooter, intake)); - - // whileTrue(operatorControl::homeClimber, new HomeClimber(comLog, climber)); - - // whileTrue(operatorControl::climbUpPosition, climber.upPosition()); - // whileTrue(operatorControl::climbDownPosition, climber.downPosition()); - /////////////////////////// // // DRIVE @@ -412,117 +317,21 @@ public RobotContainer(TimedRobot100 robot) throws IOException { omegaController, driverControl::trigger)); - driveManually.register("LOCKED", false, - new ManualWithTargetLock( - fieldLog, - manLog, - swerveKinodynamics, - driverControl::target, - thetaController, - omegaController, - driverControl::trigger)); - - driveManually.register("SHOOTER_LOCK", false, - new ManualWithShooterLock( - fieldLog, - manLog, - swerveKinodynamics, - thetaController, - omegaController)); - final PIDController omega2Controller = new PIDController(0.5, 0, 0); // .5 - final ManualWithShooterLock shooterLock = new ManualWithShooterLock( - fieldLog, - comLog.child("ShooterLock"), - swerveKinodynamics, - thetaController, - omega2Controller); - - final ManualWithAmpLock ampLock = new ManualWithAmpLock( - fieldLog, - comLog, - swerveKinodynamics, - thetaController, - omega2Controller); - - final AutoMaker m_AutoMaker = new AutoMaker( - logger, - m_drive, - driveControllerFactory, - drivePID, - 0, - feeder, - m_shooter, - intake, - m_sensors, - swerveKinodynamics, - viz); - - // whileTrue(driverControl::test, m_AutoMaker.citrus(Alliance.Blue)); - whileTrue(driverControl::test, m_AutoMaker.fourNoteAuto(Alliance.Blue, m_sensors)); - - whileTrue(driverControl::ampLock, - new AmpLockCommand(ampLock, driverControl::velocity, m_drive)); - - whileTrue(driverControl::shooterLock, - new ShootSmartWithRotation(comLog, m_drive, m_shooter, feeder, intake, shooterLock, - driverControl::velocity)); - ////////////////// // // DEFAULT COMMANDS // m_drive.setDefaultCommand(driveManually); - m_shooter.setDefaultCommand(m_shooter.run(m_shooter::stop)); - feeder.setDefaultCommand(feeder.run(feeder::stop)); - intake.setDefaultCommand(intake.run(intake::stop)); - // climber.setDefaultCommand(new ClimberDefault( - // if far from the goal, go fast. if near, go slow. - // m_ampPivot.setDefaultCommand(new AmpFastThenSlow(m_ampPivot, 0.1, 0)); - - //////////////////// - // - // AUTONOMOUS - // - - // this illustrates how to use AutonCommand together with AllianceCommand - m_auton = new AutonCommand( - Map.of( - AutonChooser.Routine.FIVE_NOTE, new AllianceCommand( - m_AutoMaker.fourNoteAuto( - Alliance.Red, m_sensors), - m_AutoMaker.fourNoteAuto( - Alliance.Blue, m_sensors)), - AutonChooser.Routine.COMPLEMENTARY, new AllianceCommand( - m_AutoMaker.citrus( - Alliance.Red), - m_AutoMaker.citrus( - Alliance.Blue)), - AutonChooser.Routine.COMPLEMENTARY2, new AllianceCommand( - m_AutoMaker.citrusv2( - Alliance.Red), - m_AutoMaker.citrusv2( - Alliance.Blue)), - AutonChooser.Routine.SIBLING, new AllianceCommand( - m_AutoMaker.sibling( - Alliance.Red), - m_AutoMaker.sibling( - Alliance.Blue)), - AutonChooser.Routine.NOTHING, new AllianceCommand( - new PrintCommand("nothing red goes here"), - new PrintCommand("nothing blue goes here"))), - AutonChooser::routine); } public void beforeCommandCycle() { // ModeSelector.selectMode(operatorControl::pov); } - public void onTeleop() { - m_shooter.reset(); - } + public void onTeleop() {} public void onInit() { // m_drive.resetPose() diff --git a/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/DriveToAmp.java b/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/DriveToAmp.java index 6c3fba298..e76eed640 100644 --- a/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/DriveToAmp.java +++ b/comp/swerve100/src/main/java/org/team100/frc2024/motion/amp/DriveToAmp.java @@ -7,6 +7,7 @@ import org.team100.lib.commands.drivetrain.DriveWithProfile2; import org.team100.lib.controller.drivetrain.HolonomicFieldRelativeController; import org.team100.lib.geometry.GeometryUtil; +import org.team100.lib.logging.FieldLogger; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; @@ -33,6 +34,7 @@ public class DriveToAmp extends SequentialCommandGroup { kAmpXM, kFieldWidthM - kCloseToAmpYM, GeometryUtil.kRotation90); public DriveToAmp( + FieldLogger.Log fieldLogger, SwerveDriveSubsystem drive, HolonomicFieldRelativeController controller, SwerveKinodynamics limits, @@ -44,6 +46,7 @@ public DriveToAmp( addCommands( new ParallelDeadlineGroup( new DriveWithProfile2( + fieldLogger, () -> DriverStation.getAlliance().map( x -> switch (x) { case Red -> kRedNearAmp; @@ -57,6 +60,7 @@ public DriveToAmp( new AmpFastThenSlow(amp, 1.7, 1.8), new SequentialCommandGroup( new DriveWithProfile2( + fieldLogger, () -> DriverStation.getAlliance().map( x -> switch (x) { case Red -> kRedCloseToAmp; diff --git a/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveWithProfile2.java b/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveWithProfile2.java index 21c8fc7d5..0dc536b77 100644 --- a/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveWithProfile2.java +++ b/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveWithProfile2.java @@ -6,6 +6,7 @@ import org.team100.lib.controller.drivetrain.HolonomicFieldRelativeController; import org.team100.lib.dashboard.Glassy; import org.team100.lib.framework.TimedRobot100; +import org.team100.lib.logging.FieldLogger; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.SwerveModel; import org.team100.lib.motion.drivetrain.kinodynamics.FieldRelativeVelocity; @@ -44,6 +45,8 @@ public class DriveWithProfile2 extends Command implements Glassy { private Pose2d m_fieldRelativeGoal = null; + private final FieldLogger.Log m_field_log; + private Control100 xSetpoint; private Control100 ySetpoint; private Control100 thetaSetpoint; @@ -53,10 +56,12 @@ public class DriveWithProfile2 extends Command implements Glassy { private Model100 m_thetaGoalRaw; public DriveWithProfile2( + FieldLogger.Log fieldLogger, Supplier> fieldRelativeGoal, SwerveDriveSubsystem drivetrain, HolonomicFieldRelativeController controller, SwerveKinodynamics limits) { + m_field_log = fieldLogger; m_fieldRelativeGoalSupplier = fieldRelativeGoal; m_swerve = drivetrain; m_controller = controller; @@ -136,6 +141,10 @@ public void execute() { m_yGoalRaw = new Model100(m_fieldRelativeGoal.getY(), 0); ySetpoint = yProfile.calculate(TimedRobot100.LOOP_PERIOD_S, ySetpoint.model(), m_yGoalRaw); + m_field_log.m_log_target.log(() -> new double[] { + m_fieldRelativeGoal.getX(), + m_fieldRelativeGoal.getY(), + m_fieldRelativeGoal.getRotation().getRadians() }); thetaSetpoint = thetaProfile.calculate(TimedRobot100.LOOP_PERIOD_S, thetaSetpoint.model(), m_thetaGoalRaw); SwerveModel goalState = new SwerveModel(xSetpoint.model(), ySetpoint.model(), thetaSetpoint.model()); diff --git a/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveWithProfileRotation.java b/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveWithProfileRotation.java index f99b127b5..d670b4cdd 100644 --- a/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveWithProfileRotation.java +++ b/lib/src/main/java/org/team100/lib/commands/drivetrain/DriveWithProfileRotation.java @@ -7,6 +7,7 @@ import org.team100.lib.controller.drivetrain.HolonomicFieldRelativeController; import org.team100.lib.experiments.Experiment; import org.team100.lib.experiments.Experiments; +import org.team100.lib.logging.FieldLogger; import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem; import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics; @@ -30,11 +31,12 @@ public class DriveWithProfileRotation extends DriveWithProfile2 { private Translation2d m_previousGoal; public DriveWithProfileRotation( + FieldLogger.Log fieldLogger, Supplier> fieldRelativeGoal, SwerveDriveSubsystem drivetrain, HolonomicFieldRelativeController controller, SwerveKinodynamics limits) { - super(() -> m_goal, drivetrain, controller, limits); + super(fieldLogger, () -> m_goal, drivetrain, controller, limits); kRotationToleranceRad = 2 * Math.PI; kRotationToleranceRad_S = 6 * Math.PI; kTranslationalToleranceM = 0.05; @@ -68,7 +70,7 @@ private Optional getGoal() { // return Optional.of(m_previousGoal); } -/** + /** * Returns the current goal, or the previous one if the current one is newly * empty. */ @@ -98,6 +100,7 @@ private double getThetaGoalRad(Translation2d goal, Pose2d pose) { Translation2d toGoal = goal.minus(pose.getTranslation()); switch(Identity.instance) { case COMP_BOT: + case BLANK: return toGoal.getAngle().getRadians() + Math.PI; default: return toGoal.getAngle().getRadians(); diff --git a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModuleCollection.java b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModuleCollection.java index f86c1f976..48106f36b 100644 --- a/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModuleCollection.java +++ b/lib/src/main/java/org/team100/lib/motion/drivetrain/module/SwerveModuleCollection.java @@ -103,40 +103,40 @@ public static SwerveModuleCollection get( 32, DriveRatio.FAST, DutyCycleRotaryPositionSensor.class, 12, - 2, - 0.34, + 7, + 0.651, kinodynamics, - EncoderDrive.DIRECT, MotorPhase.REVERSE), + EncoderDrive.INVERSE, MotorPhase.REVERSE), WCPSwerveModule100.getFalconDrive(frontRightLogger, currentLimit, statorLimit, 30, DriveRatio.FAST, DutyCycleRotaryPositionSensor.class, 11, - 1, - 0.62, + 8, + 0.38, kinodynamics, - EncoderDrive.DIRECT, MotorPhase.REVERSE), + EncoderDrive.INVERSE, MotorPhase.REVERSE), WCPSwerveModule100.getFalconDrive(rearLeftLogger, currentLimit, statorLimit, 31, DriveRatio.FAST, DutyCycleRotaryPositionSensor.class, 21, - 3, - 0.59, + 6, + 0.41, kinodynamics, - EncoderDrive.DIRECT, MotorPhase.REVERSE), + EncoderDrive.INVERSE, MotorPhase.REVERSE), WCPSwerveModule100.getFalconDrive(rearRightLogger, currentLimit, statorLimit, 22, DriveRatio.FAST, DutyCycleRotaryPositionSensor.class, 33, - 0, - 0.59, + 9, + 0.388, kinodynamics, - EncoderDrive.DIRECT, MotorPhase.REVERSE)); + EncoderDrive.INVERSE, MotorPhase.REVERSE)); case BETA_BOT: case SWERVE_TWO: case BLANK: diff --git a/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleLinearMechanism.java b/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleLinearMechanism.java index e8ebbd155..b359d469b 100644 --- a/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleLinearMechanism.java +++ b/lib/src/main/java/org/team100/lib/motion/mechanism/SimpleLinearMechanism.java @@ -58,6 +58,8 @@ public void setPosition( @Override public OptionalDouble getVelocityM_S() { OptionalDouble velocityRad_S = m_encoder.getVelocityRad_S(); + + //TODO for some reason this number is always zero if (velocityRad_S.isEmpty()) return OptionalDouble.empty(); return OptionalDouble.of(velocityRad_S.getAsDouble() * m_wheelRadiusM / m_gearRatio);