Skip to content

Commit

Permalink
Merge branch 'shooter-commands' into indexer-subsystem
Browse files Browse the repository at this point in the history
  • Loading branch information
@tenumars
tenumars authored Jan 29, 2024
2 parents 0bb670a + 6932da6 commit 0cb965e
Show file tree
Hide file tree
Showing 9 changed files with 104 additions and 141 deletions.
2 changes: 1 addition & 1 deletion src/main/java/com/team1701/lib/drivers/encoders/EncoderIOAnalog.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
import edu.wpi.first.wpilibj.AnalogEncoder;

public class EncoderIOAnalog implements EncoderIO {
private final AnalogEncoder mEncoder;
public final AnalogEncoder mEncoder;

public EncoderIOAnalog(int channel) {
mEncoder = new AnalogEncoder(channel);
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/com/team1701/lib/drivers/encoders/EncoderIODigital.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
import edu.wpi.first.wpilibj.DutyCycleEncoder;

public class EncoderIODigital implements EncoderIO {
private final DutyCycleEncoder mEncoder;
public final DutyCycleEncoder mEncoder;

public EncoderIODigital(int channel) {
mEncoder = new DutyCycleEncoder(Constants.Shooter.kShooterThroughBoreEncoderId);
Expand Down
4 changes: 3 additions & 1 deletion src/main/java/com/team1701/lib/drivers/encoders/EncoderIOSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,12 @@
import edu.wpi.first.math.geometry.Rotation2d;

public class EncoderIOSim implements EncoderIO {
private final Supplier<Rotation2d> mRotationSupplier;
public final Supplier<Rotation2d> mRotationSupplier;
public double rotations;

public EncoderIOSim(Supplier<Rotation2d> rotationSupplier) {
mRotationSupplier = rotationSupplier;
rotations = mRotationSupplier.get().getRotations();
}

@Override
Expand Down
41 changes: 15 additions & 26 deletions src/main/java/com/team1701/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,18 +24,16 @@
import com.team1701.lib.util.GeometryUtil;
import com.team1701.lib.util.LoggedTunableNumber;
import com.team1701.robot.Configuration.Mode;
import com.team1701.robot.SparkFlexMotorFactory.ShooterMotorUsage;
import com.team1701.robot.commands.AutonomousCommands;
import com.team1701.robot.commands.DriveCommands;
import com.team1701.robot.commands.IndexCommand;
import com.team1701.robot.states.RobotState;
import com.team1701.robot.subsystems.drive.Drive;
import com.team1701.robot.subsystems.drive.DriveMotorFactory;
import com.team1701.robot.subsystems.drive.SwerveModule.SwerveModuleIO;
import com.team1701.robot.subsystems.indexer.Indexer;
import com.team1701.robot.subsystems.indexer.IndexerMotorFactory;
import com.team1701.robot.subsystems.shooter.Shooter;
import com.team1701.robot.subsystems.shooter.ShooterMotorFactory;
import com.team1701.robot.subsystems.shooter.ShooterMotorFactory.ShooterMotorUsage;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
Expand All @@ -45,7 +43,6 @@
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;

import static com.team1701.robot.commands.DriveCommands.*;
Expand Down Expand Up @@ -76,34 +73,32 @@ public RobotContainer() {
new GyroIOPigeon2(10),
new SwerveModuleIO[] {
new SwerveModuleIO(
DriveMotorFactory.createDriveMotorIOSparkMax(10),
DriveMotorFactory.createSteerMotorIOSparkMax(11),
SparkFlexMotorFactory.createDriveMotorIOSparkMax(10),
SparkFlexMotorFactory.createSteerMotorIOSparkMax(11),
new EncoderIOAnalog(0)),
new SwerveModuleIO(
DriveMotorFactory.createDriveMotorIOSparkMax(12),
DriveMotorFactory.createSteerMotorIOSparkMax(13),
SparkFlexMotorFactory.createDriveMotorIOSparkMax(12),
SparkFlexMotorFactory.createSteerMotorIOSparkMax(13),
new EncoderIOAnalog(1)),
new SwerveModuleIO(
DriveMotorFactory.createDriveMotorIOSparkMax(16),
DriveMotorFactory.createSteerMotorIOSparkMax(17),
SparkFlexMotorFactory.createDriveMotorIOSparkMax(16),
SparkFlexMotorFactory.createSteerMotorIOSparkMax(17),
new EncoderIOAnalog(3)),
new SwerveModuleIO(
DriveMotorFactory.createDriveMotorIOSparkMax(14),
DriveMotorFactory.createSteerMotorIOSparkMax(15),
SparkFlexMotorFactory.createDriveMotorIOSparkMax(14),
SparkFlexMotorFactory.createSteerMotorIOSparkMax(15),
new EncoderIOAnalog(2)),
},
mRobotState));

// TODO: update IDs
shooter = Optional.of(new Shooter(
ShooterMotorFactory.createShooterMotorIOSparkFlex(
SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER),
ShooterMotorFactory.createShooterMotorIOSparkFlex(
SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER),
ShooterMotorFactory.createShooterMotorIOSparkFlex(
SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION),
new DigitalIOSensor(Constants.Shooter.kShooterEntranceSensorId),
new DigitalIOSensor(Constants.Shooter.kShooterExitSensorId),
new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId)));
indexer = Optional.of(new Indexer(
IndexerMotorFactory.createDriveMotorIOSparkFlex(Constants.Indexer.kIntakeExitId),
Expand All @@ -126,10 +121,6 @@ public RobotContainer() {
Shooter.createMotorSim(DCMotor.getNeoVortex(1)),
Shooter.createMotorSim(DCMotor.getNeoVortex(1)),
Shooter.createMotorSim(DCMotor.getNeoVortex(1)),
Shooter.createDigitalSim(() ->
new LoggedDashboardBoolean("SimulatedShooter/EntranceSensorBlocked", false).get()),
Shooter.createDigitalSim(() ->
new LoggedDashboardBoolean("SimulatedShooter/ExitSensorBlocked", false).get()),
Shooter.createEncoderSim(() -> new Rotation2d(Units.degreesToRadians(
new LoggedTunableNumber("SimulatedThroughBoreEncoder/InitialAngleDegrees", 30)
.get())))));
Expand Down Expand Up @@ -163,14 +154,12 @@ public RobotContainer() {
new AprilTagCameraIO() {})); */

this.mShooter = shooter.orElseGet(() -> new Shooter(
ShooterMotorFactory.createShooterMotorIOSparkFlex(
SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER),
ShooterMotorFactory.createShooterMotorIOSparkFlex(
SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER),
ShooterMotorFactory.createShooterMotorIOSparkFlex(
SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION),
new DigitalIOSensor(Constants.Shooter.kShooterEntranceSensorId),
new DigitalIOSensor(Constants.Shooter.kShooterExitSensorId),
new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId)));

this.mIndexer = indexer.orElseGet(() -> new Indexer(new MotorIO() {}, new DigitalIO() {}, new DigitalIO() {}));
Expand Down
52 changes: 49 additions & 3 deletions ...t/subsystems/drive/DriveMotorFactory.java → ...team1701/robot/SparkFlexMotorFactory.java
View file
Original file line number Diff line number Diff line change
@@ -1,15 +1,56 @@
package com.team1701.robot.subsystems.drive;
package com.team1701.robot;

import java.util.function.Supplier;

import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import com.revrobotics.REVLibError;
import com.team1701.lib.alerts.REVAlert;
import com.team1701.lib.drivers.motors.MotorIOSparkFlex;
import com.team1701.lib.drivers.motors.MotorIOSparkMax;
import com.team1701.robot.Constants;

public final class DriveMotorFactory {
public class SparkFlexMotorFactory {
public static MotorIOSparkFlex createShooterMotorIOSparkFlex(int deviceId, ShooterMotorUsage motorUse) {
var motor = new CANSparkFlex(deviceId, MotorType.kBrushless);
var encoder = motor.getEncoder();
var controller = motor.getPIDController();
var errorAlert = new REVAlert(motor, deviceId);

motor.setCANTimeout(200);

// TODO: Update values for actual shooter
configureWithRetry(() -> motor.restoreFactoryDefaults(), errorAlert);

configureWithRetry(() -> motor.setSmartCurrentLimit(80), errorAlert);
configureWithRetry(() -> motor.enableVoltageCompensation(12), errorAlert);

configureWithRetry(() -> encoder.setPosition(0), errorAlert);
configureWithRetry(() -> encoder.setMeasurementPeriod(10), errorAlert);
configureWithRetry(() -> encoder.setAverageDepth(2), errorAlert);

switch (motorUse) {
case ROLLER:
configureWithRetry(() -> controller.setP(Constants.Shooter.kRollerKp.get()), errorAlert);
configureWithRetry(() -> controller.setD(Constants.Shooter.kRollerKd.get()), errorAlert);
configureWithRetry(() -> controller.setFF(Constants.Shooter.kRollerKff.get()), errorAlert);
break;
case ROTATION:
configureWithRetry(() -> controller.setP(Constants.Shooter.kRotationKp.get()), errorAlert);
configureWithRetry(() -> controller.setD(Constants.Shooter.kRotationKd.get()), errorAlert);
configureWithRetry(() -> controller.setFF(Constants.Shooter.kRotationKff.get()), errorAlert);
break;
default:
break;
}

configureWithRetry(() -> motor.burnFlash(), errorAlert);

motor.setCANTimeout(0);

return new MotorIOSparkFlex(motor, Constants.Shooter.kShooterReduction);
}

public static MotorIOSparkMax createDriveMotorIOSparkMax(int deviceId) {
var motor = new CANSparkMax(deviceId, MotorType.kBrushless);
var encoder = motor.getEncoder();
Expand Down Expand Up @@ -83,4 +124,9 @@ private static void configureWithRetry(Supplier<REVLibError> config, REVAlert fa

failureAlert.enable(error);
}

public enum ShooterMotorUsage {
ROLLER,
ROTATION
}
}
6 changes: 3 additions & 3 deletions src/main/java/com/team1701/robot/commands/RotateRelativeToRobot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,14 +64,14 @@ public class RotateRelativeToRobot extends Command {
@Override
public void initialize() {
mDrive.setKinematicLimits(Constants.Drive.kFastKinematicLimits);
mRotationalOffset = mRobotState.getPose2d().getRotation();
mRotationalOffset = mRobotState.getHeading();

Logger.recordOutput(kLoggingPrefix + "requestedRotation/robotRelative", mTargetRelativeRotation);
Logger.recordOutput(kLoggingPrefix + "requestedRotation/fieldRelative", mTargetFieldRotation);

mRotationController.reset();

var currentActualRotation = mRobotState.getPose2d().getRotation();
var currentActualRotation = mRobotState.getHeading();
mTargetFieldRotation = currentActualRotation.plus(mTargetRelativeRotation);
var fieldRelativeChassisSpeeds = mDrive.getFieldRelativeVelocity();
mRotationState = new TrapezoidProfile.State(
Expand Down Expand Up @@ -136,7 +136,7 @@ public boolean isFinished() {
}

public boolean atTargetPose() {
var currentRotation = mRobotState.getPose2d().getRotation();
var currentRotation = mRobotState.getHeading();
var rotationError = (mTargetRelativeRotation.plus(mRotationalOffset)).minus(currentRotation);

return Util.inRange(rotationError.getRadians(), kRotationToleranceRadians.get())
Expand Down
32 changes: 16 additions & 16 deletions src/main/java/com/team1701/robot/commands/Shoot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,43 +5,44 @@
import com.team1701.robot.states.RobotState;
import com.team1701.robot.subsystems.drive.Drive;
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.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import org.littletonrobotics.junction.AutoLogOutput;

public class Shoot extends Command {
private final Shooter mShooter;
private final Drive mDrive;

private final RobotState mRobotState = new RobotState();

@AutoLogOutput(key = "Commands/Shoot/calculatedDistanceToTarget")
private double distanceToTarget;
private Rotation2d headingToTarget;

@AutoLogOutput(key = "Commands/Shoot/calculatedShooterAngleFromHorizontal")
private Rotation2d shooterAngleFromHorizontal;
private Pose2d currentPose;
private final Translation2d targetSpeaker;

@AutoLogOutput(key = "Commands/Shoot/robotRelativeRotationDemand")
private Rotation2d robotRelativeRotationDemand;

@AutoLogOutput(key = "Commands/Shoot/shotFired")
private boolean shotFired;

public Shoot(Shooter shooter, Drive drive, boolean allianceIsBlue) {
mShooter = shooter;
mDrive = drive;
targetSpeaker = allianceIsBlue
? FieldConstants.kBlueSpeakerOpeningCenter.toTranslation2d()
: FieldConstants.kRedSpeakerOpeningCenter.toTranslation2d();
addRequirements(shooter, drive);
}

@Override
public void initialize() {
shotFired = false;
currentPose = mRobotState.getPose2d();
distanceToTarget = currentPose.getTranslation().getDistance(targetSpeaker);
headingToTarget =
new Rotation2d(targetSpeaker.getX() - currentPose.getX(), targetSpeaker.getY() - currentPose.getY());
robotRelativeRotationDemand = headingToTarget.minus(currentPose.getRotation());
distanceToTarget = mRobotState
.getPose2d()
.getTranslation()
.getDistance(mRobotState.getSpeakerPose().toTranslation2d());
robotRelativeRotationDemand =
mRobotState.getSpeakerHeading().minus(mRobotState.getPose2d().getRotation());
shooterAngleFromHorizontal = new Rotation2d(
distanceToTarget - Constants.Shooter.kShooterAxisOffset,
FieldConstants.kBlueSpeakerOpeningCenter.getZ() - Constants.Shooter.kShooterAxisHeight);
Expand All @@ -50,14 +51,13 @@ public void initialize() {

@Override
public void execute() {
if (currentPose.getRotation() != headingToTarget) {
if (mRobotState.getPose2d().getRotation() != mRobotState.getSpeakerHeading()) {
DriveCommands.rotateRelativeToRobot(
mDrive, robotRelativeRotationDemand, Constants.Drive.kFastKinematicLimits, true);
}
// TODO add throughBoreEncoder

mShooter.setRotationAngle(shooterAngleFromHorizontal);

// TODO: add check for value change
mShooter.setRollerSpeed(Units.rotationsPerMinuteToRadiansPerSecond(Constants.Motors.kMaxKrakenRPM));
// TODO: implement autostart/stop with sensors

Expand Down
Loading

0 comments on commit 0cb965e

Please sign in to comment.