Skip to content
Open
31 changes: 26 additions & 5 deletions src/main/java/frc/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -183,6 +183,10 @@ public void placeGamePiecesOnField() {}
SimulatedArena.overrideInstance(new EvergreenArena());
}

Indexer indexer = null;
Intake intake = null;
Shooter shooter = null;

// this is here because it doesn't like that the power distribution logger is never closed
@SuppressWarnings("resource")
public Robot() {
Expand Down Expand Up @@ -235,11 +239,11 @@ public Robot() {
shooter =
new ShooterSubsystem(
ROBOT_MODE == RobotMode.REAL
? new HoodIO(HoodIO.getHoodConfiguration(), canivore)
: new HoodIOSim(canivore),
? new HoodIO(HoodIO.getHoodAlphaConfiguration(), canivore)
: new HoodIOSim(canivore,HoodIO.getHoodAlphaConfiguration(),ShooterSubsystem.HOOD_GEAR_RATIO_A),
ROBOT_MODE == RobotMode.REAL
? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore)
: new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore));
? new FlywheelIO(FlywheelIO.getFlywheelAlphaConfiguration(), canivore)
: new FlywheelIOSim(FlywheelIO.getFlywheelAlphaConfiguration(), canivore, ShooterSubsystem.FLYWHEEL_GEAR_RATIO));

