Skip to content

Commit

Permalink
more enums
Browse files Browse the repository at this point in the history
  • Loading branch information
@Abigail-27
Abigail-27 committed Jan 8, 2025
1 parent b03ec0b commit 252328f
Show file tree
Hide file tree
Showing 8 changed files with 414 additions and 32 deletions.
10 changes: 5 additions & 5 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,17 +14,17 @@ public static final class LogPaths {
public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/";
}

public static final Mode CURRENT_MODE = Mode.SIM;
public static final RobotType CURRENT_MODE = RobotType.SIMROBOT;

public static enum Mode {
public static enum RobotType {
/** Running on a real robot. */
CompRobot,
COMPROBOT,

/** Running a physics simulator. */
SimRobot,
SIMROBOT,

/** Replaying from a log file. */
DevRobot
DEVROBOT
}

/**
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,19 +43,19 @@ public Robot() {

// Set up data receivers & replay source
switch (Constants.CURRENT_MODE) {
case REAL:
case COMPROBOT:
// Running on a real robot, log to a USB stick ("/U/logs")
Logger.addDataReceiver(new WPILOGWriter());
// Gets data from network tables
Logger.addDataReceiver(new NT4Publisher());
break;

case SIM:
case SIMROBOT:
// Running a physics simulator, log to NT
Logger.addDataReceiver(new NT4Publisher());
break;

case REPLAY:
case DEVROBOT:
// Replaying a log, set up replay source
setUseTiming(false); // Run as fast as possible
String logPath = LogFileUtil.findReplayLog();
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ public class RobotContainer {

public RobotContainer() {
switch (Constants.CURRENT_MODE) {
case REAL -> {
case COMPROBOT -> {
/* Real robot, instantiate hardware IO implementations */

/* Disable Simulations */
Expand All @@ -72,7 +72,7 @@ public RobotContainer() {
visionSubsystem = new VisionSubsystem(new PhysicalVision());
}

case SIM -> {
case SIMROBOT -> {
/* Sim robot, instantiate physics sim IO implementations */

/* create simulations */
Expand Down
6 changes: 6 additions & 0 deletions src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -296,6 +296,12 @@ public boolean threesecsinactive() {
public void setXStance(SwerveModuleState[] xState) {
isMoving = false;
if (threesecsinactive()) { // Only lock if inactive for 3 seconds
Rotation2d[] xStanceAngles = {
Rotation2d.fromDegrees(45), // Front-left
Rotation2d.fromDegrees(-45), // Front-right
Rotation2d.fromDegrees(-45), // Back-left
Rotation2d.fromDegrees(45) // Back-right
};
Rotation2d[] swerveHeadings = new Rotation2d[swerveModules.length];
for (int i = 0; i < swerveHeadings.length; i++) {
swerveHeadings[i] = swerveModules[i].getPosition().angle;
Expand Down
204 changes: 198 additions & 6 deletions src/main/java/frc/robot/subsystems/swerve/module/CompModule.java
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,200 @@
// 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.subsystems.swerve.module;

/** Add your docs here. */
public class CompModule {}
import static edu.wpi.first.units.Units.*;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Current;
import edu.wpi.first.units.measure.Voltage;
import frc.robot.Constants.HardwareConstants;
import frc.robot.extras.util.DeviceCANBus;
import frc.robot.subsystems.swerve.SwerveConstants.ModuleConfig;
import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
import frc.robot.subsystems.swerve.odometryThread.OdometryThread;
import java.util.Queue;

public class CompModule implements ModuleInterface {
private final TalonFX driveMotor;
private final TalonFX turnMotor;
private final CANcoder turnEncoder;

private final VoltageOut voltageOut = new VoltageOut(0.0);
private final VelocityVoltage velocityRequest = new VelocityVoltage(0.0);
private final MotionMagicVoltage mmPositionRequest = new MotionMagicVoltage(0.0);

private final Queue<Angle> drivePosition;
private final StatusSignal<AngularVelocity> driveVelocity;
private final StatusSignal<Voltage> driveMotorAppliedVoltage;
private final StatusSignal<Current> driveMotorCurrent;

private final Queue<Angle> turnEncoderAbsolutePosition;
private final StatusSignal<AngularVelocity> turnEncoderVelocity;
private final StatusSignal<Voltage> turnMotorAppliedVolts;
private final StatusSignal<Current> turnMotorCurrent;

private final BaseStatusSignal[] periodicallyRefreshedSignals;

public CompModule(ModuleConfig moduleConfig) {
driveMotor = new TalonFX(moduleConfig.driveMotorChannel(), DeviceCANBus.CANIVORE.name);
turnMotor = new TalonFX(moduleConfig.turnMotorChannel(), DeviceCANBus.CANIVORE.name);
turnEncoder = new CANcoder(moduleConfig.turnEncoderChannel(), DeviceCANBus.CANIVORE.name);

CANcoderConfiguration turnEncoderConfig = new CANcoderConfiguration();
turnEncoderConfig.MagnetSensor.MagnetOffset = -moduleConfig.angleZero();
turnEncoderConfig.MagnetSensor.SensorDirection = moduleConfig.encoderReversed();
turnEncoder.getConfigurator().apply(turnEncoderConfig, HardwareConstants.TIMEOUT_S);

TalonFXConfiguration driveConfig = new TalonFXConfiguration();
driveConfig.Slot0.kP = ModuleConstants.DRIVE_P;
driveConfig.Slot0.kI = ModuleConstants.DRIVE_I;
driveConfig.Slot0.kD = ModuleConstants.DRIVE_D;
driveConfig.Slot0.kS = ModuleConstants.DRIVE_S;
driveConfig.Slot0.kV = ModuleConstants.DRIVE_V;
driveConfig.Slot0.kA = ModuleConstants.DRIVE_A;
driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
driveConfig.MotorOutput.Inverted = moduleConfig.driveReversed();
driveConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND;
driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
driveConfig.CurrentLimits.SupplyCurrentLimit = ModuleConstants.DRIVE_SUPPLY_LIMIT;
driveConfig.CurrentLimits.StatorCurrentLimit = ModuleConstants.DRIVE_STATOR_LIMIT;
driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;

driveMotor.getConfigurator().apply(driveConfig, HardwareConstants.TIMEOUT_S);

TalonFXConfiguration turnConfig = new TalonFXConfiguration();
turnConfig.Slot0.kP = ModuleConstants.TURN_P;
turnConfig.Slot0.kI = ModuleConstants.TURN_I;
turnConfig.Slot0.kD = ModuleConstants.TURN_D;
turnConfig.Slot0.kS = ModuleConstants.TURN_S;
turnConfig.Slot0.kV = ModuleConstants.TURN_V;
turnConfig.Slot0.kA = ModuleConstants.TURN_A;
turnConfig.Feedback.FeedbackRemoteSensorID = turnEncoder.getDeviceID();
turnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
turnConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
turnConfig.MotorOutput.Inverted = moduleConfig.turnReversed();
turnConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND;
turnConfig.MotionMagic.MotionMagicCruiseVelocity =
ModuleConstants.MAX_ANGULAR_SPEED_ROTATIONS_PER_SECOND;
turnConfig.MotionMagic.MotionMagicAcceleration =
ModuleConstants.MAX_ANGULAR_ACCELERATION_ROTATIONS_PER_SECOND_SQUARED;
turnConfig.ClosedLoopGeneral.ContinuousWrap = true;
turnConfig.CurrentLimits.SupplyCurrentLimit = 20;
turnConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
turnMotor.getConfigurator().apply(turnConfig, HardwareConstants.TIMEOUT_S);

drivePosition = OdometryThread.registerSignalInput(driveMotor.getPosition());
driveVelocity = driveMotor.getVelocity();
driveMotorAppliedVoltage = driveMotor.getMotorVoltage();
driveMotorCurrent = driveMotor.getSupplyCurrent();

turnEncoderAbsolutePosition =
OdometryThread.registerSignalInput(turnEncoder.getAbsolutePosition());
turnEncoderVelocity = turnEncoder.getVelocity();
turnMotorAppliedVolts = turnMotor.getMotorVoltage();
turnMotorCurrent = turnMotor.getSupplyCurrent();

periodicallyRefreshedSignals =
new BaseStatusSignal[] {
driveVelocity,
driveMotorAppliedVoltage,
driveMotorCurrent,
turnEncoderVelocity,
turnMotorAppliedVolts,
turnMotorCurrent
};

driveMotor.setPosition(0.0);
turnMotor.setPosition(0.0);

BaseStatusSignal.setUpdateFrequencyForAll(50.0, periodicallyRefreshedSignals);
driveMotor.optimizeBusUtilization();
turnMotor.optimizeBusUtilization();
}

@Override
public void updateInputs(ModuleInputs inputs) {
inputs.isConnected = BaseStatusSignal.isAllGood(periodicallyRefreshedSignals);

inputs.driveVelocity = driveVelocity.getValueAsDouble();

// Handle drive positions
if (!drivePosition.isEmpty()) {
Angle driveRelativePosition = Rotations.zero();
for (Angle angle : drivePosition) {
driveRelativePosition = angle;
}
inputs.drivePosition = driveRelativePosition.in(Rotations);
drivePosition.clear();
}

// Handle turn absolute positions
if (!turnEncoderAbsolutePosition.isEmpty()) {
Rotation2d turnPosition = new Rotation2d();
for (Angle angle : turnEncoderAbsolutePosition) {
turnPosition = Rotation2d.fromRotations(angle.in(Rotations));
}
inputs.turnAbsolutePosition = turnPosition;
turnEncoderAbsolutePosition.clear();
}

inputs.driveAppliedVolts = driveMotorAppliedVoltage.getValueAsDouble();
inputs.driveCurrentAmps = driveMotorCurrent.getValueAsDouble();

inputs.turnVelocity = turnEncoderVelocity.getValueAsDouble();
inputs.turnAppliedVolts = turnMotorAppliedVolts.getValueAsDouble();
inputs.turnCurrentAmps = turnMotorCurrent.getValueAsDouble();
}

@Override
public void setDriveVoltage(Voltage volts) {
driveMotor.setControl(voltageOut.withOutput(volts));
}

@Override
public void setTurnVoltage(Voltage volts) {
turnMotor.setControl(voltageOut.withOutput(volts));
}

@Override
public void setDesiredState(SwerveModuleState desiredState) {
// Converts meters per second to rotations per second
double desiredDriveRPS =
desiredState.speedMetersPerSecond
* ModuleConstants.DRIVE_GEAR_RATIO
/ ModuleConstants.WHEEL_CIRCUMFERENCE_METERS;

driveMotor.setControl(velocityRequest.withVelocity(RotationsPerSecond.of(desiredDriveRPS)));
turnMotor.setControl(
mmPositionRequest.withPosition(Rotations.of(desiredState.angle.getRotations())));
}

public double getTurnRotations() {
turnEncoder.getAbsolutePosition().refresh();
return Rotation2d.fromRotations(turnEncoder.getAbsolutePosition().getValueAsDouble())
.getRotations();
}

@Override
public void xthing(double desiredPositionDegrees) {
turnMotor.setControl(mmPositionRequest.withPosition(Degrees.of(desiredPositionDegrees)));
}

@Override
public void stopModule() {
driveMotor.stopMotor();
turnMotor.stopMotor();
}
}
Loading

0 comments on commit 252328f

Please sign in to comment.