import org.littletonrobotics.junction.Logger;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.constants.Constants;
import frc.robot.util.Elastic;
}
hubActive = current;
+
+ // keep this
+ SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST");
}
@Override
*
* @return inches
*/
- double getPosition();
+ public double getPosition();
/**
* Set the intake extender position
*
* @param setpoint in inches
*/
- void setPosition(double setpoint);
+ public void setPosition(double setpoint);
- void setLeftMotor(double speed);
+ public void setLeftMotor(double speed);
- void setRightMotor(double speed);
+ public void setRightMotor(double speed);
- void setRollerMotor(double speed);
+ public void setRollerMotor(double speed);
- void setLimits(CurrentLimitsConfigs limits);
+ public void setLimits(CurrentLimitsConfigs limits);
- void setRawPosition(double pos);
+ public void setRawPosition(double pos);
/**
* Reclaim all the resources (e.g., motors and other devices).
* This step is necessary for multiple unit tests to work.
*/
- void close();
+ public void close();
}
--- /dev/null
+package frc.robot.subsystems.Intake;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+import frc.robot.constants.IntakeConstants;
+
+public class IntakeIOTalonFX implements IntakeIO {
+
+ // create the motors
+ /** Motor to move the roller */
+ private TalonFX rollerMotor = new TalonFX(IdConstants.ROLLER_MOTOR_ID, Constants.CANIVORE_SUB);
+ /** Right motor (master) */
+ private TalonFX rightMotor = new TalonFX(IdConstants.RIGHT_MOTOR_ID, Constants.CANIVORE_SUB);
+ /** Left motor (slave) */
+ private TalonFX leftMotor = new TalonFX(IdConstants.LEFT_MOTOR_ID, Constants.CANIVORE_SUB);
+
+ private final MotionMagicVoltage voltageRequest = new MotionMagicVoltage(0);
+
+ /**
+ * Motor characteristics for the roller motor, a single Kraken X44 (aka gearbox)
+ */
+ private final DCMotor dcMotorRoller = DCMotor.getKrakenX44(1);
+ /**
+ * Motor characteristics for the extending pair of Kraken X44 motors (aka
+ * gearbox)
+ */
+ private final DCMotor dcMotorExtend = DCMotor.getKrakenX44(2);
+
+ public IntakeIOTalonFX() {
+
+ // get the maximum free speed
+ double maxFreeSpeed = Units.radiansToRotations(dcMotorExtend.freeSpeedRadPerSec) / IntakeConstants.GEAR_RATIO;
+
+ // max free speed (rot/s) = motor free speed (rad/s to rot/s)/ gear ratio
+ // safety margin, limits velocity to .75 free speed
+ double maxVelocity = 0.75 * maxFreeSpeed;
+ double maxAcceleration = maxVelocity / 0.25;
+
+ // Configure the motors
+ // Build the configuration for the roller
+ TalonFXConfiguration rollerConfig = new TalonFXConfiguration();
+
+ // config Slot 0 PID params
+ var slot0Configs = rollerConfig.Slot0;
+ slot0Configs.kP = 5.0;
+ slot0Configs.kI = 0.0;
+ slot0Configs.kD = 0.0;
+ slot0Configs.kV = 0.0;
+ slot0Configs.kA = 0.0;
+
+ // set the brake mode
+ rollerConfig.MotorOutput.withNeutralMode(NeutralModeValue.Brake);
+
+ // apply the configuration to the right motor (master)
+ rollerMotor.getConfigurator().apply(rollerConfig);
+
+ // Build the configuration for the left and right Motor
+ TalonFXConfiguration config = new TalonFXConfiguration();
+
+ // config the current limits (low value for testing)
+ config.CurrentLimits
+ .withStatorCurrentLimit(IntakeConstants.EXTENDER_CURRENT_LIMITS)
+ .withStatorCurrentLimitEnable(true)
+ .withSupplyCurrentLimit(IntakeConstants.EXTENDER_CURRENT_LIMITS)
+ .withSupplyCurrentLimitEnable(true);
+
+ // config Slot 0 PID params
+ var rollerSlot0Configs = config.Slot0;
+ rollerSlot0Configs.kP = 5.0;
+ rollerSlot0Configs.kI = 0.0;
+ rollerSlot0Configs.kD = 0.0;
+ rollerSlot0Configs.kV = 0.0;
+ rollerSlot0Configs.kA = 0.0;
+
+ // configure MotionMagic
+ MotionMagicConfigs motionMagicConfigs = config.MotionMagic;
+
+ motionMagicConfigs.MotionMagicCruiseVelocity = IntakeConstants.GEAR_RATIO * maxVelocity
+ / IntakeConstants.RADIUS_RACK_PINION / Math.PI / 2;
+ motionMagicConfigs.MotionMagicAcceleration = IntakeConstants.GEAR_RATIO * maxAcceleration
+ / IntakeConstants.RADIUS_RACK_PINION / Math.PI / 2;
+
+ rightMotor.getConfigurator().apply(config);
+ leftMotor.getConfigurator().apply(config);
+
+ leftMotor.getConfigurator().apply(
+ new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
+ .withNeutralMode(NeutralModeValue.Coast));
+
+ rightMotor.getConfigurator().apply(
+ new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast));
+
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = IntakeConstants.NORMAL_CURRENT_LIMIT;
+ limitConfig.StatorCurrentLimitEnable = true;
+ leftMotor.getConfigurator().apply(limitConfig);
+ rightMotor.getConfigurator().apply(limitConfig);
+
+ leftMotor.setPosition(0.0);
+ rightMotor.setPosition(0.0);
+ }
+
+ @Override
+ public void updateInputs(IntakeIOInputs inputs) {
+ inputs.leftPosition = Intake.rotationsToInches(leftMotor.getPosition().getValueAsDouble());
+ inputs.rightPosition = Intake.rotationsToInches(rightMotor.getPosition().getValueAsDouble());
+ inputs.leftCurrent = leftMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rightCurrent = rightMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rollerVelocity = rollerMotor.getVelocity().getValueAsDouble();
+ inputs.rollerCurrent = rollerMotor.getStatorCurrent().getValueAsDouble();
+ inputs.rightVoltage = rightMotor.getMotorVoltage().getValueAsDouble();
+ inputs.leftVoltage = leftMotor.getMotorVoltage().getValueAsDouble();
+ inputs.rollerSetSpeed = rollerMotor.get();
+ }
+
+ /**
+ * Set the intake extender position
+ *
+ * @param setpoint in inches
+ */
+ @Override
+ public void setPosition(double setpoint) {
+ double motorRotations = -Intake.inchesToRotations(setpoint);
+ rightMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true));
+ leftMotor.setControl(voltageRequest.withPosition(motorRotations).withEnableFOC(true));
+ }
+
+ /**
+ * Get the intake extender position
+ *
+ * @return inches
+ */
+ @Override
+ public double getPosition() {
+ return Intake.rotationsToInches(leftMotor.getPosition().getValueAsDouble());
+ }
+
+ @Override
+ public void setLeftMotor(double speed) {
+ leftMotor.set(speed);
+ }
+
+ @Override
+ public void setRightMotor(double speed) {
+ rightMotor.set(speed);
+ }
+
+ @Override
+ public void setRollerMotor(double speed) {
+ rollerMotor.set(speed);
+ }
+
+ @Override
+ public void setLimits(CurrentLimitsConfigs limits) {
+ leftMotor.getConfigurator().apply(limits);
+ rightMotor.getConfigurator().apply(limits);
+ }
+
+ @Override
+ public void setRawPosition(double pos) {
+ leftMotor.setPosition(pos);
+ rightMotor.setPosition(pos);
+ }
+
+
+ @Override
+ public void close() {
+ leftMotor.close();
+ rightMotor.close();
+ rollerMotor.close();
+ }
+
+}
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
-import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.VelocityVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.Constants;
-import frc.robot.constants.IdConstants;
-import frc.robot.util.HubActive;
-public class Shooter extends SubsystemBase implements ShooterIO {
-
- private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.CANIVORE_SUB);
- private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.CANIVORE_SUB);
+public class Shooter extends SubsystemBase {
+ private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
- //TODO Add current limits
+ LoggedNetworkNumber powerModifier = new LoggedNetworkNumber("OPERATOR: Shooter Power Modifier", 1.05);
+ ShooterIO io;
+
// Goal Velocity / Double theCircumfrence
private double shooterTargetSpeed = 0;
- // Velocity in rotations per second
- VelocityVoltage voltageRequest = new VelocityVoltage(0);
-
- private final ShooterIOInputsAutoLogged inputs = new ShooterIOInputsAutoLogged();
-
-
- LoggedNetworkNumber powerModifier = new LoggedNetworkNumber("OPERATOR: Shooter Power Modifier", 1.05);
-
- public Shooter() {
- updateInputs();
-
- TalonFXConfiguration config = new TalonFXConfiguration();
- config.Slot0.kP = 0.5; // 0.5 stable
- config.Slot0.kI = 0;
- config.Slot0.kD = 0.0;
- config.Slot0.kV = 0.125; //Maximum rps = 100 --> 12V/100rps
-
- config.CurrentLimits
- .withSupplyCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT)
- .withSupplyCurrentLimitEnable(true);
-
- shooterMotorLeft.getConfigurator().apply(config);
- shooterMotorRight.getConfigurator().apply(config);
-
- shooterMotorLeft.getConfigurator().apply(
- new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
- .withNeutralMode(NeutralModeValue.Coast)
- );
-
- shooterMotorRight.getConfigurator().apply(
- new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast)
- );
-
- CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
- limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_CURRENT_LIMIT;
- limitConfig.StatorCurrentLimitEnable = true;
- shooterMotorLeft.getConfigurator().apply(limitConfig);
- shooterMotorRight.getConfigurator().apply(limitConfig);
-
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putData("Turn on shooter", new InstantCommand(()-> setShooter(12.0)));
- }
+ public Shooter(ShooterIO io) {
+ this.io = io;
+ io.updateInputs(inputs);
}
@Override
public void periodic(){
- updateInputs();
+ io.updateInputs(inputs);
+ Logger.processInputs("Shooter", inputs);
// shooterTargetSpeed = SmartDashboard.getNumber("Shooter Setpoint", shooterTargetSpeed);
// SmartDashboard.putNumber("Shooter Setpoint", shooterTargetSpeed);
// Convert to RPS
double targetVelocityRPS = Units.radiansToRotations(shooterTargetSpeed / (ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2)) * powerModifier.get();
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putNumber("Target Velocity RPS", targetVelocityRPS);
- SmartDashboard.putNumber("Shooter Motor RPS", shooterMotorLeft.getVelocity().getValueAsDouble());
- }
// Sets the motor control to target velocity
- shooterMotorLeft.setControl(voltageRequest.withVelocity(targetVelocityRPS).withEnableFOC(true));
- shooterMotorRight.setControl(voltageRequest.withVelocity(targetVelocityRPS).withEnableFOC(true));
+ io.setTargetVelocityRps(targetVelocityRPS);
if (!Constants.DISABLE_LOGGING) {
- Logger.recordOutput("Shooter/realVelocity", shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER);
+ Logger.recordOutput("Shooter/realVelocity", inputs.shooterSpeedLeft);
Logger.recordOutput("Shooter/targetVelocity", shooterTargetSpeed);
}
-
- double actualWheelVelocity = shooterMotorLeft.getVelocity().getValueAsDouble() * ShooterConstants.SHOOTER_LAUNCH_DIAMETER;
-
- if (!Constants.DISABLE_SMART_DASHBOARD) {
- SmartDashboard.putNumber("Shooter Speed Error (mps)", shooterTargetSpeed - actualWheelVelocity);
- SmartDashboard.putBoolean("Shooter At Speed", atTargetSpeed());
- SmartDashboard.putBoolean("Shooter Running", shooterTargetSpeed > 0);
- }
-
- // keep this
- SmartDashboard.putString("WON AUTO?", (HubActive.wonAuto()) ? "WON" : "LOST");
}
/**
return inputs.shooterSpeedLeft;
}
- public void setNewCurrentLimit(double newCurrentLimit) {
- CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
- limitConfig.StatorCurrentLimit = newCurrentLimit;
- limitConfig.StatorCurrentLimitEnable = true;
- shooterMotorLeft.getConfigurator().apply(limitConfig);
- shooterMotorRight.getConfigurator().apply(limitConfig);
- }
-
- @Override
- public void updateInputs(){
- inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
- inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble())* ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2;
- inputs.shooterCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble();
- inputs.shooterCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble();
-
-
- Logger.processInputs("Shooter", inputs);
- }
-
public void bumpUpShooterModifier() {
powerModifier.set(powerModifier.get() + 0.025);
}
return Math.abs(getShooterVelocity() - shooterTargetSpeed) < 1.0;
}
+ public void setNewCurrentLimits(double limit) {
+ io.setNewCurrentLimit(limit);
+ }
+
/**
* @return Gets the target velocity in m/s
*/
import org.littletonrobotics.junction.AutoLog;
public interface ShooterIO {
- @AutoLog
- public static class ShooterIOInputs {
- public double shooterSpeedLeft = 0.0;
- public double shooterSpeedRight = 0.0;
- public double shooterCurrentLeft = 0.0;
- public double shooterCurrentRight = 0.0;
- }
-
- public void updateInputs();
+ @AutoLog
+ public static class ShooterIOInputs {
+ public double shooterSpeedLeft = 0.0;
+ public double shooterSpeedRight = 0.0;
+ public double shooterCurrentLeft = 0.0;
+ public double shooterCurrentRight = 0.0;
+ }
+
+ public void updateInputs(ShooterIOInputs inputs);
+
+ public void setNewCurrentLimit(double newCurrentLimit);
+
+ public void setTargetVelocityRps(double target);
}
--- /dev/null
+package frc.robot.subsystems.shooter;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.util.Units;
+import frc.robot.constants.Constants;
+import frc.robot.constants.IdConstants;
+
+public class ShooterIOTalonFX implements ShooterIO {
+
+ private TalonFX shooterMotorLeft = new TalonFX(IdConstants.SHOOTER_LEFT_ID, Constants.CANIVORE_SUB);
+ private TalonFX shooterMotorRight = new TalonFX(IdConstants.SHOOTER_RIGHT_ID, Constants.CANIVORE_SUB);
+
+ // TODO Add current limits
+
+ // Velocity in rotations per second
+ VelocityVoltage voltageRequest = new VelocityVoltage(0);
+
+ public ShooterIOTalonFX() {
+ TalonFXConfiguration config = new TalonFXConfiguration();
+ config.Slot0.kP = 0.5; // 0.5 stable
+ config.Slot0.kI = 0;
+ config.Slot0.kD = 0.0;
+ config.Slot0.kV = 0.125; // Maximum rps = 100 --> 12V/100rps
+
+ config.CurrentLimits
+ .withSupplyCurrentLimit(ShooterConstants.SHOOTER_CURRENT_LIMIT)
+ .withSupplyCurrentLimitEnable(true);
+
+ shooterMotorLeft.getConfigurator().apply(config);
+ shooterMotorRight.getConfigurator().apply(config);
+
+ shooterMotorLeft.getConfigurator().apply(
+ new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive)
+ .withNeutralMode(NeutralModeValue.Coast));
+
+ shooterMotorRight.getConfigurator().apply(
+ new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Coast));
+
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = ShooterConstants.SHOOTER_CURRENT_LIMIT;
+ limitConfig.StatorCurrentLimitEnable = true;
+ shooterMotorLeft.getConfigurator().apply(limitConfig);
+ shooterMotorRight.getConfigurator().apply(limitConfig);
+ }
+
+ @Override
+ public void updateInputs(ShooterIOInputs inputs) {
+ inputs.shooterSpeedLeft = Units.rotationsToRadians(shooterMotorLeft.getVelocity().getValueAsDouble())
+ * ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2;
+ inputs.shooterSpeedRight = Units.rotationsToRadians(shooterMotorRight.getVelocity().getValueAsDouble())
+ * ShooterConstants.SHOOTER_LAUNCH_DIAMETER / 2;
+ inputs.shooterCurrentLeft = shooterMotorLeft.getStatorCurrent().getValueAsDouble();
+ inputs.shooterCurrentRight = shooterMotorRight.getStatorCurrent().getValueAsDouble();
+
+ }
+
+ @Override
+ public void setNewCurrentLimit(double newCurrentLimit) {
+ CurrentLimitsConfigs limitConfig = new CurrentLimitsConfigs();
+ limitConfig.StatorCurrentLimit = newCurrentLimit;
+ limitConfig.StatorCurrentLimitEnable = true;
+ shooterMotorLeft.getConfigurator().apply(limitConfig);
+ shooterMotorRight.getConfigurator().apply(limitConfig);
+ }
+
+ @Override
+ public void setTargetVelocityRps(double target) {
+ shooterMotorLeft.setControl(voltageRequest.withVelocity(target).withEnableFOC(true));
+ shooterMotorRight.setControl(voltageRequest.withVelocity(target).withEnableFOC(true));
+ }
+}