Skip to content

Commit

Permalink
Add Triggers/Sim
Browse files Browse the repository at this point in the history
  • Loading branch information
legoguy1000 committed Oct 3, 2024
1 parent 7aa4096 commit 38249bb
Show file tree
Hide file tree
Showing 36 changed files with 937 additions and 352 deletions.
29 changes: 11 additions & 18 deletions src/main/java/frc/lib/util/swerve/SwerveModule.java
Original file line number Diff line number Diff line change
@@ -1,13 +1,11 @@
package frc.lib.util.swerve;

import org.littletonrobotics.junction.Logger;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VelocityVoltage;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.lib.math.Conversions;
import frc.robot.Constants;
import frc.robot.Robot;
Expand All @@ -26,12 +24,6 @@ public class SwerveModule {
private final SimpleMotorFeedforward driveFeedForward = new SimpleMotorFeedforward(
Constants.Swerve.driveKS, Constants.Swerve.driveKV, Constants.Swerve.driveKA);

/* drive motor control requests */
private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0);
private final VelocityVoltage driveVelocity = new VelocityVoltage(0);

/* angle motor control requests */
private final PositionVoltage anglePosition = new PositionVoltage(0);

/**
* Swerve Module
Expand Down Expand Up @@ -75,8 +67,12 @@ public void periodic() {
*/
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop) {
desiredState = SwerveModuleState.optimize(desiredState, getState().angle);
io.setAngleMotor(anglePosition.withPosition(desiredState.angle.getRotations()));
io.setAngleMotor(desiredState.angle.getRotations());
setSpeed(desiredState, isOpenLoop);
SmartDashboard.putNumber("desired state speed/" + moduleNumber,
desiredState.speedMetersPerSecond);
SmartDashboard.putNumber("desired state angle/" + moduleNumber,
desiredState.angle.getDegrees());
}

