Skip to content

Commit

Permalink
yo im not dumb
Browse files Browse the repository at this point in the history
  • Loading branch information
@Ishan1522
Ishan1522 authored Oct 8, 2024
1 parent 4acfb86 commit 96df027
Show file tree
Hide file tree
Showing 3 changed files with 46 additions and 48 deletions.
4 changes: 4 additions & 0 deletions src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleIO.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,4 +76,8 @@ default void setDriveVoltage(double voltage) {}
default void setTurnVoltage(double voltage) {}

default void stopModule() {}

default double getTurnRotations() {
return 0.0;
}
}
28 changes: 8 additions & 20 deletions src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleIOSim.java
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,3 @@
// Copyright 2021-2024 FRC 6328
// http://github.com/Mechanical-Advantage
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// version 3 as published by the Free Software Foundation or
// available in the root directory of this project.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.

// Modified by 5516 "IRON MAPLE", original source:
// https://github.com/Shenzhen-Robotics-Alliance/maple-sim

package frc.robot.subsystems.swerve.moduleIO;

import edu.wpi.first.math.controller.PIDController;
Expand Down Expand Up @@ -77,12 +61,13 @@ public void setTurnVoltage(double volts) {
}

public void setDesiredState(SwerveModuleState desiredState) {
double turnRotations = getTurnRotations();
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState optimizedDesiredState =
SwerveModuleState.optimize(desiredState, desiredState.angle);
SwerveModuleState.optimize(desiredState, Rotation2d.fromRotations(turnRotations));

if (Math.abs(optimizedDesiredState.speedMetersPerSecond) < 0.01) {
moduleSimulation.requestDriveVoltageOut(0);;
moduleSimulation.requestDriveVoltageOut(0);
moduleSimulation.requestSteerVoltageOut(0);
return;
}
Expand All @@ -94,7 +79,10 @@ public void setDesiredState(SwerveModuleState desiredState) {
/ ModuleConstants.WHEEL_CIRCUMFERENCE_METERS;

moduleSimulation.requestDriveVoltageOut(drivePID.calculate(moduleSimulation.getDriveWheelFinalSpeedRadPerSec(), desiredDriveRPS) + driveFF.calculate(desiredDriveRPS));
moduleSimulation.requestSteerVoltageOut(turnPID.calculate(moduleSimulation.getSteerAbsoluteFacing().getRotations(), desiredState.angle.getRotations()) + turnFF.calculate(desiredDriveRPS));

moduleSimulation.requestSteerVoltageOut(turnPID.calculate(moduleSimulation.getSteerAbsoluteFacing().getRotations(), desiredState.angle.getRotations()) + turnFF.calculate(turnPID.getSetpoint().velocity));
}

public double getTurnRotations() {
return moduleSimulation.getSteerAbsoluteFacing().getRotations();
}
}
62 changes: 34 additions & 28 deletions src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleIOTalonFX.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,11 +43,11 @@ public class ModuleIOTalonFX implements ModuleIO {
private final VelocityVoltage velocityRequest = new VelocityVoltage(0.0);
private final MotionMagicVoltage mmPositionRequest = new MotionMagicVoltage(0.0);

private final Queue<Double> driveEncoderUngearedRevolutions;
private final StatusSignal<Double> driveEncoderUngearedRevolutionsPerSecond, driveMotorAppliedVoltage, driveMotorCurrent;
private final Queue<Double> driveEncoderUngeared;
private final StatusSignal<Double> driveEncoderUngearedPerSecond, driveMotorAppliedVoltage, driveMotorCurrent;

private final Queue<Double> steerEncoderAbsolutePositionRevolutions;
private final StatusSignal<Double> steerEncoderVelocityRevolutionsPerSecond, steerMotorAppliedVolts, steerMotorCurrent;
private final Queue<Double> turnEncoderAbsolutePosition;
private final StatusSignal<Double> turnEncoderVelocityPerSecond, turnMotorAppliedVolts, turnMotorCurrent;

private final BaseStatusSignal[] periodicallyRefreshedSignals;

Expand Down Expand Up @@ -99,21 +99,21 @@ public ModuleIOTalonFX(ModuleConfig moduleConfig) {
turnMotor.getConfigurator().apply(turnConfig, HardwareConstants.TIMEOUT_S);


driveEncoderUngearedRevolutions = OdometryThread.registerSignalInput(driveMotor.getPosition());
driveEncoderUngearedRevolutionsPerSecond = driveMotor.getVelocity();
driveEncoderUngeared = OdometryThread.registerSignalInput(driveMotor.getPosition());
driveEncoderUngearedPerSecond = driveMotor.getVelocity();
driveMotorAppliedVoltage = driveMotor.getMotorVoltage();
driveMotorCurrent = driveMotor.getSupplyCurrent();

steerEncoderAbsolutePositionRevolutions = OdometryThread.registerSignalInput(turnEncoder.getAbsolutePosition());
steerEncoderVelocityRevolutionsPerSecond = turnEncoder.getVelocity();
steerMotorAppliedVolts = turnMotor.getMotorVoltage();
steerMotorCurrent = turnMotor.getSupplyCurrent();
turnEncoderAbsolutePosition = OdometryThread.registerSignalInput(turnEncoder.getAbsolutePosition());
turnEncoderVelocityPerSecond = turnEncoder.getVelocity();
turnMotorAppliedVolts = turnMotor.getMotorVoltage();
turnMotorCurrent = turnMotor.getSupplyCurrent();

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

driveMotor.setPosition(0.0);
Expand All @@ -128,30 +128,30 @@ public ModuleIOTalonFX(ModuleConfig moduleConfig) {
public void updateInputs(ModuleIOInputs inputs) {
inputs.hardwareConnected = BaseStatusSignal.refreshAll(periodicallyRefreshedSignals).isOK();

inputs.odometryDriveWheelRevolutions = driveEncoderUngearedRevolutions.stream()
inputs.odometryDriveWheel = driveEncoderUngeared.stream()
.mapToDouble(value -> value / ModuleConstants.DRIVE_GEAR_RATIO)
.toArray();
driveEncoderUngearedRevolutions.clear();
if (inputs.odometryDriveWheelRevolutions.length > 0)
inputs.driveWheelFinalRevolutions = inputs.odometryDriveWheelRevolutions[inputs.odometryDriveWheelRevolutions.length-1];
driveEncoderUngeared.clear();
if (inputs.odometryDriveWheel.length > 0)
inputs.driveWheelFinal = inputs.odometryDriveWheel[inputs.odometryDriveWheel.length-1];

inputs.odometrySteerPositions = steerEncoderAbsolutePositionRevolutions.stream()
.map(this::getSteerFacingFromCANCoderReading)
inputs.odometryturnPositions = turnEncoderAbsolutePosition.stream()
.map(this::getturnFacingFromCANCoderReading)
.toArray(Rotation2d[]::new);
steerEncoderAbsolutePositionRevolutions.clear();
if (inputs.odometrySteerPositions.length > 0)
inputs.turnRotation = inputs.odometrySteerPositions[inputs.odometrySteerPositions.length-1];
turnEncoderAbsolutePosition.clear();
if (inputs.odometryturnPositions.length > 0)
inputs.turnRotation = inputs.odometryturnPositions[inputs.odometryturnPositions.length-1];

inputs.driveWheelFinalVelocityRevolutionsPerSec = driveEncoderUngearedRevolutionsPerSecond.getValueAsDouble() / ModuleConstants.DRIVE_GEAR_RATIO;
inputs.driveWheelFinalVelocityPerSec = driveEncoderUngearedPerSecond.getValueAsDouble() / ModuleConstants.DRIVE_GEAR_RATIO;
inputs.driveMotorAppliedVolts = driveMotorAppliedVoltage.getValueAsDouble();
inputs.driveMotorCurrentAmps = driveMotorCurrent.getValueAsDouble();

inputs.steerVelocityRadPerSec = Units.rotationsToRadians(steerEncoderVelocityRevolutionsPerSecond.getValueAsDouble());
inputs.steerMotorAppliedVolts = steerMotorAppliedVolts.getValueAsDouble();
inputs.steerMotorCurrentAmps = steerMotorCurrent.getValueAsDouble();
inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnEncoderVelocityPerSecond.getValueAsDouble());
inputs.turnMotorAppliedVolts = turnMotorAppliedVolts.getValueAsDouble();
inputs.turnMotorCurrentAmps = turnMotorCurrent.getValueAsDouble();
}

private Rotation2d getSteerFacingFromCANCoderReading(double canCoderReadingRotations) {
private Rotation2d getturnFacingFromCANCoderReading(double canCoderReadingRotations) {
return Rotation2d.fromRotations(canCoderReadingRotations);
}

Expand All @@ -172,9 +172,10 @@ public void setTurnSpeed(double powerPercent) {
*/
@Override
public void setDesiredState(SwerveModuleState desiredState) {
double turnRotations = getTurnRotations();
// Optimize the reference state to avoid spinning further than 90 degrees
SwerveModuleState optimizedDesiredState =
SwerveModuleState.optimize(desiredState, desiredState.angle);
SwerveModuleState.optimize(desiredState, Rotation2d.fromRotations(turnRotations));

if (Math.abs(optimizedDesiredState.speedMetersPerSecond) < 0.01) {
driveMotor.set(0);
Expand All @@ -198,6 +199,11 @@ public void setDriveBrake(boolean enable) {
driveMotor.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast);
}

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

@Override
public void setTurnBrake(boolean enable) {
turnMotor.setNeutralMode(enable ? NeutralModeValue.Brake : NeutralModeValue.Coast);
Expand Down

0 comments on commit 96df027

Please sign in to comment.