Skip to content
This repository has been archived by the owner on Apr 20, 2024. It is now read-only.

Commit

Permalink
Merge branch 'main' into Queuer-Subsystem
Browse files Browse the repository at this point in the history
  • Loading branch information
@rhetorr
rhetorr authored Feb 11, 2024
2 parents 492dfb4 + ad977ca commit e8bb1cb
Show file tree
Hide file tree
Showing 14 changed files with 477 additions and 266 deletions.
18 changes: 7 additions & 11 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,11 +27,11 @@
import frc.robot.robot_manager.RobotCommands;
import frc.robot.robot_manager.RobotManager;
import frc.robot.shooter.ShooterSubsystem;
import frc.robot.shoulder.ShoulderSubsystem;
import frc.robot.snaps.SnapManager;
import frc.robot.swerve.SwerveSubsystem;
import frc.robot.util.scheduling.LifecycleSubsystemManager;
import frc.robot.vision.VisionSubsystem;
import frc.robot.wrist.WristSubsystem;
import org.littletonrobotics.junction.LoggedRobot;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.NT4Publisher;
Expand All @@ -43,17 +43,14 @@ public class Robot extends LoggedRobot {
private final CommandXboxController operatorController = new CommandXboxController(1);

private final PowerDistribution pdh = new PowerDistribution(1, ModuleType.kRev);
private final ShoulderSubsystem shoulder =
new ShoulderSubsystem(
new TalonFX(
RobotConfig.get().shoulder().rightMotorID(), RobotConfig.get().canivoreName()),
new TalonFX(
RobotConfig.get().shoulder().leftMotorID(), RobotConfig.get().canivoreName()));
private final WristSubsystem wrist =
new WristSubsystem(
new TalonFX(RobotConfig.get().wrist().motorID(), RobotConfig.get().canivoreName()));
private final ShooterSubsystem shooter =
new ShooterSubsystem(
new TalonFX(RobotConfig.get().shooter().leftMotorID(), RobotConfig.get().canivoreName()),
new TalonFX(
RobotConfig.get().shooter().bottomMotorID(), RobotConfig.get().canivoreName()),
new TalonFX(RobotConfig.get().shooter().topMotorID(), RobotConfig.get().canivoreName()));
RobotConfig.get().shooter().rightMotorID(), RobotConfig.get().canivoreName()));
private final IClimberSubsystem climber =
RobotConfig.IS_PRACTICE_BOT
? new ClimberSubsystemStub()
Expand All @@ -74,8 +71,7 @@ public class Robot extends LoggedRobot {
private final LocalizationSubsystem localization = new LocalizationSubsystem(swerve, imu, vision);
private final SnapManager snaps = new SnapManager(swerve, driverController);
private final RobotManager robotManager =
new RobotManager(
shoulder, intake, shooter, localization, vision, climber, swerve, snaps, imu);
new RobotManager(wrist, intake, shooter, localization, vision, climber, swerve, snaps, imu);
private final RobotCommands actions = new RobotCommands(robotManager);
private final Autos autos = new Autos(swerve, localization, imu, actions);
private final LightsSubsystem lightsSubsystem =
Expand Down
39 changes: 36 additions & 3 deletions src/main/java/frc/robot/config/CompConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,14 @@
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.geometry.Rotation2d;
import frc.robot.config.RobotConfig.ClimberConfig;
import frc.robot.config.RobotConfig.ElevatorConfig;
import frc.robot.config.RobotConfig.IMUConfig;
import frc.robot.config.RobotConfig.IntakeConfig;
import frc.robot.config.RobotConfig.LightsConfig;
import frc.robot.config.RobotConfig.QueuerConfig;
import frc.robot.config.RobotConfig.ShooterConfig;
import frc.robot.config.RobotConfig.ShoulderConfig;
import frc.robot.config.RobotConfig.SwerveConfig;
import frc.robot.config.RobotConfig.WristConfig;

class CompConfig {
public static final RobotConfig competitionBot =
Expand All @@ -34,6 +35,7 @@ class CompConfig {
new ShooterConfig(
18,
19,
// Left Motor
new TalonFXConfiguration()
.withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1.0 / 1.5))
.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(120))
Expand All @@ -42,6 +44,7 @@ class CompConfig {
new MotorOutputConfigs()
.withNeutralMode(NeutralModeValue.Coast)
.withInverted(InvertedValue.Clockwise_Positive)),
// Right Motor
new TalonFXConfiguration()
.withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1.0 / 1.5))
.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(120))
Expand Down Expand Up @@ -72,9 +75,8 @@ class CompConfig {
new VoltageConfigs()
.withPeakForwardVoltage(12.0)
.withPeakReverseVoltage(-2.0))),
new ShoulderConfig(
new WristConfig(
14,
15,
new TalonFXConfiguration()
.withSlot0(
new Slot0Configs()
Expand Down Expand Up @@ -127,6 +129,37 @@ class CompConfig {
floorSpotDistanceToAngle.put(4.8, 20.0);
floorSpotDistanceToAngle.put(15.0, 10.0);
}),
new ElevatorConfig(
0,
new TalonFXConfiguration()
.withSlot0(
new Slot0Configs()
.withGravityType(GravityTypeValue.Arm_Cosine)
.withKG(0.0)
.withKP(0.0))
.withSlot1(
new Slot1Configs()
.withGravityType(GravityTypeValue.Arm_Cosine)
.withKG(0.0)
.withKP(0.0))
.withMotionMagic(
new MotionMagicConfigs()
.withMotionMagicAcceleration(2.0)
.withMotionMagicCruiseVelocity(1.5)
.withMotionMagicJerk(0))
.withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(0))
.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(40))
.withMotorOutput(
new MotorOutputConfigs()
.withInverted(InvertedValue.Clockwise_Positive)
.withNeutralMode(NeutralModeValue.Brake)),
new CurrentLimitsConfigs().withSupplyCurrentLimit(40),
-0.05,
0,
0,
4,
0.0,
0.0),
new IntakeConfig(
16,
22,
Expand Down
41 changes: 36 additions & 5 deletions src/main/java/frc/robot/config/PracticeConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,14 @@
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.geometry.Rotation2d;
import frc.robot.config.RobotConfig.ClimberConfig;
import frc.robot.config.RobotConfig.ElevatorConfig;
import frc.robot.config.RobotConfig.IMUConfig;
import frc.robot.config.RobotConfig.IntakeConfig;
import frc.robot.config.RobotConfig.LightsConfig;
import frc.robot.config.RobotConfig.QueuerConfig;
import frc.robot.config.RobotConfig.ShooterConfig;
import frc.robot.config.RobotConfig.ShoulderConfig;
import frc.robot.config.RobotConfig.SwerveConfig;
import frc.robot.config.RobotConfig.WristConfig;

class PracticeConfig {
public static final RobotConfig config =
Expand All @@ -33,7 +34,7 @@ class PracticeConfig {
new ShooterConfig(
18,
19,
// Bottom motor
// Left motor
new TalonFXConfiguration()
.withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1))
.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(120))
Expand All @@ -42,7 +43,7 @@ class PracticeConfig {
new MotorOutputConfigs()
.withInverted(InvertedValue.Clockwise_Positive)
.withNeutralMode(NeutralModeValue.Coast)),
// Top motor
// Right motor
new TalonFXConfiguration()
.withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1))
.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(120))
Expand All @@ -61,9 +62,8 @@ class PracticeConfig {
floorSpotDistanceToRPM.put(4.8, 2700.0);
}),
new ClimberConfig(20, 21, new TalonFXConfiguration()),
new ShoulderConfig(
new WristConfig(
14,
15,
new TalonFXConfiguration()
.withSlot0(
new Slot0Configs()
Expand Down Expand Up @@ -116,6 +116,37 @@ class PracticeConfig {
floorSpotDistanceToAngle.put(3.4, 60.0);
floorSpotDistanceToAngle.put(4.8, 50.0);
}),
new ElevatorConfig(
0,
new TalonFXConfiguration()
.withSlot0(
new Slot0Configs()
.withGravityType(GravityTypeValue.Arm_Cosine)
.withKG(0.0)
.withKP(0.0))
.withSlot1(
new Slot1Configs()
.withGravityType(GravityTypeValue.Arm_Cosine)
.withKG(0.0)
.withKP(0.0))
.withMotionMagic(
new MotionMagicConfigs()
.withMotionMagicAcceleration(2.0)
.withMotionMagicCruiseVelocity(1.5)
.withMotionMagicJerk(0))
.withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(0))
.withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(40))
.withMotorOutput(
new MotorOutputConfigs()
.withInverted(InvertedValue.Clockwise_Positive)
.withNeutralMode(NeutralModeValue.Brake)),
new CurrentLimitsConfigs().withSupplyCurrentLimit(40),
-0.05,
0,
0,
4,
0.0,
0.0),
new IntakeConfig(
16,
22,
Expand Down
30 changes: 21 additions & 9 deletions src/main/java/frc/robot/config/RobotConfig.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,17 +16,19 @@ public record RobotConfig(
String canivoreName,
ShooterConfig shooter,
ClimberConfig climber,
ShoulderConfig shoulder,
WristConfig wrist,
ElevatorConfig elevator,
IntakeConfig intake,
QueuerConfig queuer,
SwerveConfig swerve,
IMUConfig imu,
LightsConfig lights) {

public record ShooterConfig(
int bottomMotorID,
int topMotorID,
TalonFXConfiguration bottomMotorConfig,
TalonFXConfiguration topMotorConfig,
int leftMotorID,
int rightMotorID,
TalonFXConfiguration leftMotorConfig,
TalonFXConfiguration rightMotorConfig,
Consumer<InterpolatingDoubleTreeMap> speakerShotRpms,
Consumer<InterpolatingDoubleTreeMap> floorShotRpms) {}

Expand All @@ -41,10 +43,9 @@ public record IntakeConfig(
TalonFXConfiguration bottomMotorConfig) {}

public record QueuerConfig(int motorID, int sensorID, TalonFXConfiguration motorConfig) {}

public record ShoulderConfig(
int rightMotorID,
int leftMotorID,

public record WristConfig(
int motorID,
TalonFXConfiguration motorConfig,
CurrentLimitsConfigs strictCurrentLimits,
double homingVoltage,
Expand All @@ -57,6 +58,17 @@ public record ShoulderConfig(
Consumer<InterpolatingDoubleTreeMap> speakerShotAngles,
Consumer<InterpolatingDoubleTreeMap> floorShotAngles) {}

public record ElevatorConfig(
int motorID,
TalonFXConfiguration motorConfig,
CurrentLimitsConfigs strictCurrentLimits,
double homingVoltage,
double homingCurrentThreshold,
double homingEndPosition,
int currentTaps,
double minHeight,
double maxHeight) {}

public record IMUConfig(
int deviceID, Consumer<InterpolatingDoubleTreeMap> distanceToAngleTolerance) {}

Expand Down
24 changes: 24 additions & 0 deletions src/main/java/frc/robot/elevator/ElevatorPositions.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.elevator;

public class ElevatorPositions {
public static final double STOWED = 0;

public static final double GROUND_INTAKING = 0.0;
public static final double SOURCE_INTAKING = 0.0;

public static final double OUTTAKING = 0.0;

public static final double FLOOR_SHOT = 0.0;

public static final double SUBWOOFER_SHOT = 0.0;
public static final double AMP_SHOT = 0.0;

public static final double WAITING_CLIMBER_RAISED = 0.0;
public static final double TRAP_SHOT = 0.0;

private ElevatorPositions() {}
}
Loading

0 comments on commit e8bb1cb

Please sign in to comment.