intake =
new FintakeSubsystem(
Expand All @@ -259,7 +263,15 @@ public Robot() {
case COMP:
indexer = new SpindexerSubsystem();
intake = new LintakeSubsystem();
shooter = new TurretSubsystem();
shooter =
new TurretSubsystem(
ROBOT_MODE == RobotMode.REAL
? new FlywheelIO(FlywheelIO.getFlywheelCompConfiguration(), canivore)
: new FlywheelIOSim(FlywheelIO.getFlywheelCompConfiguration(), canivore, TurretSubsystem.FLYWHEEL_GEAR_RATIO_C),
ROBOT_MODE == RobotMode.REAL
? new HoodIO(HoodIO.getHoodCompConfiguration(), canivore)
: new HoodIOSim(canivore, HoodIO.getHoodCompConfiguration(),TurretSubsystem.HOOD_GEAR_RATIO_C));

climber = new ClimberSubsystem(); // TODO climber
break;
}
Expand Down Expand Up @@ -288,6 +300,10 @@ public Robot() {
// log if we have uncommitted changes
switch (BuildConstants.DIRTY) {
case 0:




Logger.recordMetadata("GitDirty", "All changes committed");
break;
case 1:
Expand Down Expand Up @@ -461,6 +477,11 @@ private void addAutos() {
// autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId());
// autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid());
// autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid());
autoChooser.addOption("Pitcheck/Intake ", Commands.sequence(
intake.intake().withTimeout(1),
intake.rest().withTimeout(1),
intake.outtake().withTimeout(1))
);
haveAutosGenerated = true;
System.out.println("Done generating autos");
}
Expand Down
24 changes: 24 additions & 0 deletions src/main/java/frc/robot/subsystems/climber/ClimberSubsystem
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
package frc.robot.subsystems.climber;

import static edu.wpi.first.units.Units.Volts;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism;
import frc.robot.utils.autoaim.AutoAim;
import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData;
import java.util.function.Supplier;
import org.littletonrobotics.junction.Logger;



public class ClimberSubsystem extends SubsystemBase{

}
25 changes: 24 additions & 1 deletion src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ public FlywheelIO(TalonFXConfiguration config, CANBus canbus) {
flywheelFollower.optimizeBusUtilization();
}

public static TalonFXConfiguration getFlywheelConfiguration() {
public static TalonFXConfiguration getFlywheelAlphaConfiguration() {
TalonFXConfiguration config = new TalonFXConfiguration();

config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
Expand All @@ -136,6 +136,29 @@ public static TalonFXConfiguration getFlywheelConfiguration() {
return config;
}

public static TalonFXConfiguration getFlywheelCompConfiguration() {
TalonFXConfiguration config = new TalonFXConfiguration();

config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;

config.Feedback.SensorToMechanismRatio = TurretSubsystem.FLYWHEEL_GEAR_RATIO_C;

config.Slot0.kS = 0;
config.Slot0.kV = 0;
config.Slot0.kA = 0;
config.Slot0.kP = 0;
config.Slot0.kD = 0;

config.CurrentLimits.StatorCurrentLimit = 120.0;
config.CurrentLimits.StatorCurrentLimitEnable = true;
config.CurrentLimits.SupplyCurrentLimit = 80.0;

config.MotionMagic.MotionMagicAcceleration = 100.0;

return config;
}

public void setFlywheelVoltage(double volts) {
flywheelLeader.setControl(voltageOut.withOutput(volts));
}
Expand Down
19 changes: 11 additions & 8 deletions src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java
Original file line number Diff line number Diff line change
Expand Up @@ -22,18 +22,21 @@
/** Add your docs here. */
public class FlywheelIOSim extends FlywheelIO {
TalonFXSimState leaderFxSimState;
DCMotorSim physicsSim =
new DCMotorSim(
LinearSystemId.createDCMotorSystem(
DCMotor.getKrakenX60Foc(2), 0.0136, ShooterSubsystem.FLYWHEEL_GEAR_RATIO),
DCMotor.getKrakenX60Foc(2));
DCMotorSim physicsSim;


private final double simLoopPeriod = 0.002;
private Notifier simNotifier;
private double lastSimTime = 0.0;

public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus) {
public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus, double gearRatio) {

super(config, canbus);
physicsSim =
new DCMotorSim(
LinearSystemId.createDCMotorSystem(
DCMotor.getKrakenX60Foc(2), 0.0136, gearRatio),
DCMotor.getKrakenX60Foc(2));
leaderFxSimState = flywheelLeader.getSimState();
leaderFxSimState.setMotorType(MotorType.KrakenX60);
leaderFxSimState.Orientation = ChassisReference.CounterClockwise_Positive;
Expand All @@ -51,9 +54,9 @@ public FlywheelIOSim(TalonFXConfiguration config, CANBus canbus) {
physicsSim.update(deltaTime);

leaderFxSimState.setRawRotorPosition(
physicsSim.getAngularPosition().in(Rotations) * physicsSim.getGearing());
physicsSim.getAngularPosition().in(Rotations) * gearRatio);
leaderFxSimState.setRotorVelocity(
physicsSim.getAngularVelocity().in(RotationsPerSecond) * physicsSim.getGearing());
physicsSim.getAngularVelocity().in(RotationsPerSecond) * gearRatio);
});

simNotifier.startPeriodic(simLoopPeriod);
Expand Down
35 changes: 32 additions & 3 deletions src/main/java/frc/robot/subsystems/shooter/HoodIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ public static class HoodIOInputs {

public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) {
hoodMotor = new TalonFX(11, canbus);
hoodMotor.getConfigurator().apply(HoodIO.getHoodConfiguration());
hoodMotor.getConfigurator().apply(talonFXConfiguration);

hoodPositionRotations = hoodMotor.getPosition();
hoodAngularVelocity = hoodMotor.getVelocity();
Expand All @@ -75,7 +75,7 @@ public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) {
hoodMotor.optimizeBusUtilization();
}

public static TalonFXConfiguration getHoodConfiguration() {
public static TalonFXConfiguration getHoodAlphaConfiguration() {
TalonFXConfiguration config = new TalonFXConfiguration();

config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
Expand All @@ -86,7 +86,7 @@ public static TalonFXConfiguration getHoodConfiguration() {

config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;

config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO;
config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO_A;

config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;

Expand All @@ -103,6 +103,35 @@ public static TalonFXConfiguration getHoodConfiguration() {
return config;
}

public static TalonFXConfiguration getHoodCompConfiguration(){
TalonFXConfiguration config = new TalonFXConfiguration();

config.MotorOutput.NeutralMode = NeutralModeValue.Brake;

config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;

config.MotorOutput.NeutralMode = NeutralModeValue.Brake;

config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;

config.Feedback.SensorToMechanismRatio = TurretSubsystem.HOOD_GEAR_RATIO_C;

config.Slot0.GravityType = GravityTypeValue.Arm_Cosine;

config.Slot0.kS = 0;
config.Slot0.kG = 0;
config.Slot0.kV = 0;
config.Slot0.kP = 0;
config.Slot0.kD = 0;

config.CurrentLimits.StatorCurrentLimit = 80.0;
config.CurrentLimits.StatorCurrentLimitEnable = true;
config.CurrentLimits.SupplyCurrentLimit = 60.0;

return config;

}

public void setHoodVoltage(double hoodVoltage) {
hoodMotor.setControl(voltageOut.withOutput(hoodVoltage));
}
Expand Down
47 changes: 34 additions & 13 deletions src/main/java/frc/robot/subsystems/shooter/HoodIOSim.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@

import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.sim.ChassisReference;
import com.ctre.phoenix6.sim.TalonFXSimState;
import com.ctre.phoenix6.sim.TalonFXSimState.MotorType;
Expand All @@ -14,20 +15,34 @@
public class HoodIOSim extends HoodIO {
TalonFXSimState hoodMotorSim;

private final DCMotorSim hoodPhysicsSim =
new DCMotorSim(
LinearSystemId.createDCMotorSystem(
DCMotor.getKrakenX44Foc(1), 0.01, ShooterSubsystem.HOOD_GEAR_RATIO),
DCMotor.getKrakenX44Foc(1));

private final DCMotorSim hoodPhysicsSim;

// will get updated when i get specs

private final double simLoopPeriod = 0.002; // 2 ms
private Notifier simNotifier = null;
private double lastSimTime = 0.0;

public HoodIOSim(CANBus canbus) {
super(HoodIO.getHoodConfiguration(), canbus);












public HoodIOSim(CANBus canbus, TalonFXConfiguration config, double gearRatio ) {
super(config, canbus);
hoodPhysicsSim =
new DCMotorSim(
LinearSystemId.createDCMotorSystem(
DCMotor.getKrakenX44Foc(1), 0.01,gearRatio),
DCMotor.getKrakenX44Foc(1));

hoodMotorSim = hoodMotor.getSimState();
hoodMotorSim.setMotorType(MotorType.KrakenX44);
hoodMotorSim.Orientation = ChassisReference.Clockwise_Positive;
Expand All @@ -45,12 +60,18 @@ public HoodIOSim(CANBus canbus) {
hoodPhysicsSim.update(deltaTime);

// rotor position stuff added later when i have access to onshape

hoodMotorSim.setRawRotorPosition(
hoodPhysicsSim.getAngularPositionRad() * (ShooterSubsystem.HOOD_GEAR_RATIO));
hoodMotorSim.setRotorVelocity(
hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * ShooterSubsystem.HOOD_GEAR_RATIO);

hoodMotorSim.setRawRotorPosition(
hoodPhysicsSim.getAngularPositionRad() * (gearRatio));
hoodMotorSim.setRotorVelocity(
hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * gearRatio);




});

simNotifier.startPeriodic(simLoopPeriod);
}
}

Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,8 @@

/** Fixed shooter. !! ALPHA !! */
public class ShooterSubsystem extends SubsystemBase implements Shooter {
public static double HOOD_GEAR_RATIO = 24.230769;
public static double HOOD_GEAR_RATIO_A = 24.230769;

public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40);
public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2);

Expand Down
Loading
Loading