mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 00:38:03 -06:00
Delete TestRobot.java
This commit is contained in:
Michael Mikovsky
@@ -1,244 +0,0 @@
|
|||||||
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.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;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
Reference in New Issue
Block a user