/**
Expand All @@ -87,16 +83,13 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop)
*/
private void setSpeed(SwerveModuleState desiredState, boolean isOpenLoop) {
if (isOpenLoop) {
driveDutyCycle.Output = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed;
io.setDriveMotor(driveDutyCycle);
inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output;
double power = desiredState.speedMetersPerSecond / Constants.Swerve.maxSpeed;
io.setDriveMotorPower(power);
} else {
driveVelocity.Velocity = Conversions.metersPerSecondToRotationPerSecond(
double driveRPM = Conversions.metersPerSecondToRotationPerSecond(
desiredState.speedMetersPerSecond, Constants.Swerve.wheelCircumference);
driveVelocity.FeedForward =
driveFeedForward.calculate(desiredState.speedMetersPerSecond);
io.setDriveMotor(driveVelocity);
inputs.driveMotorSelectedSensorVelocity = driveDutyCycle.Output;
double driveFF = driveFeedForward.calculate(desiredState.speedMetersPerSecond);
io.setDriveMotor(driveRPM, driveFF);
}
}

Expand Down
7 changes: 4 additions & 3 deletions src/main/java/frc/lib/util/swerve/SwerveModuleIO.java
Original file line number Diff line number Diff line change
@@ -1,7 +1,6 @@
package frc.lib.util.swerve;

import org.littletonrobotics.junction.AutoLog;
import com.ctre.phoenix6.controls.ControlRequest;

/** IO Class for SwerveModule */
public interface SwerveModuleIO {
Expand All @@ -18,9 +17,11 @@ public static class SwerveModuleInputs {

public default void updateInputs(SwerveModuleInputs inputs) {}

public default void setDriveMotor(ControlRequest request) {}
public default void setDriveMotor(double rpm, double feedforward) {}

public default void setAngleMotor(ControlRequest request) {}
public default void setDriveMotorPower(double power) {}

public default void setAngleMotor(double angle) {}

public default void setAngleSelectedSensorPosition(double angle) {}

Expand Down
21 changes: 16 additions & 5 deletions src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,9 @@
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.ControlRequest;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.math.geometry.Rotation2d;
Expand All @@ -27,6 +29,13 @@ public class SwerveModuleReal implements SwerveModuleIO {
private StatusSignal<Double> angleMotorSelectedPosition;
private StatusSignal<Double> absolutePositionAngleEncoder;

/* drive motor control requests */
private final DutyCycleOut driveDutyCycle = new DutyCycleOut(0);
private final VelocityVoltage driveVelocity = new VelocityVoltage(0);

/* angle motor control requests */
private final PositionVoltage anglePosition = new PositionVoltage(0);

/** Instantiating motors and Encoders */
public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID,
Rotation2d angleOffset) {
Expand Down Expand Up @@ -117,13 +126,15 @@ private void configAngleEncoder() {
}

@Override
public void setAngleMotor(ControlRequest request) {
mAngleMotor.setControl(request);
public void setAngleMotor(double angle) {
mAngleMotor.setControl(anglePosition.withPosition(angle));
}

@Override
public void setDriveMotor(ControlRequest request) {
mDriveMotor.setControl(request);
public void setDriveMotor(double rpm, double feedforward) {
driveVelocity.FeedForward = feedforward;
driveVelocity.Velocity = rpm;
mDriveMotor.setControl(driveVelocity);
}

@Override
Expand Down
91 changes: 91 additions & 0 deletions src/main/java/frc/lib/util/swerve/SwerveModuleSim.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,91 @@
package frc.lib.util.swerve;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.simulation.FlywheelSim;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Constants;

public class SwerveModuleSim implements SwerveModuleIO {
public int moduleNumber;
private FlywheelSim driveSim =
new FlywheelSim(DCMotor.getFalcon500(1), Constants.Swerve.driveGearRatio, 0.025);
private FlywheelSim turnSim =
new FlywheelSim(DCMotor.getFalcon500(1), Constants.Swerve.angleGearRatio, 0.004);

private final Rotation2d turnAbsoluteInitPosition =
new Rotation2d(Math.random() * 2.0 * Math.PI);

private double turnRelativePositionRad = 0.0;
private double turnAbsolutePositionRad = Math.random() * 2.0 * Math.PI;
private double driveAppliedVolts = 0.0;
private double turnAppliedVolts = 0.0;

private final PIDController driveFeedback;
private final PIDController turnFeedback;

public SwerveModuleSim(int moduleNumber) {
this.moduleNumber = moduleNumber;
System.out.println("[Init] Creating ServeModuleSim");
driveFeedback = new PIDController(Constants.Swerve.driveKP, Constants.Swerve.driveKI,
Constants.Swerve.driveKD);
turnFeedback = new PIDController(Constants.Swerve.angleKP, Constants.Swerve.angleKI,
Constants.Swerve.angleKD);
turnFeedback.enableContinuousInput(-Math.PI, Math.PI);
}

public void updateInputs(SwerveModuleInputs inputs) {
driveSim.update(Constants.loopPeriodSecs);
turnSim.update(Constants.loopPeriodSecs);

double angleDiffRad = turnSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs;
turnRelativePositionRad += angleDiffRad;
turnAbsolutePositionRad += angleDiffRad;
while (turnAbsolutePositionRad < 0) {
turnAbsolutePositionRad += 2.0 * Math.PI;
}
while (turnAbsolutePositionRad > 2.0 * Math.PI) {
turnAbsolutePositionRad -= 2.0 * Math.PI;
}

inputs.driveMotorSelectedPosition = Rotation2d
.fromRadians(inputs.driveMotorSelectedPosition
+ (driveSim.getAngularVelocityRadPerSec() * Constants.loopPeriodSecs))
.getRotations();
inputs.driveMotorSelectedSensorVelocity =
Rotation2d.fromRadians(driveSim.getAngularVelocityRadPerSec()).getRotations();

inputs.angleMotorSelectedPosition =
Rotation2d.fromRadians(turnAbsolutePositionRad).getRotations();
inputs.absolutePositionAngleEncoder = 0;
}

public void setDriveMotor(double rpm, double feedforward) {
driveFeedback.setSetpoint(rpm);
double volts =
driveFeedback.calculate(driveSim.getAngularVelocityRadPerSec() / (2 * Math.PI))
+ feedforward;
SmartDashboard.putNumber("rpm/" + moduleNumber, rpm);
SmartDashboard.putNumber("ff/" + moduleNumber, feedforward);
SmartDashboard.putNumber("volts/" + moduleNumber, volts);
setDriveVoltage(volts);
}

public void setAngleMotor(double angle) {
turnFeedback.setSetpoint(angle);
double volts = turnFeedback.calculate(turnAbsolutePositionRad / (2 * Math.PI));
setTurnVoltage(volts);
}

public void setDriveVoltage(double volts) {
driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
driveSim.setInputVoltage(driveAppliedVolts);
}

public void setTurnVoltage(double volts) {
turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0);
turnSim.setInputVoltage(turnAppliedVolts);
}
}
1 change: 1 addition & 0 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
* Constants file.
*/
public final class Constants {
public static final double loopPeriodSecs = 0.02;

/**
* Stick Deadband
Expand Down
10 changes: 10 additions & 0 deletions src/main/java/frc/robot/OperatorState.java
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
package frc.robot;

import edu.wpi.first.wpilibj2.command.button.Trigger;

/** Singleton tracker for operator state. */
public class OperatorState {

Expand Down Expand Up @@ -38,6 +40,14 @@ public State decrement() {
private static State currentState = State.kShootWhileMove;
private static boolean manualMode = false;

public static Trigger isManualMode = new Trigger(() -> manualModeEnabled());
public static Trigger isAmpMode = new Trigger(() -> getCurrentState() == State.kAmp);
public static Trigger isSpeakerMode = new Trigger(() -> getCurrentState() == State.kSpeaker);
public static Trigger isShootWhileMoveMode =
new Trigger(() -> getCurrentState() == State.kShootWhileMove);
public static Trigger isPostMode = new Trigger(() -> getCurrentState() == State.kPost);
public static Trigger isClimbMode = new Trigger(() -> getCurrentState() == State.kClimb);

/** Get whether or not operator should be able to control wrist and elevator manually. */
public static boolean manualModeEnabled() {
return manualMode;
Expand Down
Loading

0 comments on commit 38249bb

Please sign in to comment.