Skip to content

Commit

Permalink
Built out shooter command and simulation
Browse files Browse the repository at this point in the history
  • Loading branch information
@mpulte
mpulte committed Feb 3, 2024
1 parent 8fd214f commit af5e65e
Show file tree
Hide file tree
Showing 15 changed files with 324 additions and 239 deletions.
12 changes: 12 additions & 0 deletions src/main/java/com/team1701/lib/util/GeometryUtil.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.math.geometry.Twist2d;

Expand Down Expand Up @@ -32,4 +33,15 @@ public static boolean isNear(Rotation2d expected, Rotation2d actual, Rotation2d
var difference = MathUtil.angleModulus(expected.minus(actual).getRadians());
return MathUtil.isNear(difference, 0.0, tolerance.getRadians());
}

public static boolean isNear(Translation2d expected, Translation2d actual, double tolerance) {
var difference = expected.minus(actual).getNorm();
return MathUtil.isNear(difference, 0.0, tolerance);
}

public static boolean isNear(
Pose2d expected, Pose2d actual, double translationTolerance, Rotation2d rotationTolerance) {
return isNear(expected.getTranslation(), actual.getTranslation(), translationTolerance)
&& isNear(expected.getRotation(), actual.getRotation(), rotationTolerance);
}
}
35 changes: 22 additions & 13 deletions src/main/java/com/team1701/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import com.team1701.lib.swerve.ExtendedSwerveDriveKinematics;
import com.team1701.lib.swerve.SwerveSetpointGenerator.KinematicLimits;
import com.team1701.lib.util.LoggedTunableNumber;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;

