Skip to content

Robot sim #65

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
GhostR406 wants to merge 7 commits into
base: main
Choose a base branch
from
Open

Robot sim #65

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
fb84e05
Done with all sim that is relevant
GhostR406 Mar 5, 2026
778fb39
Merge branch 'Robot-Sim' of https://github.com/FRC5188/RebuiltCode in…
GhostR406 Mar 5, 2026
21f2c0e
please no merge conflicts i beg
GhostR406 Mar 5, 2026
69521da
Merge branch 'main' of https://github.com/FRC5188/RebuiltCode into Ro…
GhostR406 Mar 6, 2026
ca20c0d
great value unicorn sparkle ice cream
GhostR406 Mar 6, 2026
cc23391
sim done part 46^(12534221)
GhostR406 Mar 7, 2026
819fa84
i forgot spotless
GhostR406 Mar 7, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 7 additions & 13 deletions src/main/java/frc/lib/W8/io/motor/MotorIOTalonFX.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,9 +75,9 @@ public record TalonFXFollower(Device.CAN id, MotorAlignmentValue opposesMain) {}
// Preconfigured control objects reused for efficiency
protected final CoastOut coastControl = new CoastOut();
protected final StaticBrake brakeControl = new StaticBrake();
protected final VoltageOut voltageControl = new VoltageOut(0).withEnableFOC(true);
protected final VoltageOut voltageControl = new VoltageOut(0);
protected final TorqueCurrentFOC currentControl = new TorqueCurrentFOC(0);
protected final DutyCycleOut dutyCycleControl = new DutyCycleOut(0).withEnableFOC(true);
protected final DutyCycleOut dutyCycleControl = new DutyCycleOut(0);
protected final DynamicMotionMagicTorqueCurrentFOC positionControl =
new DynamicMotionMagicTorqueCurrentFOC(0, 0, 0);
protected final VelocityTorqueCurrentFOC velocityControl = new VelocityTorqueCurrentFOC(0);
Expand Down Expand Up @@ -166,7 +166,7 @@ protected boolean isRunningPositionControl() {
return (control instanceof PositionTorqueCurrentFOC)
|| (control instanceof PositionVoltage)
|| (control instanceof MotionMagicTorqueCurrentFOC)
|| (control instanceof DynamicMotionMagicTorqueCurrentFOC)
|| (control instanceof MotionMagicDutyCycle)
|| (control instanceof MotionMagicVoltage);
}

Expand All @@ -181,7 +181,7 @@ protected boolean isRunningVelocityControl() {
var control = motor.getAppliedControl();
return (control instanceof VelocityTorqueCurrentFOC)
|| (control instanceof VelocityVoltage)
|| (control instanceof MotionMagicVelocityTorqueCurrentFOC)
|| (control instanceof MotionMagicVelocityDutyCycle)
|| (control instanceof MotionMagicVelocityVoltage);
}

