mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Merge branch 'Subsystem-Boilerplate' into Intake-boilerplate
This commit is contained in:
Shikhar Dey
@@ -0,0 +1,245 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.controls.PositionDutyCycle;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Talon;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
|
||||
public class TestRobot extends SubsystemBase {
|
||||
|
||||
// TalonFX m_intakeMotor;
|
||||
// TalonFX m_armMotor;
|
||||
// TalonFX m_storageMotor;
|
||||
TalonFX m_outerShooter;
|
||||
TalonFX m_innerShooter;
|
||||
|
||||
ConfigurableDouble intakeRate = new ConfigurableDouble("Intake Rate", 0);
|
||||
|
||||
|
||||
ConfigurableDouble armUpPosition = new ConfigurableDouble("Arm Up Position", 0);
|
||||
ConfigurableDouble armDownPosition = new ConfigurableDouble("Arm Down Position", 0);
|
||||
|
||||
ConfigurableDouble storageRate = new ConfigurableDouble("Storage Rate", 0);
|
||||
ConfigurableDouble outerRate = new ConfigurableDouble("Outer Rate", 0);
|
||||
ConfigurableDouble innerRate = new ConfigurableDouble("Inner Rate", 0);
|
||||
|
||||
VelocityDutyCycle outerVelocity = new VelocityDutyCycle(0);
|
||||
VelocityDutyCycle innerVelocity = new VelocityDutyCycle(0);
|
||||
|
||||
public static final double MAX_OUTER_VELOCITY = 1200; // Rots per second
|
||||
|
||||
public enum RobotStare {
|
||||
IntakeDown,
|
||||
Idle,
|
||||
Shoot
|
||||
}
|
||||
|
||||
public RobotStare robotStare = RobotStare.Idle;
|
||||
|
||||
|
||||
public TestRobot(
|
||||
// TalonFX intakeMotor,
|
||||
// TalonFX armMotor,
|
||||
// TalonFX storageMotor,
|
||||
TalonFX outerShooter,
|
||||
TalonFX innerShooter
|
||||
) {
|
||||
// m_intakeMotor = intakeMotor;
|
||||
// m_armMotor = armMotor;
|
||||
// m_storageMotor = storageMotor;
|
||||
m_outerShooter = outerShooter;
|
||||
m_innerShooter = innerShooter;
|
||||
|
||||
// m_intakeMotor.getConfigurator().apply(INTAKE_MOTOR_CONFIG);
|
||||
// m_armMotor.getConfigurator().apply(ARM_MOTOR_CONFIG);
|
||||
// m_storageMotor.getConfigurator().apply(STORAGE_MOTOR_CONFIG);
|
||||
m_outerShooter.getConfigurator().apply(OUTER_MOTOR_CONFIG);
|
||||
m_innerShooter.getConfigurator().apply(INNER_MOTOR_CONFIG);
|
||||
|
||||
m_outerShooter.getConfigurator().apply(SHOOTER_PID);
|
||||
m_innerShooter.getConfigurator().apply(SHOOTER_PID);
|
||||
|
||||
outerVelocity.Slot = 0;
|
||||
innerVelocity.Slot = 0;
|
||||
}
|
||||
|
||||
// public static final TalonFXConfiguration INTAKE_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
// .withCurrentLimits(
|
||||
// new CurrentLimitsConfigs()
|
||||
// .withStatorCurrentLimit(40) // TODO: tune???
|
||||
// .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
|
||||
// ).withMotorOutput(
|
||||
// new MotorOutputConfigs()
|
||||
// .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
|
||||
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
// );
|
||||
// public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
// .withCurrentLimits(
|
||||
// new CurrentLimitsConfigs()
|
||||
// .withStatorCurrentLimit(40) // TODO: tune???
|
||||
// .withStatorCurrentLimitEnable(true)
|
||||
// ).withMotorOutput(
|
||||
// new MotorOutputConfigs()
|
||||
// .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
|
||||
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
// );
|
||||
|
||||
// public static final TalonFXConfiguration STORAGE_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
// .withCurrentLimits(
|
||||
// new CurrentLimitsConfigs()
|
||||
// .withStatorCurrentLimit(40) // TODO: tune???
|
||||
// .withStatorCurrentLimitEnable(true) // TODO: Figure out what this means
|
||||
// ).withMotorOutput(
|
||||
// new MotorOutputConfigs()
|
||||
// .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
|
||||
// .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
// );
|
||||
public static final TalonFXConfiguration OUTER_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
|
||||
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
);
|
||||
public static final TalonFXConfiguration INNER_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
.withStatorCurrentLimitEnable(true)
|
||||
).withMotorOutput(
|
||||
new MotorOutputConfigs()
|
||||
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
|
||||
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
|
||||
);
|
||||
|
||||
public static Slot0Configs SHOOTER_PID;
|
||||
|
||||
static {
|
||||
SHOOTER_PID = new Slot0Configs();
|
||||
SHOOTER_PID.kV = 0.12;
|
||||
// SHOOTER_PID.kP = 0.11;
|
||||
// SHOOTER_PID.kI = 0.48;
|
||||
// SHOOTER_PID.kD = 0.01;
|
||||
SHOOTER_PID.kP = 0.3;
|
||||
SHOOTER_PID.kI = 0.0;
|
||||
SHOOTER_PID.kD = 0.0;
|
||||
}
|
||||
|
||||
|
||||
ConfigurableDouble kV = new ConfigurableDouble("kV", 0.12);
|
||||
ConfigurableDouble kP = new ConfigurableDouble("kP", 0.11);
|
||||
ConfigurableDouble kI = new ConfigurableDouble("kI", 0.48);
|
||||
ConfigurableDouble kD = new ConfigurableDouble("kD", 0.01);
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
SHOOTER_PID = new Slot0Configs();
|
||||
SHOOTER_PID.kV = kV.get();
|
||||
SHOOTER_PID.kP = kP.get();
|
||||
SHOOTER_PID.kI = kI.get();
|
||||
SHOOTER_PID.kD = kD.get();
|
||||
|
||||
m_outerShooter.getConfigurator().apply(SHOOTER_PID);
|
||||
m_innerShooter.getConfigurator().apply(SHOOTER_PID);
|
||||
|
||||
|
||||
SmartDashboard.putNumber("Outer Velocity", m_outerShooter.getVelocity().getValue().in(RotationsPerSecond));
|
||||
SmartDashboard.putNumber("Inner Velocity", m_innerShooter.getVelocity().getValue().in(RotationsPerSecond));
|
||||
|
||||
|
||||
switch (robotStare) {
|
||||
case Idle:
|
||||
|
||||
// Move the arm to the up position
|
||||
PositionDutyCycle armPosition = new PositionDutyCycle(armUpPosition.get());
|
||||
// m_armMotor.setControl(armPosition);
|
||||
|
||||
|
||||
// // Stop the intake motor
|
||||
// m_intakeMotor.set(0);
|
||||
|
||||
// // Stop the storage motor
|
||||
// m_storageMotor.set(0);
|
||||
|
||||
|
||||
// Stop the outer shooter motor
|
||||
m_outerShooter.set(0);
|
||||
// Stop the inner shooter motor
|
||||
m_innerShooter.set(0);
|
||||
|
||||
break;
|
||||
case IntakeDown:
|
||||
// Move the arm to the down sposition
|
||||
PositionDutyCycle armPosition1 = new PositionDutyCycle(armDownPosition.get());
|
||||
// m_armMotor.setControl(armPosition1);
|
||||
|
||||
// Move balls into the robot because the arm is down
|
||||
VelocityDutyCycle intakeVelocity = new VelocityDutyCycle(intakeRate.get());
|
||||
// m_intakeMotor.setControl(intakeVelocity);
|
||||
|
||||
// Move balls into the main box
|
||||
VelocityDutyCycle storageVelocity = new VelocityDutyCycle(storageRate.get());
|
||||
// m_storageMotor.setControl(storageVelocity);
|
||||
|
||||
|
||||
// Stop the outer shooter motor
|
||||
m_outerShooter.set(0);
|
||||
// Stop the inner shooter motor
|
||||
m_innerShooter.set(0);
|
||||
|
||||
break;
|
||||
case Shoot:
|
||||
|
||||
// Move the arm to the up position
|
||||
PositionDutyCycle armPosition2 = new PositionDutyCycle(armUpPosition.get());
|
||||
// m_armMotor.setControl(armPosition2);
|
||||
|
||||
// // Stop the intake motor
|
||||
// m_intakeMotor.set(0);
|
||||
|
||||
// // Move the balls into the
|
||||
VelocityDutyCycle storageVelocity1 = new VelocityDutyCycle(-storageRate.get());
|
||||
// m_storageMotor.setControl(storageVelocity1);
|
||||
|
||||
// outerVelocity.
|
||||
// m_outerShooter.setControl(outerVelocity);
|
||||
double outerRPM = outerRate.get();
|
||||
m_outerShooter.setControl(outerVelocity.withVelocity(outerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY));
|
||||
|
||||
|
||||
|
||||
// m_innerShooter.setControl(innerVelocity);
|
||||
// m_innerShooter.set(innerRate.get());
|
||||
|
||||
double innerRPM = innerRate.get();
|
||||
m_innerShooter.setControl(innerVelocity.withVelocity(innerRPM));//.withFeedForward(outerRPM/MAX_OUTER_VELOCITY));
|
||||
|
||||
|
||||
// SmartDashboard.putNumber("Test", outerRate.get());
|
||||
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,9 +1,10 @@
|
||||
package frc4388.robot.subsystems.shooter;
|
||||
|
||||
import static edu.wpi.first.units.Units.*;
|
||||
import static edu.wpi.first.units.Units.Degrees;
|
||||
|
||||
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.configs.TalonFXConfiguration;
|
||||
import com.ctre.phoenix6.signals.NeutralModeValue;
|
||||
|
||||
@@ -12,10 +13,17 @@ import frc4388.utility.status.CanDevice;
|
||||
|
||||
public class ShooterConstants {
|
||||
// Motor conversions
|
||||
// public static final double ANGLE_MOTOR_GEAR_RATIO = 1.;
|
||||
public static final double PITCH_MOTOR_GEAR_RATIO = 1.;
|
||||
public static final double FLYWHEEL_GEAR_RATIO = 1.;
|
||||
|
||||
public static final double FEEDER_INCHES_PER_ROT = 1.;
|
||||
public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.;
|
||||
public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.;
|
||||
public static final double INDEXER_GEAR_RATIO = 1.;
|
||||
|
||||
public static Slot0Configs SHOOTER_PID = new Slot0Configs()
|
||||
.withKV(0.0)
|
||||
.withKP(0.0)
|
||||
.withKI(0.0)
|
||||
.withKD(0.0);
|
||||
|
||||
// Limits
|
||||
|
||||
@@ -44,7 +52,7 @@ public class ShooterConstants {
|
||||
public static final CanDevice FLYWHEEK_CAN_DEVICE = new CanDevice("Flywheel", 22);
|
||||
}
|
||||
|
||||
public static final TalonFXConfiguration PITCH_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
@@ -54,7 +62,7 @@ public class ShooterConstants {
|
||||
.withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate
|
||||
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
);
|
||||
public static final TalonFXConfiguration FLYWHEEL_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
@@ -64,7 +72,7 @@ public class ShooterConstants {
|
||||
.withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny
|
||||
.withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means
|
||||
);
|
||||
public static final TalonFXConfiguration FEEDER_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
.withStatorCurrentLimit(40) // TODO: tune???
|
||||
|
||||
@@ -24,19 +24,26 @@ public interface ShooterIO {
|
||||
// Angle shooterTargetPitch = Rotations.of(0);
|
||||
// Current pitchMotorCurrent = Amps.of(0);
|
||||
|
||||
AngularVelocity flywheelVelocity = RotationsPerSecond.of(0);
|
||||
AngularVelocity flywheelTargetVelocity = RotationsPerSecond.of(0);
|
||||
Current flywheelMotorCurrent = Amps.of(0);
|
||||
AngularVelocity motor1TargetVelocity = RotationsPerSecond.of(0);
|
||||
AngularVelocity motor2TargetVelocity = RotationsPerSecond.of(0);
|
||||
AngularVelocity indexerTargetVelocity = RotationsPerSecond.of(0);
|
||||
|
||||
LinearVelocity feederVelocity = InchesPerSecond.of(0);
|
||||
LinearVelocity feederTargetVelocity = InchesPerSecond.of(0);
|
||||
Current feederMotorCurrent = Amps.of(0);
|
||||
AngularVelocity motor1Velocity = RotationsPerSecond.of(0);
|
||||
AngularVelocity motor2Velocity = RotationsPerSecond.of(0);
|
||||
AngularVelocity indexerVelocity = RotationsPerSecond.of(0);
|
||||
|
||||
Current motor1Current = Amps.of(0);
|
||||
Current motor2Current = Amps.of(0);
|
||||
Current indexerCurrent = Amps.of(0);
|
||||
|
||||
LinearVelocity motorLinearVelocity = InchesPerSecond.of(0);
|
||||
}
|
||||
|
||||
// public default void setShooterAngle(ShooterState state, Angle angle) {}
|
||||
// public default void setShooterPitch(ShooterState state, Angle angle) {}
|
||||
public default void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {}
|
||||
public default void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {}
|
||||
public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {}
|
||||
public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {}
|
||||
public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {}
|
||||
|
||||
public default void updateInputs(ShooterState state) {}
|
||||
}
|
||||
@@ -1,38 +1,43 @@
|
||||
package frc4388.robot.subsystems.shooter;
|
||||
|
||||
import static edu.wpi.first.units.Units.InchesPerSecond;
|
||||
import static edu.wpi.first.units.Units.Rotations;
|
||||
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
||||
|
||||
import com.ctre.phoenix6.controls.PositionDutyCycle;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.units.measure.*;
|
||||
import edu.wpi.first.units.measure.Angle;
|
||||
import edu.wpi.first.units.measure.AngularVelocity;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.utility.configurable.ConfigurableDouble;
|
||||
|
||||
public class ShooterReal implements ShooterIO {
|
||||
|
||||
// TalonFX m_angleMotor;
|
||||
// TalonFX m_pitchMotor;
|
||||
TalonFX m_flywheelMotor;
|
||||
TalonFX m_feederMotor;
|
||||
TalonFX m_shooter1Motor;
|
||||
TalonFX m_shooter2Motor;
|
||||
TalonFX m_indexerMotor;
|
||||
VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0);
|
||||
VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
|
||||
VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
|
||||
|
||||
|
||||
public ShooterReal(
|
||||
// TalonFX angleMotor,
|
||||
// TalonFX pitchMotor,
|
||||
TalonFX flywheelMotor,
|
||||
TalonFX feederMotor
|
||||
TalonFX shooter1Motor,
|
||||
TalonFX shooter2Motor,
|
||||
TalonFX indexerMotor
|
||||
) {
|
||||
// m_angleMotor = angleMotor;
|
||||
// m_pitchMotor = pitchMotor;
|
||||
m_flywheelMotor = flywheelMotor;
|
||||
m_feederMotor = feederMotor;
|
||||
m_shooter1Motor= shooter1Motor;
|
||||
m_shooter2Motor= shooter2Motor;
|
||||
m_indexerMotor = indexerMotor;
|
||||
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
|
||||
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
|
||||
m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID);
|
||||
|
||||
// Apply the configs
|
||||
// m_angleMotor.getConfigurator().apply(ShooterConstants.ANGLE_MOTOR_CONFIG);
|
||||
// m_pitchMotor.getConfigurator().apply(ShooterConstants.PITCH_MOTOR_CONFIG);
|
||||
m_flywheelMotor.getConfigurator().apply(ShooterConstants.FLYWHEEL_MOTOR_CONFIG);
|
||||
m_feederMotor.getConfigurator().apply(ShooterConstants.FEEDER_MOTOR_CONFIG);
|
||||
|
||||
m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG);
|
||||
m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG);
|
||||
m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG);
|
||||
}
|
||||
|
||||
private Angle clampAng(Angle x, Angle min, Angle max){
|
||||
@@ -45,6 +50,10 @@ public class ShooterReal implements ShooterIO {
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// // TODO: Test
|
||||
// @Override
|
||||
// public void setShooterAngle(ShooterState state, Angle angle) {
|
||||
@@ -75,37 +84,36 @@ public class ShooterReal implements ShooterIO {
|
||||
// m_pitchMotor.setControl(posRequest);
|
||||
// }
|
||||
|
||||
|
||||
@Override
|
||||
public void setFlywheelVelocity(ShooterState state, AngularVelocity angularVelocity) {
|
||||
state.flywheelTargetVelocity = angularVelocity;
|
||||
// (REAL_ROT / SEC) * (MOTOR_ROT / REAL_ROT) = (MOTOR_ROT / SEC)
|
||||
double motorSpeed = angularVelocity.in(RotationsPerSecond) / ShooterConstants.FLYWHEEL_GEAR_RATIO;
|
||||
VelocityDutyCycle velocity = new VelocityDutyCycle(motorSpeed);
|
||||
m_feederMotor.setControl(velocity);
|
||||
public void setMotor1Velocity(ShooterState state, AngularVelocity target) {
|
||||
state.motor1TargetVelocity = target;
|
||||
state.motor2TargetVelocity = target;
|
||||
|
||||
double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
|
||||
m_shooter1Motor.setControl(new VelocityDutyCycle(motorRps));
|
||||
m_shooter2Motor.setControl(new VelocityDutyCycle(motorRps));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setFeederVelocity(ShooterState state, LinearVelocity linearVelocity) {
|
||||
state.feederTargetVelocity = linearVelocity;
|
||||
// (IN / SEC) * (ROT / IN) = (ROT / SEC)
|
||||
double motorSpeed = linearVelocity.in(InchesPerSecond) / ShooterConstants.FEEDER_INCHES_PER_ROT;
|
||||
VelocityDutyCycle velRequest = new VelocityDutyCycle(motorSpeed);
|
||||
m_feederMotor.setControl(velRequest);
|
||||
public void setIndexerVelocity(ShooterState state, AngularVelocity target) {
|
||||
state.indexerTargetVelocity = target;
|
||||
double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO;
|
||||
m_indexerMotor.setControl(new VelocityDutyCycle(motorRps));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void updateInputs(ShooterState state) {
|
||||
// state.shooterAngle = m_angleMotor.getPosition().getValue().times(ShooterConstants.ANGLE_MOTOR_GEAR_RATIO);
|
||||
// state.angleMotorCurrent = m_angleMotor.getStatorCurrent(false).getValue();
|
||||
|
||||
// state.shooterPitch = m_pitchMotor.getPosition().getValue().times(ShooterConstants.PITCH_MOTOR_GEAR_RATIO);
|
||||
// state.pitchMotorCurrent = m_pitchMotor.getStatorCurrent().getValue();
|
||||
state.motor1Velocity = m_shooter1Motor.getVelocity().getValue();
|
||||
state.motor2Velocity = m_shooter2Motor.getVelocity().getValue();
|
||||
state.indexerVelocity = m_indexerMotor.getVelocity().getValue();
|
||||
|
||||
state.flywheelVelocity = m_flywheelMotor.getVelocity().getValue();
|
||||
state.flywheelMotorCurrent = m_flywheelMotor.getStatorCurrent().getValue();
|
||||
state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
|
||||
|
||||
state.feederVelocity = InchesPerSecond.of(m_feederMotor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT);
|
||||
state.feederMotorCurrent = m_feederMotor.getStatorCurrent().getValue();
|
||||
state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue();
|
||||
state.motor2Current = m_shooter2Motor.getStatorCurrent().getValue();
|
||||
state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user