Expand Down Expand Up @@ -157,7 +158,8 @@ public static final class Drive {

public static final class Shooter {
// TODO: Update values
public static final double kShooterReduction = 1;
public static final double kRollerReduction = 1.0;
public static final double kAngleReduction = 1.0 / 105.0;
public static final int kShooterUpperRollerMotorId = 0;
public static final int kShooterLowerRollerMotorId = 1;
public static final int kShooterRotationMotorId = 2;
Expand All @@ -173,6 +175,8 @@ public static final class Shooter {
public static final LoggedTunableNumber kRotationKp = new LoggedTunableNumber("Shooter/Motor/Rotation/Kp");
public static final LoggedTunableNumber kRotationKd = new LoggedTunableNumber("Shooter/Motor/Rotation/Kd");

public static final Rotation2d kShooterAngleEncoderOffset;

public static int kShooterEntranceSensorId;
public static int kShooterExitSensorId;
public static int kShooterThroughBoreEncoderId;
Expand All @@ -182,23 +186,28 @@ public static final class Shooter {
static {
switch (Configuration.getRobot()) {
case COMPETITION_BOT:
kRollerKd.initDefault(1);
kRollerKp.initDefault(0.6);
kRollerKff.initDefault(0);
kRollerKff.initDefault(0.0);
kRollerKp.initDefault(0.0);
kRollerKd.initDefault(0.0);

kRotationKff.initDefault(0.0);
kRotationKp.initDefault(0.0);
kRotationKd.initDefault(0.0);

kRotationKd.initDefault(1);
kRotationKp.initDefault(0.6);
kRotationKff.initDefault(0);
kShooterAngleEncoderOffset = Rotation2d.fromRotations(0); // TODO: Update value

break;
case SIMULATION_BOT:
kRollerKd.initDefault(1);
kRollerKp.initDefault(0.6);
kRollerKff.initDefault(0);
kRollerKff.initDefault(0.017);
kRollerKp.initDefault(0.1);
kRollerKd.initDefault(0.0);

kRotationKff.initDefault(0.0);
kRotationKp.initDefault(2.0);
kRotationKd.initDefault(0.0);

kShooterAngleEncoderOffset = Rotation2d.fromRotations(Math.random());

kRotationKd.initDefault(1);
kRotationKp.initDefault(0.6);
kRotationKff.initDefault(0);
break;
default:
throw new UnsupportedOperationException("No shooter configuration for " + Configuration.getRobot());
Expand Down
5 changes: 3 additions & 2 deletions src/main/java/com/team1701/robot/FieldConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,11 @@ public final class FieldConstants {
public static double kCenterLine = kFieldLongLengthMeters / 2.0;
public static double kWingLength = Units.inchesToMeters(231.2);

public static double kSpeakerHeight = 204.5;
public static Translation3d kBlueSpeakerOpeningCenter =
new Translation3d(Units.inchesToMeters(0.9), Units.inchesToMeters(218.42), 204.5);
new Translation3d(Units.inchesToMeters(0.9), Units.inchesToMeters(218.42), kSpeakerHeight);
public static Translation3d kRedSpeakerOpeningCenter = new Translation3d(
kFieldLongLengthMeters - kBlueSpeakerOpeningCenter.getX(),
kBlueSpeakerOpeningCenter.getY(),
kBlueSpeakerOpeningCenter.getZ());
kSpeakerHeight);
}
5 changes: 5 additions & 0 deletions src/main/java/com/team1701/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import edu.wpi.first.hal.AllianceStationID;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import io.javalin.Javalin;
Expand Down Expand Up @@ -99,6 +100,9 @@ private void initializeAdvantageKit() {
// Build robot container
mRobotContainer = new RobotContainer();

// Enable command logging
SmartDashboard.putData(CommandScheduler.getInstance());

// Launch web server
Javalin.create(config -> {
config.staticFiles.add(
Expand All @@ -112,6 +116,7 @@ private void initializeAdvantageKit() {
@Override
public void robotPeriodic() {
CommandScheduler.getInstance().run();
mRobotContainer.getRobotState().periodic();
}

@Override
Expand Down
36 changes: 18 additions & 18 deletions src/main/java/com/team1701/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
import com.team1701.lib.drivers.motors.MotorIO;
import com.team1701.lib.drivers.motors.MotorIOSim;
import com.team1701.lib.util.GeometryUtil;
import com.team1701.lib.util.LoggedTunableNumber;
import com.team1701.robot.Configuration.Mode;
import com.team1701.robot.commands.AutonomousCommands;
import com.team1701.robot.commands.DriveCommands;
Expand All @@ -35,9 +34,7 @@
import com.team1701.robot.util.SparkFlexMotorFactory;
import com.team1701.robot.util.SparkFlexMotorFactory.ShooterMotorUsage;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
Expand All @@ -50,14 +47,13 @@

public class RobotContainer {
private final RobotState mRobotState = new RobotState();
public final Drive mDrive;
public final Shooter mShooter;
public final Indexer mIndexer;
private final Drive mDrive;
private final Shooter mShooter;
private final Indexer mIndexer;

// public final Vision mVision;

private final CommandXboxController mDriverController = new CommandXboxController(0);
// Trigger bButton = mDriverController.b();
private final LoggedDashboardChooser<Command> autonomousModeChooser = new LoggedDashboardChooser<>("Auto Mode");

public RobotContainer() {
Expand Down Expand Up @@ -101,7 +97,7 @@ public RobotContainer() {
Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION),
new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId)));
indexer = Optional.of(new Indexer(
SparkFlexMotorFactory.createIndexerMotorFactory(Constants.Indexer.kIndexerMotorId),
SparkFlexMotorFactory.createIndexerMotorIOSparkFlex(Constants.Indexer.kIndexerMotorId),
new DigitalIOSensor(Constants.Indexer.kIndexerEntranceSensorId),
new DigitalIOSensor(Constants.Indexer.kIndexerExitSensorId)));
break;
Expand All @@ -117,13 +113,14 @@ public RobotContainer() {
() -> simDrive.getVelocity().omegaRadiansPerSecond, Constants.kLoopPeriodSeconds);

drive = Optional.of(simDrive);

var rotationMotor = Shooter.createRotationMotorSim(DCMotor.getNeoVortex(1));
shooter = Optional.of(new Shooter(
Shooter.createMotorSim(DCMotor.getNeoVortex(1)),
Shooter.createMotorSim(DCMotor.getNeoVortex(1)),
Shooter.createMotorSim(DCMotor.getNeoVortex(1)),
Shooter.createEncoderSim(() -> new Rotation2d(Units.degreesToRadians(
new LoggedTunableNumber("SimulatedThroughBoreEncoder/InitialAngleDegrees", 30)
.get())))));
Shooter.createRollerMotorSim(DCMotor.getNeoVortex(1)),
Shooter.createRollerMotorSim(DCMotor.getNeoVortex(1)),
rotationMotor,
Shooter.createEncoderSim(rotationMotor)));

indexer = Optional.of(new Indexer(
new MotorIOSim(DCMotor.getNeoVortex(1), 1, 0.001, Constants.kLoopPeriodSeconds),
new DigitalIOSim(() -> false),
Expand Down Expand Up @@ -192,10 +189,9 @@ private void setupControllerBindings() {
.withName("ZeroGyroscopeToHeading"));
mDriverController
.rightBumper()
.whileTrue(DriveCommands.rotateRelativeToRobot(
mDrive, () -> mRobotState.getSpeakerHeading(), Constants.Drive.kFastKinematicLimits, true));
.whileTrue(
DriveCommands.rotateToSpeaker(mDrive, mRobotState, Constants.Drive.kFastKinematicLimits, true));
mDriverController.leftTrigger().whileTrue(swerveLock(mDrive));
TriggeredAlert.info("Driver right bumper pressed", mDriverController.rightBumper());

DriverStation.silenceJoystickConnectionWarning(true);
}
Expand All @@ -214,7 +210,7 @@ private void setupAutonomous() {
PathPlannerLogging.setLogActivePathCallback(
poses -> Logger.recordOutput("PathPlanner/Path", poses.toArray(Pose2d[]::new)));

var commands = new AutonomousCommands(mRobotState, mDrive);
var commands = new AutonomousCommands(mRobotState, mDrive, mShooter, mIndexer);
autonomousModeChooser.addDefaultOption("Demo", commands.demo());
autonomousModeChooser.addOption("Four Piece", commands.fourPiece());
autonomousModeChooser.addOption("Four Piece Planner", new PathPlannerAuto("FourPiece"));
Expand All @@ -229,4 +225,8 @@ private void setupStateTriggers() {
public Optional<Command> getAutonomousCommand() {
return Optional.ofNullable(autonomousModeChooser.get());
}

public RobotState getRobotState() {
return mRobotState;
}
}
13 changes: 12 additions & 1 deletion src/main/java/com/team1701/robot/commands/AutonomousCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@
import com.team1701.robot.FieldConstants;
import com.team1701.robot.states.RobotState;
import com.team1701.robot.subsystems.drive.Drive;
import com.team1701.robot.subsystems.indexer.Indexer;
import com.team1701.robot.subsystems.shooter.Shooter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
Expand All @@ -23,10 +25,14 @@
public class AutonomousCommands {
private final RobotState mRobotState;
private final Drive mDrive;
private final Shooter mShooter;
private final Indexer mIndexer;

public AutonomousCommands(RobotState robotState, Drive drive) {
public AutonomousCommands(RobotState robotState, Drive drive, Shooter shooter, Indexer indexer) {
mRobotState = robotState;
mDrive = drive;
mShooter = shooter;
mIndexer = indexer;

NamedCommands.registerCommand("printHello", print("Hello from autonomous path"));
}
Expand Down Expand Up @@ -96,10 +102,15 @@ private Command followChoreoPath(String pathName, boolean resetPose) {
return new DriveChoreoTrajectory(mDrive, trajectory, mRobotState, resetPose);
}

private Command aimAndShoot() {
return ShootCommands.aimAndShoot(mShooter, mIndexer, mDrive, mRobotState);
}

public Command demo() {
return loggedSequence(
print("Starting demo"),
followPath("demo1", true),
aimAndShoot(),
driveToPose(new Pose2d(2.0, 1.0, Rotation2d.fromRadians(-Math.PI * 2.0 / 3.0))),
driveToPose(new Pose2d(10.0, 1.0, GeometryUtil.kRotationHalfPi)),
driveToPose(
Expand Down
13 changes: 11 additions & 2 deletions src/main/java/com/team1701/robot/commands/DriveCommands.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
import java.util.function.Supplier;

import com.team1701.lib.swerve.SwerveSetpointGenerator.KinematicLimits;
import com.team1701.robot.states.RobotState;
import com.team1701.robot.subsystems.drive.Drive;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -29,12 +30,20 @@ public static Command driveToPose(
return new DriveToPose(drive, poseSupplier, robotPoseSupplier, kinematicLimits, finishAtPose);
}

public static RotateToFieldHeading rotateRelativeToRobot(
public static Command rotateToSpeaker(
Drive drive, RobotState state, KinematicLimits kinematicLimits, boolean finishAtRotation) {
return rotateToFieldHeading(
drive, state::getSpeakerHeading, state::getHeading, kinematicLimits, finishAtRotation);
}

public static Command rotateToFieldHeading(
Drive drive,
Supplier<Rotation2d> targetFieldHeading,
Supplier<Rotation2d> robotHeadingSupplier,
KinematicLimits kinematicLimits,
boolean finishAtRotation) {
return new RotateToFieldHeading(drive, targetFieldHeading, kinematicLimits, finishAtRotation);
return new RotateToFieldHeading(
drive, targetFieldHeading, robotHeadingSupplier, kinematicLimits, finishAtRotation);
}

public static Command swerveLock(Drive drive) {
Expand Down
Loading

0 comments on commit af5e65e

Please sign in to comment.