Adding preliminary shooter subsystem
This commit is contained in:
@@ -1,6 +1,5 @@
|
|||||||
package frc.robot.constants;
|
package frc.robot.constants;
|
||||||
|
|
||||||
import edu.wpi.first.math.MathUtil;
|
|
||||||
import edu.wpi.first.math.geometry.Translation2d;
|
import edu.wpi.first.math.geometry.Translation2d;
|
||||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||||
import edu.wpi.first.math.util.Units;
|
import edu.wpi.first.math.util.Units;
|
||||||
|
|||||||
112
src/main/java/frc/robot/constants/ShooterConstants.java
Normal file
112
src/main/java/frc/robot/constants/ShooterConstants.java
Normal file
@@ -0,0 +1,112 @@
|
|||||||
|
package frc.robot.constants;
|
||||||
|
|
||||||
|
import com.revrobotics.spark.FeedbackSensor;
|
||||||
|
import com.revrobotics.spark.config.SparkMaxConfig;
|
||||||
|
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
|
||||||
|
|
||||||
|
import edu.wpi.first.math.util.Units;
|
||||||
|
|
||||||
|
public class ShooterConstants {
|
||||||
|
public enum ShooterSpeeds {
|
||||||
|
kHubSpeed(0, 0),
|
||||||
|
kFeedSpeed(0, 0);
|
||||||
|
|
||||||
|
private double frontRollerMPS;
|
||||||
|
private double rearRollerMPS;
|
||||||
|
|
||||||
|
private ShooterSpeeds(double frontRollerMPS, double rearRollerMPS) {
|
||||||
|
this.frontRollerMPS = frontRollerMPS;
|
||||||
|
this.rearRollerMPS = rearRollerMPS;
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getFrontRollerMPS() {
|
||||||
|
return frontRollerMPS;
|
||||||
|
}
|
||||||
|
|
||||||
|
public double getRearRollerMPS() {
|
||||||
|
return rearRollerMPS;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public static final double kWheelDiameter = Units.inchesToMeters(6);
|
||||||
|
|
||||||
|
// TODO Real values
|
||||||
|
public static final int kFrontShooterMotor1CANID = 0;
|
||||||
|
public static final int kFrontShooterMotor2CANID = 0;
|
||||||
|
public static final int kRearShooterMotor1CANID = 0;
|
||||||
|
public static final int kRearShooterMotor2CANID = 0;
|
||||||
|
|
||||||
|
public static final boolean kFrontShooterMotor1Inverted = false;
|
||||||
|
public static final boolean kFrontShooterMotor2Inverted = false;
|
||||||
|
public static final boolean kRearShooterMotor1Inverted = false;
|
||||||
|
public static final boolean kRearShooterMotor2Inverted = false;
|
||||||
|
|
||||||
|
public static final double kFrontP = 0;
|
||||||
|
public static final double kFrontI = 0;
|
||||||
|
public static final double kFrontD = 0;
|
||||||
|
public static final double kFrontS = 0;
|
||||||
|
public static final double kFrontV = 0;
|
||||||
|
public static final double kFrontA = 0;
|
||||||
|
|
||||||
|
public static final double kRearP = 0;
|
||||||
|
public static final double kRearI = 0;
|
||||||
|
public static final double kRearD = 0;
|
||||||
|
public static final double kRearS = 0;
|
||||||
|
public static final double kRearV = 0;
|
||||||
|
public static final double kRearA = 0;
|
||||||
|
|
||||||
|
// TODO Is this value sane?
|
||||||
|
public static final int kCurrentLimit = 30;
|
||||||
|
|
||||||
|
public static final IdleMode kShooterIdleMode = IdleMode.kCoast;
|
||||||
|
|
||||||
|
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
|
||||||
|
|
||||||
|
public static final SparkMaxConfig kFrontMotor1Config = new SparkMaxConfig();
|
||||||
|
public static final SparkMaxConfig kFrontMotor2Config = new SparkMaxConfig();
|
||||||
|
public static final SparkMaxConfig kRearMotor1Config = new SparkMaxConfig();
|
||||||
|
public static final SparkMaxConfig kRearMotor2Config = new SparkMaxConfig();
|
||||||
|
|
||||||
|
|
||||||
|
static {
|
||||||
|
kFrontMotor1Config
|
||||||
|
.idleMode(kShooterIdleMode)
|
||||||
|
.smartCurrentLimit(kCurrentLimit)
|
||||||
|
.inverted(kFrontShooterMotor1Inverted);
|
||||||
|
kFrontMotor1Config.absoluteEncoder
|
||||||
|
.positionConversionFactor(kWheelDiameter * Math.PI)
|
||||||
|
.velocityConversionFactor(kWheelDiameter * Math.PI / 60);
|
||||||
|
kFrontMotor1Config.closedLoop
|
||||||
|
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
|
||||||
|
.pid(kFrontP, kFrontI, kFrontD)
|
||||||
|
.outputRange(-1, 1)
|
||||||
|
.feedForward
|
||||||
|
.sva(kFrontS, kFrontV, kFrontA);
|
||||||
|
|
||||||
|
kFrontMotor2Config
|
||||||
|
.idleMode(kShooterIdleMode)
|
||||||
|
.smartCurrentLimit(kCurrentLimit)
|
||||||
|
.inverted(kFrontShooterMotor2Inverted)
|
||||||
|
.follow(kFrontShooterMotor1CANID);
|
||||||
|
|
||||||
|
kRearMotor1Config
|
||||||
|
.idleMode(kShooterIdleMode)
|
||||||
|
.smartCurrentLimit(kCurrentLimit)
|
||||||
|
.inverted(kRearShooterMotor1Inverted);
|
||||||
|
kRearMotor1Config.absoluteEncoder
|
||||||
|
.positionConversionFactor(kWheelDiameter * Math.PI)
|
||||||
|
.velocityConversionFactor(kWheelDiameter * Math.PI / 60);
|
||||||
|
kRearMotor1Config.closedLoop
|
||||||
|
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
|
||||||
|
.pid(kRearP, kRearI, kRearD)
|
||||||
|
.outputRange(-1, 1)
|
||||||
|
.feedForward
|
||||||
|
.sva(kRearS, kRearV, kRearA);
|
||||||
|
|
||||||
|
kRearMotor2Config
|
||||||
|
.idleMode(kShooterIdleMode)
|
||||||
|
.smartCurrentLimit(kCurrentLimit)
|
||||||
|
.inverted(kRearShooterMotor2Inverted)
|
||||||
|
.follow(kRearShooterMotor1CANID);
|
||||||
|
}
|
||||||
|
}
|
||||||
135
src/main/java/frc/robot/subsystems/Shooter.java
Normal file
135
src/main/java/frc/robot/subsystems/Shooter.java
Normal file
@@ -0,0 +1,135 @@
|
|||||||
|
package frc.robot.subsystems;
|
||||||
|
|
||||||
|
import java.util.Optional;
|
||||||
|
|
||||||
|
import org.littletonrobotics.junction.Logger;
|
||||||
|
|
||||||
|
import com.revrobotics.AbsoluteEncoder;
|
||||||
|
import com.revrobotics.PersistMode;
|
||||||
|
import com.revrobotics.ResetMode;
|
||||||
|
import com.revrobotics.spark.SparkClosedLoopController;
|
||||||
|
import com.revrobotics.spark.SparkMax;
|
||||||
|
import com.revrobotics.spark.SparkBase.ControlType;
|
||||||
|
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||||
|
|
||||||
|
import edu.wpi.first.wpilibj2.command.Command;
|
||||||
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
|
import frc.robot.constants.ShooterConstants;
|
||||||
|
import frc.robot.constants.ShooterConstants.ShooterSpeeds;
|
||||||
|
|
||||||
|
public class Shooter extends SubsystemBase {
|
||||||
|
private SparkMax frontMotor1;
|
||||||
|
private SparkMax frontMotor2;
|
||||||
|
private SparkMax rearMotor1;
|
||||||
|
private SparkMax rearMotor2;
|
||||||
|
|
||||||
|
private AbsoluteEncoder frontEncoder;
|
||||||
|
private AbsoluteEncoder rearEncoder;
|
||||||
|
|
||||||
|
private SparkClosedLoopController frontClosedLoopController;
|
||||||
|
private SparkClosedLoopController rearClosedLoopController;
|
||||||
|
|
||||||
|
private ShooterSpeeds currentSpeeds;
|
||||||
|
|
||||||
|
public Shooter() {
|
||||||
|
frontMotor1 = new SparkMax(ShooterConstants.kFrontShooterMotor1CANID, MotorType.kBrushless);
|
||||||
|
frontMotor2 = new SparkMax(ShooterConstants.kFrontShooterMotor2CANID, MotorType.kBrushless);
|
||||||
|
rearMotor1 = new SparkMax(ShooterConstants.kRearShooterMotor1CANID, MotorType.kBrushless);
|
||||||
|
rearMotor2 = new SparkMax(ShooterConstants.kRearShooterMotor2CANID, MotorType.kBrushless);
|
||||||
|
|
||||||
|
frontMotor1.configure(
|
||||||
|
ShooterConstants.kFrontMotor1Config,
|
||||||
|
ResetMode.kResetSafeParameters,
|
||||||
|
PersistMode.kPersistParameters
|
||||||
|
);
|
||||||
|
|
||||||
|
rearMotor1.configure(
|
||||||
|
ShooterConstants.kRearMotor1Config,
|
||||||
|
ResetMode.kResetSafeParameters,
|
||||||
|
PersistMode.kPersistParameters
|
||||||
|
);
|
||||||
|
|
||||||
|
frontMotor2.configure(
|
||||||
|
ShooterConstants.kFrontMotor2Config,
|
||||||
|
ResetMode.kResetSafeParameters,
|
||||||
|
PersistMode.kPersistParameters
|
||||||
|
);
|
||||||
|
|
||||||
|
rearMotor2.configure(
|
||||||
|
ShooterConstants.kRearMotor2Config,
|
||||||
|
ResetMode.kResetSafeParameters,
|
||||||
|
PersistMode.kPersistParameters
|
||||||
|
);
|
||||||
|
|
||||||
|
frontEncoder = frontMotor1.getAbsoluteEncoder();
|
||||||
|
rearEncoder = rearMotor1.getAbsoluteEncoder();
|
||||||
|
|
||||||
|
frontClosedLoopController = frontMotor1.getClosedLoopController();
|
||||||
|
rearClosedLoopController = rearMotor1.getClosedLoopController();
|
||||||
|
|
||||||
|
// TODO Set this to the initial startup speed
|
||||||
|
currentSpeeds = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
@Override
|
||||||
|
public void periodic() {
|
||||||
|
if(currentSpeeds == null) {
|
||||||
|
if(frontClosedLoopController.getSetpoint() != 0) {
|
||||||
|
frontClosedLoopController.setSetpoint(
|
||||||
|
0,
|
||||||
|
ControlType.kVelocity
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(rearClosedLoopController.getSetpoint() != 0) {
|
||||||
|
rearClosedLoopController.setSetpoint(
|
||||||
|
0,
|
||||||
|
ControlType.kVelocity
|
||||||
|
);
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
frontClosedLoopController.setSetpoint(
|
||||||
|
currentSpeeds.getFrontRollerMPS(),
|
||||||
|
ControlType.kVelocity
|
||||||
|
);
|
||||||
|
|
||||||
|
rearClosedLoopController.setSetpoint(
|
||||||
|
currentSpeeds.getRearRollerMPS(),
|
||||||
|
ControlType.kVelocity
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
Logger.recordOutput(
|
||||||
|
"Shooter/FrontRollers/TargetMPS",
|
||||||
|
currentSpeeds == null ? 0 : currentSpeeds.getFrontRollerMPS()
|
||||||
|
);
|
||||||
|
|
||||||
|
Logger.recordOutput(
|
||||||
|
"Shooter/RearRollers/TargetMPS",
|
||||||
|
currentSpeeds == null ? 0 : currentSpeeds.getRearRollerMPS()
|
||||||
|
);
|
||||||
|
|
||||||
|
Logger.recordOutput("Shooter/FrontRollers/CurrentMPS", frontEncoder.getVelocity());
|
||||||
|
Logger.recordOutput("Shooter/RearRollers/CurrentMPS", rearEncoder.getVelocity());
|
||||||
|
|
||||||
|
// TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
|
||||||
|
Logger.recordOutput("Shooter/FrontRollers/AtSetpoint", frontClosedLoopController.isAtSetpoint());
|
||||||
|
Logger.recordOutput("Shooter/RearRollers/AtSetpoint", rearClosedLoopController.isAtSetpoint());
|
||||||
|
}
|
||||||
|
|
||||||
|
public Command setCurrentSpeeds(ShooterSpeeds speeds) {
|
||||||
|
return runOnce(() -> {
|
||||||
|
currentSpeeds = speeds;
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
public Command stop() {
|
||||||
|
return runOnce(() -> {
|
||||||
|
currentSpeeds = null;
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
public Optional<ShooterSpeeds> getCurrentSpeeds() {
|
||||||
|
return Optional.ofNullable(currentSpeeds);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -12,7 +12,6 @@ import com.revrobotics.spark.SparkMax;
|
|||||||
import com.revrobotics.spark.SparkBase.ControlType;
|
import com.revrobotics.spark.SparkBase.ControlType;
|
||||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||||
|
|
||||||
import edu.wpi.first.math.MathUtil;
|
|
||||||
import edu.wpi.first.math.geometry.Rotation2d;
|
import edu.wpi.first.math.geometry.Rotation2d;
|
||||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||||
|
|||||||
Reference in New Issue
Block a user