Compare commits
8 Commits
3791333f56
...
918876923f
| Author | SHA1 | Date | |
|---|---|---|---|
| 918876923f | |||
|
|
208cfa3ce4 | ||
| fb937d86dc | |||
| db443cfe63 | |||
| cbcfc9cab0 | |||
| 96fb68cb32 | |||
| 80ef3a3431 | |||
| 866e6b99df |
31
src/main/deploy/pathplanner/autos/Outpost.auto
Normal file
31
src/main/deploy/pathplanner/autos/Outpost.auto
Normal file
@@ -0,0 +1,31 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Start to outpost"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Outpost to Ranged Shot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Auto Shoot"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.7089624329216013,
|
||||
"y": 0.668228980317108
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.1043470483005366,
|
||||
"y": 0.7169051878354196
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": "Outpost"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.3639534883720925,
|
||||
"y": 2.9722361359570666
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.234150268336314,
|
||||
"y": 2.5666010733452596
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 24.304549265936604
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Right Outpost",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
66
src/main/deploy/pathplanner/paths/Start to outpost.path
Normal file
66
src/main/deploy/pathplanner/paths/Start to outpost.path
Normal file
@@ -0,0 +1,66 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.6619856887266913,
|
||||
"y": 2.2583184257605784
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.4937567084046877,
|
||||
"y": 1.3172450805011864
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.7089624329216013,
|
||||
"y": 0.668228980317108
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.6987119856944102,
|
||||
"y": 1.0414132379199703
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": "Outpost"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [
|
||||
{
|
||||
"waypointRelativePos": 0.7234468937875753,
|
||||
"rotationDegrees": 180.0
|
||||
}
|
||||
],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [
|
||||
{
|
||||
"name": "Intake Start",
|
||||
"waypointRelativePos": 0.4500000000000002,
|
||||
"endWaypointRelativePos": null,
|
||||
"command": null
|
||||
}
|
||||
],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 3.0,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 540.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Right Outpost",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -2,7 +2,9 @@
|
||||
"robotWidth": 0.921,
|
||||
"robotLength": 0.787,
|
||||
"holonomicMode": true,
|
||||
"pathFolders": [],
|
||||
"pathFolders": [
|
||||
"Right Outpost"
|
||||
],
|
||||
"autoFolders": [],
|
||||
"defaultMaxVel": 3.0,
|
||||
"defaultMaxAccel": 3.0,
|
||||
|
||||
@@ -42,7 +42,7 @@ import frc.robot.utilities.Elastic;
|
||||
import frc.robot.utilities.Utilities;
|
||||
|
||||
public class RobotContainer {
|
||||
//private PhotonVision vision;
|
||||
private PhotonVision vision;
|
||||
private Drivetrain drivetrain;
|
||||
private Hood hood;
|
||||
private Shooter shooter;
|
||||
@@ -59,7 +59,7 @@ public class RobotContainer {
|
||||
private Timer shiftTimer;
|
||||
|
||||
public RobotContainer() {
|
||||
//vision = new PhotonVision();
|
||||
vision = new PhotonVision();
|
||||
drivetrain = new Drivetrain(null);
|
||||
hood = new Hood();
|
||||
shooter = new Shooter();
|
||||
@@ -68,14 +68,14 @@ public class RobotContainer {
|
||||
spindexer = new Spindexer();
|
||||
//climber = new Climber();
|
||||
|
||||
/*
|
||||
|
||||
vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
|
||||
vision.addPoseEstimateConsumer((vp) -> {
|
||||
Logger.recordOutput(
|
||||
"Vision/" + vp.cameraName() + "/Pose",
|
||||
vp.visualPose()
|
||||
);
|
||||
});*/
|
||||
});
|
||||
|
||||
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
|
||||
secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
|
||||
@@ -108,8 +108,8 @@ public class RobotContainer {
|
||||
// This should just work, if it doesn't it's likely modules aren't assigned the right IDs
|
||||
// after the electronics rebuild. For testing normal operation nothing about the Drivetrain
|
||||
// class should need to change
|
||||
drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true));
|
||||
/*drivetrain.setDefaultCommand(
|
||||
//drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true));
|
||||
drivetrain.setDefaultCommand(
|
||||
drivetrain.drive(
|
||||
driver::getLeftY,
|
||||
driver::getLeftX,
|
||||
@@ -118,6 +118,8 @@ public class RobotContainer {
|
||||
)
|
||||
);
|
||||
|
||||
driver.y().whileTrue(drivetrain.zeroHeading());
|
||||
/*
|
||||
// This needs to be tested after a prolonged amount of driving around <i>aggressively</i>.
|
||||
// Do things like going over the bump repeatedly, spin around a bunch, etc.
|
||||
// If this works well over time, then this is likely all we need
|
||||
@@ -168,7 +170,6 @@ public class RobotContainer {
|
||||
// DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the
|
||||
// constants file for the subsystem having the problem
|
||||
driver.rightBumper().whileTrue(
|
||||
//intakeRoller.runIn().alongWith(spindexer.spinToShooter())
|
||||
spindexer.spinToShooter()
|
||||
);
|
||||
|
||||
@@ -178,7 +179,8 @@ public class RobotContainer {
|
||||
// DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the
|
||||
// constants file for the subsystem having the problem
|
||||
driver.leftBumper().whileTrue(
|
||||
intakeRoller.runOut().alongWith(spindexer.spinToIntake())
|
||||
intakeRoller.runIn()
|
||||
//intakeRoller.runOut().alongWith(spindexer.spinToIntake())
|
||||
);
|
||||
|
||||
// While holding D-Pad up on the secondary controller, the shooter should spin
|
||||
@@ -226,6 +228,10 @@ public class RobotContainer {
|
||||
secondary.x().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kDown));
|
||||
secondary.y().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kUp));
|
||||
|
||||
secondary.povUp().whileTrue(hood.trackToAngle(() -> Math.toRadians(40.0)));
|
||||
secondary.povDown().whileTrue(hood.trackToAngle(() -> Math.toRadians(0.0)));
|
||||
|
||||
|
||||
// TODO Some means of testing hood PIDF
|
||||
// TODO Some means of testing climber PIDF
|
||||
}
|
||||
|
||||
@@ -6,6 +6,7 @@ import java.io.FileReader;
|
||||
import java.io.IOException;
|
||||
import java.nio.file.Path;
|
||||
|
||||
import com.revrobotics.spark.ClosedLoopSlot;
|
||||
import com.revrobotics.spark.FeedbackSensor;
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
@@ -17,16 +18,17 @@ public class HoodConstants {
|
||||
// TODO Real Values
|
||||
public static final int kMotorCANID = 12;
|
||||
|
||||
public static final double kConversionFactor = 3.0*147.0/8.0;
|
||||
public static final double kConversionFactor = (1.0/3.0)*(8.0/147.0)*2*Math.PI;
|
||||
|
||||
public static final double kP = 0;
|
||||
public static final double kP = 1.75;
|
||||
public static final double kI = 0;
|
||||
public static final double kD = 0;
|
||||
public static final double kS = 0;
|
||||
public static final double kS = 0.435;
|
||||
public static final double kV = 0;
|
||||
public static final double kA = 0;
|
||||
public static final double kStartupAngle = 0;
|
||||
public static final double kMaxManualSpeedMultiplier = 1;
|
||||
public static final double kStartupAngle = 0.0;
|
||||
public static final double kMaxManualSpeedMultiplier = 0.1;
|
||||
public static final double kTolerance = Math.toRadians(0.5);
|
||||
|
||||
public static final double kAmpsToTriggerPositionReset = 10;
|
||||
|
||||
@@ -36,7 +38,7 @@ public class HoodConstants {
|
||||
|
||||
public static final int kCurrentLimit = 15;
|
||||
|
||||
public static final boolean kInverted = false;
|
||||
public static final boolean kInverted = true;
|
||||
public static final boolean kUseInterpolatorForAngle = false;
|
||||
|
||||
public static final IdleMode kIdleMode = IdleMode.kBrake;
|
||||
@@ -60,6 +62,7 @@ public class HoodConstants {
|
||||
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
|
||||
.pid(kP, kI, kD)
|
||||
.outputRange(-1, 1)
|
||||
.allowedClosedLoopError(kTolerance, ClosedLoopSlot.kSlot0)
|
||||
.feedForward
|
||||
.sva(kS, kV, kA);
|
||||
|
||||
|
||||
@@ -7,6 +7,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.signals.InvertedValue;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
import com.revrobotics.spark.ClosedLoopSlot;
|
||||
import com.revrobotics.spark.FeedbackSensor;
|
||||
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||
@@ -63,10 +64,12 @@ public class ModuleConstants {
|
||||
public static final double kTurningMotorReduction = 150.0/7.0;
|
||||
public static final double kTurningFactor = 2 * Math.PI / kTurningMotorReduction;
|
||||
// TODO Adjust? Let over from 2025
|
||||
public static final double kTurnP = 1;
|
||||
public static final double kTurnP = 12;
|
||||
public static final double kTurnI = 0;
|
||||
public static final double kTurnD = 0;
|
||||
|
||||
public static final double kTurnTolerance = Math.toRadians(0.25);
|
||||
|
||||
public static final boolean kIsEncoderInverted = false;
|
||||
|
||||
// TODO How sensitive is too sensitive?
|
||||
@@ -118,6 +121,7 @@ public class ModuleConstants {
|
||||
.feedbackSensor(FeedbackSensor.kPrimaryEncoder)
|
||||
.pid(kTurnP, kTurnI, kTurnD)
|
||||
.outputRange(-1, 1)
|
||||
.allowedClosedLoopError(kTurnTolerance, ClosedLoopSlot.kSlot0)
|
||||
.positionWrappingEnabled(true)
|
||||
.positionWrappingInputRange(0, 2 * Math.PI);
|
||||
|
||||
|
||||
@@ -2,7 +2,9 @@ package frc.robot.constants;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import frc.robot.utilities.PhotonVisionConfig;
|
||||
|
||||
public class PhotonConstants {
|
||||
@@ -11,7 +13,16 @@ public class PhotonConstants {
|
||||
|
||||
// TODO Need actual values for all of this
|
||||
public static final Transform3d kCamera1RobotToCam = new Transform3d();
|
||||
public static final Transform3d kCamera2RobotToCam = new Transform3d();
|
||||
public static final Transform3d kCamera2RobotToCam = new Transform3d(
|
||||
Units.inchesToMeters(1.5),
|
||||
Units.inchesToMeters(-10.5),
|
||||
Units.inchesToMeters(28.5),
|
||||
new Rotation3d(
|
||||
Units.degreesToRadians(0),
|
||||
Units.degreesToRadians(24),
|
||||
Units.degreesToRadians(-10)
|
||||
)
|
||||
);
|
||||
|
||||
public static final double kCamera1HeightMeters = 0;
|
||||
public static final double kCamera1PitchRadians = 0;
|
||||
@@ -21,7 +32,7 @@ public class PhotonConstants {
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
|
||||
|
||||
public static final List<PhotonVisionConfig> configs = List.of(
|
||||
new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
|
||||
//new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
|
||||
new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians)
|
||||
);
|
||||
}
|
||||
@@ -8,17 +8,23 @@ import edu.wpi.first.math.util.Units;
|
||||
|
||||
public class ShooterConstants {
|
||||
public enum ShooterSpeeds {
|
||||
kHubSpeed(0),
|
||||
kFeedSpeed(0.35);
|
||||
kHubSpeed(3000.0),
|
||||
kFeedSpeed(5000.0);
|
||||
|
||||
private double speedMPS;
|
||||
private double speedRPM;
|
||||
|
||||
private ShooterSpeeds(double speedMPS) {
|
||||
this.speedMPS = speedMPS;
|
||||
private ShooterSpeeds(double speedRPM) {
|
||||
this.speedMPS = speedRPM * kWheelDiameter*Math.PI;
|
||||
this.speedRPM = speedRPM;
|
||||
}
|
||||
|
||||
public double getSpeedMPS() {
|
||||
return speedMPS;
|
||||
return speedMPS * kWheelDiameter*Math.PI;
|
||||
}
|
||||
|
||||
public double getSpeedRPM(){
|
||||
return speedRPM;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -33,18 +39,18 @@ public class ShooterConstants {
|
||||
public static final boolean kLeftShooterMotorInverted = true;
|
||||
public static final boolean kRightShooterMotorInverted = false;
|
||||
|
||||
public static final double kLeftP = 0.1;
|
||||
public static final double kLeftP = 0.001;
|
||||
public static final double kLeftI = 0;
|
||||
public static final double kLeftD = 0;
|
||||
public static final double kLeftS = 0;
|
||||
public static final double kLeftV = 0.21;
|
||||
public static final double kLeftV = 0.0013;
|
||||
public static final double kLeftA = 0;
|
||||
|
||||
public static final double kRightP = 0.1;
|
||||
public static final double kRightP = 0.001;
|
||||
public static final double kRightI = 0;
|
||||
public static final double kRightD = 0;
|
||||
public static final double kRightD = 0.000;
|
||||
public static final double kRightS = 0;
|
||||
public static final double kRightV = 0.21;
|
||||
public static final double kRightV = 0.00121;
|
||||
public static final double kRightA = 0;
|
||||
|
||||
public static final double kMaxManualSpeedMultiplier = 1;
|
||||
@@ -52,7 +58,7 @@ public class ShooterConstants {
|
||||
public static final double kShooterHeightMeters = 0;
|
||||
|
||||
// TODO Is this value sane?
|
||||
public static final int kCurrentLimit = 50;
|
||||
public static final int kCurrentLimit = 60;
|
||||
|
||||
public static final IdleMode kShooterIdleMode = IdleMode.kCoast;
|
||||
|
||||
@@ -68,8 +74,8 @@ public class ShooterConstants {
|
||||
.smartCurrentLimit(kCurrentLimit)
|
||||
.inverted(kLeftShooterMotorInverted);
|
||||
kLeftMotorConfig.absoluteEncoder
|
||||
.positionConversionFactor(kWheelDiameter * Math.PI)
|
||||
.velocityConversionFactor(kWheelDiameter * Math.PI / 60);
|
||||
.positionConversionFactor(1)
|
||||
.velocityConversionFactor(60);
|
||||
kLeftMotorConfig.closedLoop
|
||||
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
|
||||
.pid(kLeftP, kLeftI, kLeftD)
|
||||
@@ -82,8 +88,9 @@ public class ShooterConstants {
|
||||
.smartCurrentLimit(kCurrentLimit)
|
||||
.inverted(kRightShooterMotorInverted);
|
||||
kRightMotorConfig.absoluteEncoder
|
||||
.positionConversionFactor(kWheelDiameter * Math.PI)
|
||||
.velocityConversionFactor(kWheelDiameter * Math.PI / 60);
|
||||
.positionConversionFactor(1)
|
||||
.velocityConversionFactor(60)
|
||||
.inverted(true);
|
||||
kRightMotorConfig.closedLoop
|
||||
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
|
||||
.pid(kRightP, kRightI, kRightD)
|
||||
|
||||
@@ -50,9 +50,9 @@ public class Climber extends SubsystemBase {
|
||||
}
|
||||
|
||||
public Command maintainPosition(ClimberPositions position) {
|
||||
return run(() -> {
|
||||
targetPosition = position;
|
||||
|
||||
return run(() -> {
|
||||
controller.setSetpoint(
|
||||
position.getPositionMeters(),
|
||||
ControlType.kPosition
|
||||
@@ -61,9 +61,9 @@ public class Climber extends SubsystemBase {
|
||||
}
|
||||
|
||||
public Command manualSpeed(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
targetPosition = null;
|
||||
|
||||
return run(() -> {
|
||||
motor.set(speed.getAsDouble());
|
||||
});
|
||||
}
|
||||
|
||||
@@ -323,6 +323,13 @@ public class Drivetrain extends SubsystemBase {
|
||||
});
|
||||
}
|
||||
|
||||
public Command zeroHeading() {
|
||||
return run(() -> {
|
||||
gyro.reset();
|
||||
estimator.resetRotation(new Rotation2d(0));
|
||||
});
|
||||
}
|
||||
|
||||
public void consumeVisualPose(VisualPose pose) {
|
||||
estimator.addVisionMeasurement(
|
||||
pose.visualPose(),
|
||||
|
||||
@@ -43,6 +43,7 @@ public class Hood extends SubsystemBase {
|
||||
);
|
||||
|
||||
encoder = motor.getEncoder();
|
||||
encoder.setPosition(HoodConstants.kStartupAngle);
|
||||
|
||||
controller = motor.getClosedLoopController();
|
||||
|
||||
@@ -68,9 +69,10 @@ public class Hood extends SubsystemBase {
|
||||
@Override
|
||||
public void periodic() {
|
||||
Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent());
|
||||
Logger.recordOutput("Hood/CurrentTarget", currentTargetDegrees);
|
||||
Logger.recordOutput("Hood/CurrentAngle", encoder.getPosition());
|
||||
Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees));
|
||||
Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition()));
|
||||
Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint());
|
||||
Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
|
||||
}
|
||||
|
||||
public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
|
||||
|
||||
@@ -60,9 +60,9 @@ public class IntakePivot extends SubsystemBase {
|
||||
}
|
||||
|
||||
public Command maintainPosition(IntakePivotPosition position) {
|
||||
return run(() -> {
|
||||
currentTargetPosition = position;
|
||||
|
||||
return run(() -> {
|
||||
if(currentTargetPosition == null) {
|
||||
leftMotor.disable();
|
||||
} else {
|
||||
@@ -72,9 +72,9 @@ public class IntakePivot extends SubsystemBase {
|
||||
}
|
||||
|
||||
public Command manualSpeed(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
currentTargetPosition = null;
|
||||
|
||||
return run(() -> {
|
||||
leftMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
|
||||
});
|
||||
}
|
||||
|
||||
@@ -7,6 +7,7 @@ import org.littletonrobotics.junction.Logger;
|
||||
|
||||
import com.revrobotics.AbsoluteEncoder;
|
||||
import com.revrobotics.PersistMode;
|
||||
import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.ResetMode;
|
||||
import com.revrobotics.spark.SparkClosedLoopController;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
@@ -25,6 +26,8 @@ public class Shooter extends SubsystemBase {
|
||||
private AbsoluteEncoder leftEncoder;
|
||||
private AbsoluteEncoder rightEncoder;
|
||||
|
||||
private RelativeEncoder rightRelative;
|
||||
|
||||
private SparkClosedLoopController leftClosedLoopController;
|
||||
private SparkClosedLoopController rightClosedLoopController;
|
||||
|
||||
@@ -54,17 +57,22 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
// TODO Set this to the initial startup speed
|
||||
targetSpeeds = null;
|
||||
|
||||
rightRelative = rightMotor.getEncoder();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
Logger.recordOutput(
|
||||
"Shooter/TargetMPS",
|
||||
targetSpeeds == null ? 0 : targetSpeeds.getSpeedMPS()
|
||||
"Shooter/TargetRPM",
|
||||
targetSpeeds == null ? 0 : targetSpeeds.getSpeedRPM()
|
||||
);
|
||||
|
||||
Logger.recordOutput("Shooter/LeftRollers/CurrentMPS", leftEncoder.getVelocity());
|
||||
Logger.recordOutput("Shooter/RightRollers/CurrentMPS", rightEncoder.getVelocity());
|
||||
Logger.recordOutput("Shooter/LeftRollers/CurrentRPM", leftEncoder.getVelocity());
|
||||
Logger.recordOutput("Shooter/RightRollers/CurrentRPM", rightEncoder.getVelocity());
|
||||
|
||||
Logger.recordOutput("Shooter/RightRollers/rightmotor", rightRelative.getVelocity());
|
||||
|
||||
|
||||
// TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
|
||||
Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint());
|
||||
@@ -72,20 +80,20 @@ public class Shooter extends SubsystemBase {
|
||||
}
|
||||
|
||||
public Command maintainSpeed(ShooterSpeeds speeds) {
|
||||
return run(() -> {
|
||||
targetSpeeds = speeds;
|
||||
|
||||
return run(() -> {
|
||||
if(targetSpeeds == null) {
|
||||
leftMotor.disable();
|
||||
rightMotor.disable();
|
||||
} else {
|
||||
leftClosedLoopController.setSetpoint(
|
||||
targetSpeeds.getSpeedMPS(),
|
||||
targetSpeeds.getSpeedRPM(),
|
||||
ControlType.kVelocity
|
||||
);
|
||||
|
||||
rightClosedLoopController.setSetpoint(
|
||||
-targetSpeeds.getSpeedMPS(),
|
||||
targetSpeeds.getSpeedRPM(),
|
||||
ControlType.kVelocity
|
||||
);
|
||||
}
|
||||
@@ -93,9 +101,9 @@ public class Shooter extends SubsystemBase {
|
||||
}
|
||||
|
||||
public Command manualSpeed(DoubleSupplier speed) {
|
||||
return run(() -> {
|
||||
targetSpeeds = null;
|
||||
|
||||
return run(() -> {
|
||||
leftMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
|
||||
rightMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier);
|
||||
});
|
||||
|
||||
Reference in New Issue
Block a user