Skip to content

Commit

Permalink
tested the chassis config reader on real chassis
Browse files Browse the repository at this point in the history
now it's working good
  • Loading branch information
catr1xLiu committed Jul 6, 2024
1 parent 112d437 commit e19d1dd
Show file tree
Hide file tree
Showing 8 changed files with 25 additions and 47 deletions.
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ public RobotContainer(String chassisName) {
new GyroIOPigeon2(),
new ModuleIOTalonFX(chassisCalibrationFile.getBlock("FrontLeft"), generalConfigBlock),
new ModuleIOTalonFX(chassisCalibrationFile.getBlock("FrontRight"), generalConfigBlock),
new ModuleIOTalonFX(chassisCalibrationFile.getBlock("backLeft"), generalConfigBlock),
new ModuleIOTalonFX(chassisCalibrationFile.getBlock("BackLeft"), generalConfigBlock),
new ModuleIOTalonFX(chassisCalibrationFile.getBlock("BackRight"), generalConfigBlock),
generalConfigBlock
);
Expand Down
22 changes: 11 additions & 11 deletions src/main/java/frc/robot/commands/DriveCommands.java
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@

package frc.robot.commands;

import edu.wpi.first.hal.simulation.REVPHDataJNI;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
Expand Down Expand Up @@ -62,17 +63,16 @@ public static Command joystickDrive(
.getTranslation();

// Convert to field relative speeds & send command
boolean isFlipped =
DriverStation.getAlliance().isPresent()
&& DriverStation.getAlliance().get() == Alliance.Red;
drive.runVelocity(
ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
omega * drive.getMaxAngularSpeedRadPerSec(),
isFlipped
? drive.getRotation().plus(new Rotation2d(Math.PI))
: drive.getRotation()));
final Rotation2d driverStationFacing = switch (DriverStation.getAlliance().orElse(Alliance.Red)) {
case Red -> new Rotation2d(Math.PI);
case Blue -> new Rotation2d(0);
};
drive.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds(
linearVelocity.getX() * drive.maxModuleVelocityMetersPerSec,
linearVelocity.getY() * drive.maxModuleVelocityMetersPerSec,
omega * drive.maxAngularVelocityRadPerSec,
drive.getPose().getRotation().minus(driverStationFacing)
));
},
drive);
}
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/drive/Drive.java
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
import org.littletonrobotics.junction.Logger;

public class Drive extends MapleSubsystem {
private final double maxModuleVelocityMetersPerSec, maxAngularVelocityRadPerSec;
public final double maxModuleVelocityMetersPerSec, maxAngularVelocityRadPerSec;
private final GyroIO gyroIO;
private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
private final OdometryThreadInputsAutoLogged odometryThreadInputs;
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/subsystems/drive/ModuleIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -51,5 +51,5 @@ default void setDriveBrakeMode(boolean enable) {}
/**
* Enable or disable brake mode on the turn motor.
*/
default void setTurnBrakeMode(boolean enable) {}
default void setSteerBrakeMode(boolean enable) {}
}
Original file line number Diff line number Diff line change
Expand Up @@ -171,7 +171,7 @@ public void setDriveBrakeMode(boolean enable) {
}

@Override
public void setTurnBrakeMode(boolean enable) {
public void setSteerBrakeMode(boolean enable) {
steerSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast);
}
}
34 changes: 6 additions & 28 deletions src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
Original file line number Diff line number Diff line change
Expand Up @@ -10,28 +10,14 @@
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.util.Units;
import frc.robot.Constants;
import frc.robot.utils.Config.MapleConfigFile;
import org.littletonrobotics.junction.Logger;

import java.util.Queue;

/**
* Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and
* CANcoder
*
* <p>NOTE: This implementation should be used as a starting point and adapted to different hardware
* configurations (e.g. If using an analog encoder, copy from "ModuleIOSparkMax")
*
* <p>To calibrate the absolute encoder offsets, point the modules straight (such that forward
* motion on the drive motor will propel the robot forward) and copy the reported values from the
* absolute encoders using AdvantageScope. These values are logged under
* "/Drive/ModuleX/TurnAbsolutePositionRad"
*/
public class ModuleIOTalonFX implements ModuleIO {
private final String name;
private final TalonFX driveTalon;
Expand Down Expand Up @@ -67,7 +53,7 @@ public ModuleIOTalonFX(MapleConfigFile.ConfigBlock moduleConfigs, MapleConfigFil
steerConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
steerTalon.getConfigurator().apply(steerConfig);
steerTalon.setInverted(moduleConfigs.getIntConfig("steeringMotorInverted") != 0);
setTurnBrakeMode(true);
setSteerBrakeMode(true);

driveEncoderUngearedRevolutions = OdometryThread.registerSignalInput(driveTalon.getPosition());
driveEncoderUngearedRevolutionsPerSecond = driveTalon.getVelocity();
Expand All @@ -81,13 +67,9 @@ public ModuleIOTalonFX(MapleConfigFile.ConfigBlock moduleConfigs, MapleConfigFil

periodicallyRefreshedSignals = new BaseStatusSignal[]{
driveEncoderUngearedRevolutionsPerSecond,
driveMotorAppliedVoltage,
driveMotorCurrent,
driveMotorAppliedVoltage, driveMotorCurrent,
steerEncoderVelocityRevolutionsPerSecond,
steerMotorAppliedVolts,
steerMotorAppliedVolts,
steerMotorAppliedVolts,
steerMotorCurrent
steerMotorAppliedVolts, steerMotorCurrent
};

BaseStatusSignal.setUpdateFrequencyForAll(50.0, periodicallyRefreshedSignals);
Expand Down Expand Up @@ -140,15 +122,11 @@ public void setSteerPower(double power) {

@Override
public void setDriveBrakeMode(boolean enable) {
var config = new MotorOutputConfigs();
config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
driveTalon.getConfigurator().apply(config);
driveTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast);
}

@Override
public void setTurnBrakeMode(boolean enable) {
var config = new MotorOutputConfigs();
config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast;
steerTalon.getConfigurator().apply(config);
public void setSteerBrakeMode(boolean enable) {
steerTalon.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast);
}
}
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/subsystems/drive/SwerveModule.java
Original file line number Diff line number Diff line change
Expand Up @@ -105,13 +105,13 @@ public void onDisable() {
io.setSteerPower(0);
io.setDrivePower(0);
io.setDriveBrakeMode(false);
io.setTurnBrakeMode(false);
io.setSteerBrakeMode(false);
}

@Override
public void onEnable() {
io.setDriveBrakeMode(true);
io.setTurnBrakeMode(true);
io.setSteerBrakeMode(true);
}

/**
Expand Down
4 changes: 2 additions & 2 deletions src/main/java/frc/robot/utils/Config/MapleConfigFile.java
Original file line number Diff line number Diff line change
Expand Up @@ -35,14 +35,14 @@ private ConfigBlock(String blockName) {
public double getDoubleConfig(String name) throws NullPointerException {
if (!doubleConfigs.containsKey(name))
throw new NullPointerException(
"Configuration not found for block: " + name + ", config: " + name + ", type: double");
"Configuration not found for block: " + blockName + ", config: " + name + ", type: double");
return doubleConfigs.get(name);
}

public int getIntConfig(String name) throws NullPointerException {
if (!intConfigs.containsKey(name))
throw new NullPointerException(
"Configuration not found for block: " + name + ", config: " + name + ", type: int");
"Configuration not found for block: " + blockName + ", config: " + name + ", type: int");
return intConfigs.get(name);
}

Expand Down

0 comments on commit e19d1dd

Please sign in to comment.