diff --git a/comp/simgui-ds.json b/comp/simgui-ds.json index a2e3cce..5729c19 100644 --- a/comp/simgui-ds.json +++ b/comp/simgui-ds.json @@ -4,9 +4,9 @@ "visible": false } }, - "Joysticks": { + "Keyboard 0 Settings": { "window": { - "visible": false + "visible": true } }, "System Joysticks": { @@ -18,8 +18,8 @@ { "axisConfig": [ { - "decKey": 69, - "incKey": 82 + "decKey": 81, + "incKey": 69 }, {}, { @@ -27,13 +27,13 @@ "keyRate": 0.009999999776482582 }, {}, - { - "decKey": 65, - "incKey": 68 - }, { "decKey": 87, "incKey": 83 + }, + { + "decKey": 68, + "incKey": 65 } ], "axisCount": 6, diff --git a/comp/src/main/java/org/team100/frc2026/Intake.java b/comp/src/main/java/org/team100/frc2026/Intake.java index 963b919..864e671 100644 --- a/comp/src/main/java/org/team100/frc2026/Intake.java +++ b/comp/src/main/java/org/team100/frc2026/Intake.java @@ -53,7 +53,7 @@ public void periodic() { public Command intake() { return run(this::fullSpeed).withName("Intake Full Speed"); - } + } public Command stop() { return run(this::stopMotor).withName("Intake Stop"); @@ -69,6 +69,7 @@ private void fullSpeed() { double velocityRad_S = 450; m_motor.setVelocity(velocityRad_S, 0, 0); // m_motor.setDutyCycle(1); + System.out.println(velocityRad_S + "wow"); } } diff --git a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java index 7977b2a..381442b 100644 --- a/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java +++ b/comp/src/main/java/org/team100/frc2026/auton/AllAutons.java @@ -35,6 +35,16 @@ public AllAutons(Machinery machinery, ControllerSE2 controller) { machinery.m_swerveKinodynamics, controller, machinery)); + m_autonChooser.add(new AutonTest( + log, + machinery.m_swerveKinodynamics, + controller, + machinery)); + m_autonChooser.add(new Auton1( + log, + machinery.m_swerveKinodynamics, + controller, + machinery)); } public Command get() { diff --git a/comp/src/main/java/org/team100/frc2026/auton/Auton1.java b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java new file mode 100644 index 0000000..4c16771 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/Auton1.java @@ -0,0 +1,139 @@ +package org.team100.frc2026.auton; + +import static edu.wpi.first.wpilibj2.command.Commands.parallel; +import static edu.wpi.first.wpilibj2.command.Commands.sequence; +import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds; + +import java.util.List; +import java.util.ArrayList; + +import org.team100.frc2026.Intake; +import org.team100.frc2026.auton.BumpZones; +import org.team100.frc2026.robot.Machinery; +import org.team100.lib.config.AnnotatedCommand; +import org.team100.lib.controller.se2.ControllerSE2; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.WaypointSE2; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction; +import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; +import org.team100.lib.trajectory.TrajectorySE2; +import org.team100.lib.trajectory.TrajectorySE2Factory; +import org.team100.lib.trajectory.TrajectorySE2Planner; +import org.team100.lib.trajectory.constraint.TimingConstraint; +import org.team100.lib.trajectory.constraint.TimingConstraintFactory; +import org.team100.lib.trajectory.constraint.VelocityLimitRegionConstraint; +import org.team100.lib.trajectory.path.PathSE2Factory; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example of a simple sequence */ +public class Auton1 implements AnnotatedCommand { + private final LoggerFactory log; + private final ControllerSE2 controller; + private final Machinery machinery; + private final List constraints; + private final TrajectorySE2Factory trajectoryFactory; + private final PathSE2Factory pathFactory; + private final TrajectorySE2Planner planner; + + public Auton1( + LoggerFactory parent, + SwerveKinodynamics kinodynamics, + ControllerSE2 controller, + Machinery machinery) { + log = parent.name(name()); + this.controller = controller; + this.machinery = machinery; + constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); + // In meters/second + double maxBumpVelocity = 1; + List new_constraints = new ArrayList<>(constraints); + + // create a new VelocityRegionContstraint `slow_bu mp_zone` + VelocityLimitRegionConstraint slow_bump_zone = new VelocityLimitRegionConstraint(log, BumpZones.BLUE_BUMP_LEFT, maxBumpVelocity); + new_constraints.add(slow_bump_zone); + // constraints.add(slow_bump_zone); + trajectoryFactory = new TrajectorySE2Factory(new_constraints); + pathFactory = new PathSE2Factory(); + planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory); + } + + @Override + public String name() { + return "Auton 1"; + } + + TrajectorySE2 t1(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(1, 0, 0), 1), + new WaypointSE2(AutonPositions.ABOVE_BALL_FIELD, + new DirectionSE2(1, 1, 0), 1)); + return planner.restToRest(waypoints); + } + + TrajectorySE2 t2(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(0, -1, 0), 1), + new WaypointSE2(AutonPositions.MIDDLE_BALL_FIELD, + new DirectionSE2(0, -1, 0), 1)); + return planner.restToRest(waypoints); + } + + TrajectorySE2 t3(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(-1, 1, 0), 1), + new WaypointSE2(StartingPositions.LEFT_BUMP, + new DirectionSE2(-1, 0, 0), 1), + new WaypointSE2(AutonPositions.SHOOT_LEFT, + new DirectionSE2(-1, -1, 0), 1)); + return planner.restToRest(waypoints); + } + + TrajectorySE2 t4(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(-1, -1, 0), 1), + new WaypointSE2(AutonPositions.CLIMB_LEFT, + new DirectionSE2(-1, -1, 0), 1)); + return planner.restToRest(waypoints); + } + + @Override + public Command command() { + DriveWithTrajectoryFunction n1 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t1); + DriveWithTrajectoryFunction n2 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t2); + DriveWithTrajectoryFunction n3 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t3); + DriveWithTrajectoryFunction n4 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t4); + return sequence( + n1.until(n1::isDone), + waitSeconds(1), + parallel( + n2, + machinery.m_intake.intake() + ).until(n2::isDone), + waitSeconds(1), + n3.until(n3::isDone), + waitSeconds(1), + n4.until(n4::isDone)); + } + + @Override + public Pose2d start() { + return StartingPositions.LEFT_BUMP; + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java b/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java new file mode 100644 index 0000000..1eccdec --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/AutonPositions.java @@ -0,0 +1,19 @@ +package org.team100.frc2026.auton; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +/** + * Areas that might be useful for autons + */ +public class AutonPositions { + public static final Pose2d ABOVE_BALL_FIELD = new Pose2d(7.75, 7, Rotation2d.kCW_90deg); + public static final Pose2d MIDDLE_BALL_FIELD = new Pose2d(7.75, 4, Rotation2d.kCW_90deg); + public static final Pose2d BELOW_BALL_FIELD = new Pose2d(7.75, 1, Rotation2d.kCW_90deg); + + public static final Pose2d CLIMB_LEFT = new Pose2d(1.175, 4.25, new Rotation2d(-135 * (Math.PI / 180))); + public static final Pose2d CLIMB_RIGHT = new Pose2d(1.175, 3.1, new Rotation2d(135 * (Math.PI / 180))); + + public static final Pose2d SHOOT_LEFT = new Pose2d(2.3, 5.1, new Rotation2d(-35 * (Math.PI / 180))); + public static final Pose2d SHOOT_RIGHT = new Pose2d(2.3, 2.7, new Rotation2d(35 * (Math.PI / 180))); +} diff --git a/comp/src/main/java/org/team100/frc2026/auton/AutonTest.java b/comp/src/main/java/org/team100/frc2026/auton/AutonTest.java new file mode 100644 index 0000000..57f1229 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/AutonTest.java @@ -0,0 +1,110 @@ +package org.team100.frc2026.auton; + +import static edu.wpi.first.wpilibj2.command.Commands.sequence; +import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds; + +import java.util.List; + +import org.team100.frc2026.robot.Machinery; +import org.team100.lib.config.AnnotatedCommand; +import org.team100.lib.controller.se2.ControllerSE2; +import org.team100.lib.geometry.DirectionSE2; +import org.team100.lib.geometry.WaypointSE2; +import org.team100.lib.logging.LoggerFactory; +import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction; +import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics; +import org.team100.lib.trajectory.TrajectorySE2; +import org.team100.lib.trajectory.TrajectorySE2Factory; +import org.team100.lib.trajectory.TrajectorySE2Planner; +import org.team100.lib.trajectory.constraint.TimingConstraint; +import org.team100.lib.trajectory.constraint.TimingConstraintFactory; +import org.team100.lib.trajectory.path.PathSE2Factory; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; + +/** An example of a simple sequence */ +public class AutonTest implements AnnotatedCommand { + private final LoggerFactory log; + private final ControllerSE2 controller; + private final Machinery machinery; + private final List constraints; + private final TrajectorySE2Factory trajectoryFactory; + private final PathSE2Factory pathFactory; + private final TrajectorySE2Planner planner; + + public AutonTest( + LoggerFactory parent, + SwerveKinodynamics kinodynamics, + ControllerSE2 controller, + Machinery machinery) { + log = parent.name(name()); + this.controller = controller; + this.machinery = machinery; + constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this)); + trajectoryFactory = new TrajectorySE2Factory(constraints); + pathFactory = new PathSE2Factory(); + planner = new TrajectorySE2Planner(pathFactory, trajectoryFactory); + } + + @Override + public String name() { + return "Auton Test"; + } + + TrajectorySE2 t1(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(-1, 0, 0), 1), + new WaypointSE2(new Pose2d(2, 4, Rotation2d.kZero), + new DirectionSE2(-1, 1, 0), 1)); + return planner.restToRest(waypoints); + } + + TrajectorySE2 t2(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(1, 1, 0), 1), + new WaypointSE2(AutonPositions.ABOVE_BALL_FIELD, + new DirectionSE2(1, 1, 0), 1), + new WaypointSE2(AutonPositions.BELOW_BALL_FIELD, + new DirectionSE2(0, -1, 0), 1)); + return planner.restToRest(waypoints); + } + + // go back where we started. + TrajectorySE2 t3(Pose2d startingPose) { + List waypoints = List.of( + new WaypointSE2(startingPose, + new DirectionSE2(0, 1, 0), 1), + new WaypointSE2(StartingPositions.RIGHT_BUMP, + new DirectionSE2(-1, 0, 0), 1)); + return planner.restToRest(waypoints); + } + + @Override + public Command command() { + DriveWithTrajectoryFunction n1 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t1); + DriveWithTrajectoryFunction n2 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t2); + DriveWithTrajectoryFunction n3 = new DriveWithTrajectoryFunction( + log, machinery.m_drive, controller, + machinery.m_trajectoryViz, this::t3); + return sequence( + n1.until(n1::isDone), + waitSeconds(1), + n2.until(n2::isDone), + waitSeconds(1), + n3.until(n3::isDone)); + } + + @Override + public Pose2d start() { + return StartingPositions.RIGHT_BUMP; + } + +} diff --git a/comp/src/main/java/org/team100/frc2026/auton/BumpZones.java b/comp/src/main/java/org/team100/frc2026/auton/BumpZones.java new file mode 100644 index 0000000..5646319 --- /dev/null +++ b/comp/src/main/java/org/team100/frc2026/auton/BumpZones.java @@ -0,0 +1,31 @@ +package org.team100.frc2026.auton; + +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class BumpZones { + public static final Rectangle2d BLUE_BUMP_LEFT = + new Rectangle2d( + new Translation2d(5.2, 6.43), + new Translation2d(4.1, 4.6) + ); + + public static final Rectangle2d BLUE_BUMP_RIGHT = + new Rectangle2d( + new Translation2d(5.2, 3.2), + new Translation2d(4.1, 1.64) + ); + + public static final Rectangle2d RED_BUMP_RIGHT = + new Rectangle2d( + new Translation2d(12.5, 6.43), + new Translation2d(11.32, 4.6) + ); + + public static final Rectangle2d RED_BUMP_LEFT = + new Rectangle2d( + new Translation2d(12.5, 3.2), + new Translation2d(11.32, 1.64) + ); + +} diff --git a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java index 081029c..4651b0f 100644 --- a/comp/src/main/java/org/team100/frc2026/robot/Machinery.java +++ b/comp/src/main/java/org/team100/frc2026/robot/Machinery.java @@ -71,8 +71,8 @@ public class Machinery { public final SwerveDriveSubsystem m_drive; final Beeper m_beeper; final Shooter m_shooter; - final Intake m_intake; - final IntakeExtend m_extender; + public final Intake m_intake; + public final IntakeExtend m_extender; final Serializer m_serializer; final ClimberExtension m_ClimberExtension; final Climber m_Climber;