Expand All @@ -196,8 +196,8 @@ protected boolean isRunningVelocityControl() {
protected boolean isRunningMotionMagic() {
var control = motor.getAppliedControl();
return (control instanceof MotionMagicTorqueCurrentFOC)
|| (control instanceof DynamicMotionMagicTorqueCurrentFOC)
|| (control instanceof MotionMagicVelocityTorqueCurrentFOC)
|| (control instanceof MotionMagicDutyCycle)
|| (control instanceof MotionMagicVelocityDutyCycle)
|| (control instanceof MotionMagicVoltage)
|| (control instanceof MotionMagicVelocityVoltage);
}
Expand Down Expand Up @@ -395,13 +395,7 @@ public void runPosition(
Velocity<AngularAccelerationUnit> maxJerk,
PIDSlot slot) {
this.goalPosition = position;
motor.setControl(
positionControl
.withPosition(position)
.withVelocity(cruiseVelocity)
.withAcceleration(acceleration)
.withJerk(maxJerk)
.withSlot(slot.getNum()));
motor.setControl(positionControl.withPosition(position).withSlot(slot.getNum()));
}

/**
Expand Down
157 changes: 119 additions & 38 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,16 +14,6 @@
package frc.robot;

import static edu.wpi.first.units.Units.*;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.KilogramSquareMeters;
import static edu.wpi.first.units.Units.Kilograms;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Volts;

import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.CANdleConfiguration;
Expand Down Expand Up @@ -266,10 +256,10 @@ public final class ShooterConstants {
public static final Angle STARTING_ANGLE = Rotations.of(0.0);
public static final Distance WHEEL_RADIUS = Meters.of(0.5);

public static final double IDLE_SPEED_RPM = (1.0);
public static final double HUB_SPEED_RPM = (1.0);
public static final double TOWER_SPEED_RPM = (1.0);
public static final double DEFAULT_SPEED_RPM = (1.0);
public static final AngularVelocity IDLE_SPEED_RPM = RotationsPerSecond.of(1.0);
public static final AngularVelocity HUB_SPEED_RPM = RotationsPerSecond.of(1.0);
public static final AngularVelocity TOWER_SPEED_RPM = RotationsPerSecond.of(1.0);
public static final AngularVelocity DEFAULT_SPEED_RPM = RotationsPerSecond.of(1.0);
public static final double FLYWHEEL_VELOCITY_TOLERANCE = 1.0;

// Hood Constants
Expand All @@ -286,9 +276,9 @@ public final class ShooterConstants {
new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE);
}

public class ShooterFlywheelConstants {

public static String NAME = "ShooterFlywheel";
// Needs tuned
public class ShooterFeederConstants {
public static String NAME = "ShooterFeederFlywheel";

public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI);
public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second);
Expand Down Expand Up @@ -340,6 +330,104 @@ public static TalonFXConfiguration getFXConfig(boolean invert) {
}
}

// Needs tuned
public class ShooterTowerConstants {
public static String NAME = "ShooterTowerFlywheel";

public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI);
public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second);

private static final double GEARING = (2.0 / 1.0);

public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1);

public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1);
public static final MomentOfInertia MOI = KilogramSquareMeters.of(1.0);

// Velocity PID
private static Slot0Configs SLOT0CONFIG =
new Slot0Configs().withKP(0.0).withKI(0.0).withKD(0.0);

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

config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal();
config.CurrentLimits.SupplyCurrentLimit = 40.0;
config.CurrentLimits.SupplyCurrentLowerLimit = 40.0;
config.CurrentLimits.SupplyCurrentLowerTime = 0.1;

config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal();
config.CurrentLimits.StatorCurrentLimit = 80.0;

config.Voltage.PeakForwardVoltage = 12.0;
config.Voltage.PeakReverseVoltage = -12.0;

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

config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false;

config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false;

config.Feedback.RotorToSensorRatio = 1.0;

config.Feedback.SensorToMechanismRatio = GEARING;

config.Slot0 = SLOT0CONFIG;

return config;
}
}

public class ShooterFlywheelConstants {
public static String NAME_L = "ShooterFlywheelLeft";
public static String NAME_R = "ShooterFlywheelRight";

public static final AngularVelocity MAX_VELOCITY = RadiansPerSecond.of(2 * Math.PI);
public static final AngularAcceleration MAX_ACCELERATION = MAX_VELOCITY.per(Second);

private static final double GEARING = (5.0 / 1.0);

public static final AngularVelocity TOLERANCE = MAX_VELOCITY.times(0.1);

public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1);
public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.2);

// Velocity PID
private static Slot0Configs SLOT0CONFIG =
new Slot0Configs().withKP(0.75).withKI(0.0).withKD(0.0);

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

config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal();
config.CurrentLimits.SupplyCurrentLimit = 40.0;
config.CurrentLimits.SupplyCurrentLowerLimit = 40.0;
config.CurrentLimits.SupplyCurrentLowerTime = 0.1;

config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal();
config.CurrentLimits.StatorCurrentLimit = 80.0;

config.Voltage.PeakForwardVoltage = 12.0;
config.Voltage.PeakReverseVoltage = -12.0;

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

config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false;

config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false;

config.Feedback.RotorToSensorRatio = 1.0;

config.Feedback.SensorToMechanismRatio = GEARING;

config.Slot0 = SLOT0CONFIG;

return config;
}
}

public class ShooterRotaryConstants {
public static String NAME = "ShooterRotary";

Expand All @@ -350,13 +438,13 @@ public class ShooterRotaryConstants {
// CRUISE_VELOCITY.div(0.1).per(Units.Second);
// public static final Velocity<AngularAccelerationUnit> JERK = ACCELERATION.per(Second);

private static final double ROTOR_TO_SENSOR = (2.0 / 1.0);
private static final double SENSOR_TO_MECHANISM = (2.0 / 1.0);
private static final double ROTOR_TO_SENSOR = (1.0 / 1.0);
private static final double SENSOR_TO_MECHANISM = (50.0 / 1.0);

public static final Translation3d OFFSET = Translation3d.kZero;

public static final Angle MIN_ANGLE = Degrees.of(0.0);
public static final Angle MAX_ANGLE = Rotations.of(45.0);
public static final Angle MAX_ANGLE = Degrees.of(45.0);
public static final Angle STARTING_ANGLE = Rotations.of(0.0);
public static final Distance ARM_LENGTH = Meters.of(1.0);

Expand Down Expand Up @@ -395,12 +483,12 @@ public class ShooterRotaryConstants {
public static TalonFXConfiguration getFXConfig() {
TalonFXConfiguration config = new TalonFXConfiguration();

config.CurrentLimits.SupplyCurrentLimitEnable = false;
config.CurrentLimits.SupplyCurrentLimitEnable = Robot.isReal();
config.CurrentLimits.SupplyCurrentLimit = 40.0;
config.CurrentLimits.SupplyCurrentLowerLimit = 40.0;
config.CurrentLimits.SupplyCurrentLowerTime = 0.1;

config.CurrentLimits.StatorCurrentLimitEnable = false;
config.CurrentLimits.StatorCurrentLimitEnable = Robot.isReal();
config.CurrentLimits.StatorCurrentLimit = 80.0;

config.Voltage.PeakForwardVoltage = 12.0;
Expand All @@ -418,8 +506,6 @@ public static TalonFXConfiguration getFXConfig() {
config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR;
config.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM;

config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;

config.Slot0 = SLOT0CONFIG;

return config;
Expand Down Expand Up @@ -452,10 +538,10 @@ public class IntakeFlywheelConstants {
public static final Angle MIN_ANGLE = Rotations.of(0.0);
public static final Angle MAX_ANGLE = Rotations.of(1);
public static final Angle STARTING_ANGLE = Rotations.of(0.0);
public static final double PICKUP_SPEED = 0.0;
public static final double PICKUP_SPEED = 10.0;
public static final Distance WHEEL_RADIUS = Meters.of(0.05);
public static final Translation3d OFFSET = Translation3d.kZero;
public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125);
public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.2);
public static final RotaryMechCharacteristics CONSTANTS =
new RotaryMechCharacteristics(OFFSET, WHEEL_RADIUS, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE);
public static final String MOTOR_NAME = "Intake Flywheel";
Expand Down Expand Up @@ -584,7 +670,10 @@ public static TalonFXConfiguration getFXConfig() {
public class IntakePivotConstants {
public static final String NAME = "Intake";

public static final Angle PICKUP_ANGLE = Rotations.of(0.0);
public static final Angle MIN_ANGLE = Degrees.of(0.0);
public static final Angle MAX_ANGLE = Degrees.of(130.0);

public static final Angle PICKUP_ANGLE = MAX_ANGLE;
public static final Angle STOW_ANGLE = Rotations.of(0.0);

public static final Angle TOLERANCE = Degrees.of(1.0);
Expand All @@ -594,11 +683,8 @@ public class IntakePivotConstants {
public static final Velocity<AngularAccelerationUnit> JERK =
RadiansPerSecondPerSecond.per(Second).of(0.1);

private static final double ROTOR_TO_SENSOR = (50.0 / 1.0);
private static final double SENSOR_TO_MECHANISM = 1.0;

public static final Angle MIN_ANGLE = Degrees.of(0.0);
public static final Angle MAX_ANGLE = Degrees.of(130.0);
private static final double ROTOR_TO_SENSOR = (1.0 / 1.0);
private static final double SENSOR_TO_MECHANISM = (50.0 / 1.0);
public static final Angle STARTING_ANGLE = Radians.zero();
public static final Distance ARM_LENGTH = Foot.one();

Expand All @@ -607,7 +693,7 @@ public class IntakePivotConstants {
new Translation3d(), ARM_LENGTH, MIN_ANGLE, MAX_ANGLE, STARTING_ANGLE);

public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1);
public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.25);
public static final MomentOfInertia MOI = KilogramSquareMeters.of(1);

// Positional PID
public static final Slot0Configs SLOT_0_CONFIG =
Expand Down Expand Up @@ -652,11 +738,6 @@ public static TalonFXConfiguration getFXConfig() {
}
}

public class FeederConstants {
public static final AngularVelocity FEED_SPEED = RotationsPerSecond.of(0.0);
public static final AngularAcceleration FEED_ACCELERATION = RotationsPerSecondPerSecond.of(0.0);
}

public class ClimberConstants {
public static final DCMotor DCMOTOR = DCMotor.getKrakenX60(1);
public static final Mass CARRIAGE_MASS = Kilograms.of(2.5);
Expand Down
Loading