Skip to content

Commit

Permalink
Merge branch 'dev' into feat/outreach
Browse files Browse the repository at this point in the history
  • Loading branch information
NoraZitnick committed Dec 5, 2024
2 parents 7b7b215 + 1b11b2a commit 2e764b1
Show file tree
Hide file tree
Showing 21 changed files with 425 additions and 398 deletions.
4 changes: 0 additions & 4 deletions .factorypath

This file was deleted.

6 changes: 2 additions & 4 deletions .github/workflows/build.yml
Original file line number Diff line number Diff line change
@@ -1,8 +1,6 @@
name: Build

on:
push:
pull_request:
on: [push, pull_request]

jobs:
build:
Expand All @@ -11,7 +9,7 @@ jobs:
container: wpilib/roborio-cross-ubuntu:2024-22.04
steps:
- name: Checkout repository
uses: actions/checkout@v2
uses: actions/checkout@v4
- name: Grant execute permission
run: chmod +x gradlew
- name: Build robot code
Expand Down
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2024.3.2"
id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-1"
id "com.peterabeles.gversion" version "1.10"
id "com.diffplug.spotless" version "6.12.0"
}
Expand Down
2 changes: 1 addition & 1 deletion gradle/wrapper/gradle-wrapper.properties
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
distributionBase=GRADLE_USER_HOME
distributionPath=permwrapper/dists
distributionUrl=https\://services.gradle.org/distributions/gradle-8.4-bin.zip
distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
networkTimeout=10000
validateDistributionUrl=true
zipStoreBase=GRADLE_USER_HOME
Expand Down
10 changes: 7 additions & 3 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -133,12 +133,16 @@ private void configureBindings() {
swerve
.run(
() -> {
swerve.driveTeleopController(-driverA.getLeftY(), -driverA.getLeftX());
swerve.driveTeleopController(
-driverA.getLeftY(),
-driverA.getLeftX(),
driverA.getLeftTriggerAxis() - driverA.getRightTriggerAxis());
})
.withName("Drive Teleop"));

DoubleSupplier rotationAbsolute =
() -> driverA.getRightTriggerAxis() - driverA.getLeftTriggerAxis();
driverA.start().onTrue(swerve.zeroGyroCommand());

// -----Intake Controls-----

new Trigger(() -> Math.abs(rotationAbsolute.getAsDouble()) > 0.07)
.whileTrue(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,23 +9,29 @@
import com.ctre.phoenix6.controls.NeutralOut;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.util.Units;
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.Temperature;
import edu.wpi.first.units.measure.Voltage;

public class FlywheelsIOTalonFX implements FlywheelsIO {
private final TalonFX topTalon;
private final TalonFX bottomTalon;

private final StatusSignal<Double> topPositionRads;
private final StatusSignal<Double> topVelocityRPM;
private final StatusSignal<Double> topAppliedVolts;
private final StatusSignal<Double> topSupplyCurrent;
private final StatusSignal<Double> topTemp;
private final StatusSignal<Double> bottomPositionRads;
private final StatusSignal<Double> bottomVelocityRPM;
private final StatusSignal<Double> bottomAppliedVolts;
private final StatusSignal<Double> bottomSupplyCurrent;
private final StatusSignal<Double> bottomTemp;
private final StatusSignal<Angle> topPosition;
private final StatusSignal<AngularVelocity> topVelocity;
private final StatusSignal<Voltage> topAppliedVolts;
private final StatusSignal<Current> topSupplyCurrent;
private final StatusSignal<Temperature> topTemp;
private final StatusSignal<Angle> bottomPosition;
private final StatusSignal<AngularVelocity> bottomVelocity;
private final StatusSignal<Voltage> bottomAppliedVolts;
private final StatusSignal<Current> bottomSupplyCurrent;
private final StatusSignal<Temperature> bottomTemp;

private final Slot0Configs gainsConfig = new Slot0Configs();
private final VelocityVoltage velocityOutput = new VelocityVoltage(0).withUpdateFreqHz(0);
Expand All @@ -39,6 +45,7 @@ public FlywheelsIOTalonFX() {
config.CurrentLimits.SupplyCurrentLimit = 60.0; // FIXME
config.CurrentLimits.SupplyCurrentLimitEnable = true;
config.MotorOutput.NeutralMode = NeutralModeValue.Coast;
config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
config.Feedback.SensorToMechanismRatio = FLYWHEEL_CONFIG.reduction();

gainsConfig.kP = GAINS.kP();
Expand All @@ -53,17 +60,14 @@ public FlywheelsIOTalonFX() {
topTalon.getConfigurator().apply(gainsConfig);
bottomTalon.getConfigurator().apply(gainsConfig);

topTalon.setInverted(true);
bottomTalon.setInverted(true); // FIXME

topPositionRads = topTalon.getPosition();
topVelocityRPM = topTalon.getVelocity();
topPosition = topTalon.getPosition();
topVelocity = topTalon.getVelocity();
topAppliedVolts = topTalon.getMotorVoltage();
topSupplyCurrent = topTalon.getSupplyCurrent();
topTemp = topTalon.getDeviceTemp();

bottomPositionRads = bottomTalon.getPosition();
bottomVelocityRPM = bottomTalon.getVelocity();
bottomPosition = bottomTalon.getPosition();
bottomVelocity = bottomTalon.getVelocity();
bottomAppliedVolts = bottomTalon.getMotorVoltage();
bottomSupplyCurrent = bottomTalon.getSupplyCurrent();
bottomTemp = bottomTalon.getDeviceTemp();
Expand All @@ -73,25 +77,21 @@ public FlywheelsIOTalonFX() {
public void updateInputs(FlywheelsIOInputs inputs) {
inputs.topMotorConnected =
BaseStatusSignal.refreshAll(
topPositionRads, topVelocityRPM, topAppliedVolts, topSupplyCurrent, topTemp)
topPosition, topVelocity, topAppliedVolts, topSupplyCurrent, topTemp)
.isOK();
inputs.bottomMotorConnected =
BaseStatusSignal.refreshAll(
bottomPositionRads,
bottomVelocityRPM,
bottomAppliedVolts,
bottomSupplyCurrent,
bottomTemp)
bottomPosition, bottomVelocity, bottomAppliedVolts, bottomSupplyCurrent, bottomTemp)
.isOK();

inputs.topPositionRads = Units.rotationsToRadians(topPositionRads.getValueAsDouble());
inputs.topVelocityRPM = topVelocityRPM.getValueAsDouble() * 60.0;
inputs.topPositionRads = Units.rotationsToRadians(topPosition.getValueAsDouble());
inputs.topVelocityRPM = topVelocity.getValueAsDouble() * 60.0;
inputs.topAppliedVolts = topAppliedVolts.getValueAsDouble();
inputs.topSupplyCurrent = topSupplyCurrent.getValueAsDouble();
inputs.topTempCelcius = topTemp.getValueAsDouble();

inputs.bottomPositionRads = Units.rotationsToRadians(bottomPositionRads.getValueAsDouble());
inputs.bottomVelocityRPM = bottomVelocityRPM.getValueAsDouble() * 60.0;
inputs.bottomPositionRads = Units.rotationsToRadians(bottomPosition.getValueAsDouble());
inputs.bottomVelocityRPM = bottomVelocity.getValueAsDouble() * 60.0;
inputs.bottomAppliedVolts = bottomAppliedVolts.getValueAsDouble();
inputs.bottomSupplyCurrent = bottomSupplyCurrent.getValueAsDouble();
inputs.bottomTempCelcius = bottomTemp.getValueAsDouble();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,18 @@
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.util.Units;
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;

public abstract class GenericRollersIOTalonFX implements GenericRollersIO {
private final TalonFX talon;

private final StatusSignal<Double> position;
private final StatusSignal<Double> velocity;
private final StatusSignal<Double> appliedVolts;
private final StatusSignal<Double> supplyCurrent;
private final StatusSignal<Angle> position;
private final StatusSignal<AngularVelocity> velocity;
private final StatusSignal<Voltage> appliedVolts;
private final StatusSignal<Current> supplyCurrent;

private final VoltageOut voltageOutput = new VoltageOut(0).withUpdateFreqHz(0);
private final NeutralOut neutralOutput = new NeutralOut();
Expand Down
76 changes: 24 additions & 52 deletions src/main/java/frc/robot/subsystems/swerve/Drive.java
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,11 @@
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Util;
import frc.robot.Constants;
import frc.robot.subsystems.swerve.controllers.TeleopController;
import org.littletonrobotics.junction.AutoLogOutput;
import org.littletonrobotics.junction.Logger;

Expand All @@ -38,9 +40,10 @@ public enum DriveModes {
@AutoLogOutput(key = "Swerve/YawOffset")
private Rotation2d gyroYawOffset = new Rotation2d();

private ChassisSpeeds teleopTargetSpeeds = new ChassisSpeeds();
private ChassisSpeeds targetSpeeds = new ChassisSpeeds();

private final TeleopController teleopController;

public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) {
this.gyroIO = gyroIO;

Expand All @@ -49,10 +52,7 @@ public Drive(GyroIO gyroIO, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br)
modules[2] = new Module(bl, 2);
modules[3] = new Module(br, 3);

rotController = new PIDController(0.01, 0, 0);
rotController.setTolerance(1);
rotController.setSetpoint(0);
rotController.setTolerance(1);
teleopController = new TeleopController();
}

@Override
Expand All @@ -71,7 +71,7 @@ public void periodic() {

switch (driveMode) {
case TELEOP -> {
targetSpeeds = teleopTargetSpeeds;
targetSpeeds = teleopController.update(arbitraryYaw);
}
case TRAJECTORY -> {}
case ANGLE -> {
Expand All @@ -84,15 +84,18 @@ public void periodic() {
/* use kinematics to get desired module states */
ChassisSpeeds discretizedSpeeds =
ChassisSpeeds.discretize(targetSpeeds, Constants.PERIODIC_LOOP_SEC);
SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(targetSpeeds);
/* ChassisSpeeds discretizedSpeeds = targetSpeeds; // FIXME
discretizedSpeeds.discretize(Constants.PERIODIC_LOOP_SEC); */

SwerveModuleState[] moduleTargetStates = KINEMATICS.toSwerveModuleStates(discretizedSpeeds);
SwerveDriveKinematics.desaturateWheelSpeeds(
moduleTargetStates, DRIVE_CONFIG.maxLinearVelocity());

SwerveModuleState[] optimizedTargetStates = new SwerveModuleState[4];

for (int i = 0; i < modules.length; i++) {
optimizedTargetStates[i] =
SwerveModuleState.optimize(moduleTargetStates[i], modules[i].getSteerHeading());
optimizedTargetStates[i] = moduleTargetStates[i];
optimizedTargetStates[i].optimize(modules[i].getSteerHeading());
modules[i].runToSetpoint(optimizedTargetStates[i]);
}

Expand All @@ -101,56 +104,25 @@ public void periodic() {
Logger.recordOutput("Swerve/DriveMode", driveMode);
}

public void driveTeleopController(double xAxis, double yAxis) {

driveAnglePeriodic(xAxis, yAxis, targetAngle);
}
public void driveTeleopController(double xAxis, double yAxis, double omega) {
if (DriverStation.isTeleopEnabled()) {
if (driveMode != DriveModes.TELEOP) {
driveMode = DriveModes.TELEOP;
}

public void setTrajectoryFollower(ChassisSpeeds trajectorySpeeds) {
if (DriverStation.isAutonomousEnabled()) {
driveMode = DriveModes.TRAJECTORY;
teleopController.acceptJoystickInput(xAxis, yAxis, omega);
}
}

public void zero() {
gyroYawOffset = Rotation2d.fromDegrees(gyroInputs.yawPosition.getDegrees());
targetAngle = 0;
}

public void driveAnglePeriodic(double xAxis, double yAxis, double targetAngle) {
this.targetAngle = targetAngle;
double angularDifference = getAngularError(targetAngle);

double rotationValue = rotController.calculate(angularDifference);

// we are treating this like a joystick, so -1 and 1 are its lower and upper bound
rotationValue = MathUtil.clamp(rotationValue, -1, 1);

// this value makes our unit-less [-1, 1] into [-max angular, max angular]
double omegaRadiansPerSecond =
rotationValue * DriveConstants.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND;

double xVelocity =
MathUtil.applyDeadband(Math.copySign(xAxis * xAxis, xAxis), 0.07)
* DRIVE_CONFIG.maxLinearVelocity();
double yVelocity =
MathUtil.applyDeadband(Math.copySign(yAxis * yAxis, yAxis), 0.07)
* DRIVE_CONFIG.maxLinearVelocity();

// initialize chassis speeds but add our desired angle
teleopTargetSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(
xVelocity, yVelocity, omegaRadiansPerSecond, arbitraryYaw);
Logger.recordOutput("Swerve/Teleop/xVelocity", xVelocity);
Logger.recordOutput("Swerve/Teleop/yVelocity", yVelocity);
Logger.recordOutput("Swerve/Teleop/radianVelocity", 0);
}

public double getAngularError(double targetAngle) {
return -Util.relativeAngularDifference(arbitraryYaw.times(-1), targetAngle);
}

public double getTargetAngle() {
return targetAngle;
private void zeroGyro() {
gyroYawOffset = gyroInputs.yawPosition;
}

public Command zeroGyroCommand() {
return this.runOnce(() -> zeroGyro());
}
}
Loading

0 comments on commit 2e764b1

Please sign in to comment.