Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Swerve #8

Closed
wants to merge 16 commits into from
Prev Previous commit
Next Next commit
some bug logging
  • Loading branch information
ConradSkelly committed Jan 30, 2025
commit 663dd0fb311918ac268146db4e44fa44470de60d
14 changes: 14 additions & 0 deletions src/main/java/frc/lib/util/Wheel.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
package frc.lib.util;

import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.units.Measure;

public class Wheel {
public final Distance circumfrence;
public final Distance radius;

public Wheel(Distance radius) {
this.radius = radius;
circumfrence = radius.times(2 * Math.PI);
}
}
46 changes: 26 additions & 20 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
import edu.wpi.first.units.measure.Time;
import frc.lib.util.FeedforwardGains;
import frc.lib.util.PIDGains;

import frc.lib.util.Wheel;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.measure.AngularVelocity;
Expand Down Expand Up @@ -68,36 +68,42 @@ public static class EndEffectorConstants {
}
public static class RobotConstants {

public static final Distance WIDTH = Inches.of(26.0);
public static final Distance LENGTH = Inches.of(26.0);

public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(
new Translation2d(RobotConstants.LENGTH.divide(2), RobotConstants.WIDTH.divide(2)),
new Translation2d(RobotConstants.LENGTH.divide(2), RobotConstants.WIDTH.divide(-2)),
new Translation2d(RobotConstants.LENGTH.divide(-2), RobotConstants.WIDTH.divide(2)),
new Translation2d(RobotConstants.LENGTH.divide(-2), RobotConstants.WIDTH.divide(-2))
);
public static final Distance WIDTH = Inches.of(23.5);
public static final Distance LENGTH = Inches.of(23.5);

public static final Time ROBOT_CLOCK_SPEED = Time.ofBaseUnits(20, Units.Milliseconds);

}

public static class SwerveConstants {
public static final PIDGains TURN_PID = new PIDGains.Builder().kp(0.2).build();
public static final PIDGains DRIVE_PID = new PIDGains.Builder().kp(0.1).build();
public static final FeedforwardGains TURN_FEEDFORWARD = new FeedforwardGains .Builder().kv(0.2).build();
public static final FeedforwardGains DRIVE_FEEDFORWARD = new FeedforwardGains .Builder().kv(0.26).build();
public static final Current TURN_CURRENT_LIMIT = Current.ofBaseUnits(20, Units.Amps);
public static final Current DRIVE_CURRENT_LIMIT = Current.ofBaseUnits(211, Units.Amps);
public static final double DRIVE_CONVERSION_FACTOR = 1;
public static final double TURN_CONVERSION_FACTOR = 1;

public static final double DEADBAND = 0.05;
// to do make a accleration cap

public static final LinearVelocity MAX_SPEED = MetersPerSecond.of(4.25);

public static final AngularVelocity MAX_ROTATION = RadiansPerSecond.of(2 * Math.PI);

public static final PIDGains TURN_PID = new PIDGains.Builder().kp(0.04).build();
public static final PIDGains DRIVE_PID = new PIDGains.Builder().kp(0.043).build();
public static final FeedforwardGains TURN_FEEDFORWARD = new FeedforwardGains .Builder().kv(0.35).build();
public static final FeedforwardGains DRIVE_FEEDFORWARD = new FeedforwardGains .Builder().kv(0.221).build();
public static final Current TURN_CURRENT_LIMIT = Current.ofBaseUnits(30, Units.Amps);
public static final Current DRIVE_CURRENT_LIMIT = Current.ofBaseUnits(80, Units.Amps);
public static final double DRIVE_CONVERSION_FACTOR = 2 * Math.PI;
public static final double TURN_CONVERSION_FACTOR = 2 * Math.PI;

public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(
new Translation2d(RobotConstants.LENGTH.divide(2), RobotConstants.WIDTH.divide(2)),
new Translation2d(RobotConstants.LENGTH.divide(2), RobotConstants.WIDTH.divide(-2)),
new Translation2d(RobotConstants.LENGTH.divide(-2), RobotConstants.WIDTH.divide(2)),
new Translation2d(RobotConstants.LENGTH.divide(-2), RobotConstants.WIDTH.divide(-2))
);

public static final Wheel SWERVE_WHEEL = new Wheel(Inches.of(1.5));

public static final double SWERVE_GEARING = 5.08;

public static final double DEADBAND = 0.05;

public static final double LEFT_STICK_SCAILING = 2;

public static final double RIGHT_STICK_SCAILING = 3;
Expand Down
90 changes: 62 additions & 28 deletions src/main/java/frc/robot/subsystems/Swerve.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Seconds;

import java.lang.Thread.State;
import java.util.function.DoubleSupplier;

import org.opencv.core.Mat;
Expand All @@ -19,11 +20,13 @@

import edu.wpi.first.epilogue.Logged;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.Kinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.LinearVelocity;
Expand All @@ -36,47 +39,83 @@

@Logged
public class Swerve extends SubsystemBase{

@Logged(name = "modules")
private Pose2d estimatedPosition;
private Rotation2d simHeading;
private Rotation2d gyroAngle;
private SwerveModuleState[] swerveModuleStates;
private SwerveModuleState frontLeft;
private SwerveModuleState frontRight;
private SwerveModuleState backLeft;
private SwerveModuleState backRight;
private SwerveModuleState[] setpointStates;
private final Pigeon2 gyro;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where are the modules? You seem to have a sim setup going on here, just need to convert it over to using the modules to interface with real properly. You can still get the sim working using the modules, you'll just need to make a SwerveDrivePoseEstimator object and pass in the data from your modules, which should be handling sim v.s. real on their own. You've got a solid framework here, just need to connect the parts. I'm happy to help you with this tomorrow if you'd like

private final SwerveModule modules[];
private final SwerveModuleState[] messuredStates;

// private final SwerveDrivePoseEstimator odometry;

public Swerve() {

// odometry = new SwerveDrivePoseEstimator(Constants.SwerveConstants.KINEMATICS, gyroAngle, this.getModulePostition(), estimatedPosition);

modules = new SwerveModule[4];

modules[0] = new SwerveModule(SwerveConstants.FRONT_LEFT_DRIVE_ID, SwerveConstants.FRONT_LEFT_TURN_ID);
modules[1] = new SwerveModule(SwerveConstants.FRONT_RIGHT_DRIVE_ID, SwerveConstants.FRONT_RIGHT_TURN_ID);
modules[2] = new SwerveModule(SwerveConstants.BACK_LEFT_DRIVE_ID, SwerveConstants.BACK_LEFT_TURN_ID);
modules[3] = new SwerveModule(SwerveConstants.BACK_RIGHT_DRIVE_ID, SwerveConstants.BACK_RIGHT_TURN_ID);



gyro = new Pigeon2(SwerveConstants.GYRO_ID);

simHeading = new Rotation2d(0.0);

estimatedPosition = new Pose2d();

swerveModuleStates = new SwerveModuleState[4];
setpointStates = new SwerveModuleState[4];
messuredStates = new SwerveModuleState[4];

frontLeft = new SwerveModuleState();
frontRight = new SwerveModuleState();
backLeft = new SwerveModuleState();
backRight = new SwerveModuleState();
}

@Override
public void periodic() {

modules[0].periodic();
modules[1].periodic();
modules[2].periodic();
modules[3].periodic();

gyroAngle = gyro.getRotation2d();

messuredStates[0] = modules[0].getModuleState();
messuredStates[1] = modules[1].getModuleState();
messuredStates[2] = modules[2].getModuleState();
messuredStates[3] = modules[3].getModuleState();

// estimatedPosition = odometry.update(gyroAngle, this.getModulePostition());
}

@Override
public void simulationPeriodic(){
public void simulationPeriodic() {
gyroAngle = simHeading;
System.out.println("anlge of robot in sim is " + gyroAngle);

modules[0].simulationPeriodic();
modules[1].simulationPeriodic();
modules[2].simulationPeriodic();
modules[3].simulationPeriodic();
}

@Override
public void periodic() {
gyroAngle = gyro.getRotation2d();
public SwerveModulePosition[] getModulePostition(){
return new SwerveModulePosition[] {
modules[0].getPosition(),
modules[1].getPosition(),
modules[2].getPosition(),
modules[3].getPosition()
};
}

private void DriveFiledRelativeBlueScailier(double x, double y, double omega){
// public Pose2d getOdometryPose(){
// return odometry.getEstimatedPosition();
// }

private void DriveFiledRelativeBlueScailier(double x, double y, double omega){

double liniarMagintued = Math.pow(Math.hypot(x, y), SwerveConstants.LEFT_STICK_SCAILING);

Expand All @@ -92,7 +131,6 @@ private void DriveFiledRelativeBlueScailier(double x, double y, double omega){

private void DriveFiledRelativeRedScailier(double x, double y, double omega){


double liniarMagintued = Math.pow(Math.hypot(x, y), SwerveConstants.LEFT_STICK_SCAILING);

Rotation2d directions = new Rotation2d(y, x);
Expand All @@ -107,20 +145,16 @@ private void DriveFiledRelativeRedScailier(double x, double y, double omega){
driveFieldRelative(MetersPerSecond.of(MathUtil.applyDeadband(x, Constants.SwerveConstants.DEADBAND)), MetersPerSecond.of(MathUtil.applyDeadband(-y, Constants.SwerveConstants.DEADBAND)), RadiansPerSecond.of(MathUtil.applyDeadband(-omega, Constants.SwerveConstants.DEADBAND)));
}


private void driveFieldRelative(LinearVelocity x, LinearVelocity y, AngularVelocity omega){
simHeading = simHeading.plus(new Rotation2d(omega.times(Seconds.of(.02))));
estimatedPosition = new Pose2d(estimatedPosition.getX() + x.times(RobotConstants.ROBOT_CLOCK_SPEED).in(Meters), estimatedPosition.getY() + (y.times(RobotConstants.ROBOT_CLOCK_SPEED).in(Meters)), new Rotation2d(estimatedPosition.getRotation().getRadians() + omega.times(Seconds.of(.02)).in(Radians)));
ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(x.times(Seconds.of(.02)).in(Meters), y.times(Seconds.of(.02)).in(Meters), omega.times(Seconds.of(.02)).in(Radians), gyroAngle);
swerveModuleStates = RobotConstants.KINEMATICS.toSwerveModuleStates(speeds);
//front left
frontLeft = swerveModuleStates[0];
//front Right
frontRight = swerveModuleStates[1];
//back Left
backLeft = swerveModuleStates[2];
//back Right
backRight = swerveModuleStates[3];
setpointStates = SwerveConstants.KINEMATICS.toSwerveModuleStates(speeds);

modules[0].setState(setpointStates[0]);
modules[1].setState(setpointStates[1]);
modules[2].setState(setpointStates[2]);
modules[3].setState(setpointStates[3]);
}

public void zeroing(boolean colour){
Expand Down
54 changes: 45 additions & 9 deletions src/main/java/frc/robot/subsystems/SwerveModule.java
Original file line number Diff line number Diff line change
@@ -1,15 +1,16 @@
package frc.robot.subsystems;

import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.Volt;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Volts;

import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkMaxAlternateEncoder;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.SparkBase;
Expand All @@ -19,21 +20,25 @@
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;

import edu.wpi.first.epilogue.Logged;
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.units.AngleUnit;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.units.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.Distance;
import edu.wpi.first.units.measure.LinearVelocity;
import edu.wpi.first.units.measure.MutCurrent;
import edu.wpi.first.units.measure.Voltage;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants.RobotConstants;
import frc.robot.Constants.SwerveConstants;

public class SwerveModule extends SubsystemBase{
@Logged
public class SwerveModule {

private final SparkFlex driveMotor;
private final SparkMax turnMotor;
Expand All @@ -54,8 +59,17 @@ public class SwerveModule extends SubsystemBase{
private Current driveCurrent;
private SparkClosedLoopController turnController;
private SparkClosedLoopController driveController;

private Distance drivePosition;
private final TrapezoidProfile turnProfile;
private final TrapezoidProfile.State turnGoalState;
private final TrapezoidProfile.State turnSetpointState;

public SwerveModule(int driveID, int turnID){

turnProfile = new TrapezoidProfile(new TrapezoidProfile.Constraints(SwerveConstants.MAX_SPEED.in(MetersPerSecond), SwerveConstants.MAX_ROTATION.in(RadiansPerSecond)));

turnGoalState = new TrapezoidProfile.State(0,0);
turnSetpointState = new TrapezoidProfile.State(0,0);

turnMotor = new SparkMax(turnID, MotorType.kBrushless);
driveMotor = new SparkFlex(driveID, MotorType.kBrushless);
Expand Down Expand Up @@ -88,6 +102,7 @@ public SwerveModule(int driveID, int turnID){
turnSetpoint = Radians.of(0);
driveSetpoint = LinearVelocity.ofBaseUnits(0, Units.MetersPerSecond);
turnPosition = Radians.of(turnEncoder.getPosition());
drivePosition = Meters.of(driveEncoder.getPosition());
driveVelocity = LinearVelocity.ofBaseUnits(driveEncoder.getVelocity(), Units.MetersPerSecond);
turnVelocity = AngularVelocity.ofBaseUnits(turnEncoder.getVelocity(), Units.RadiansPerSecond);
driveVoltage = Voltage.ofBaseUnits(driveMotor.getBusVoltage() * driveMotor.getAppliedOutput(), Volts);
Expand All @@ -99,14 +114,35 @@ public SwerveModule(int driveID, int turnID){
public void setState(SwerveModuleState swerveModuleState){
turnController.setReference(swerveModuleState.angle.getRadians(), SparkBase.ControlType.kPosition);
driveController.setReference(swerveModuleState.speedMetersPerSecond, SparkBase.ControlType.kVelocity);

driveSetpoint = MetersPerSecond.of(swerveModuleState.speedMetersPerSecond);
turnSetpoint = Radians.of(swerveModuleState.angle.getRadians());

// turnGoalState = new TrapezoidProfile(optimizedState.angle.getRadians())
}

public void periodic() {
driveCurrent = Current.ofBaseUnits(driveMotor.getOutputCurrent(), Units.Amps);
turnCurrent = Current.ofBaseUnits(turnMotor.getOutputCurrent(), Units.Amps);
driveVoltage = Voltage.ofBaseUnits(driveMotor.getBusVoltage() * driveMotor.getAppliedOutput(), Volts);
turnVoltage = Voltage.ofBaseUnits(turnMotor.getBusVoltage() * turnMotor.getAppliedOutput(), Volts);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ideally you have a separate periodic for updating logged variables, setpoints excluded since they do need access to the setpoint state

}
drivePosition = Meters.of(driveEncoder.getPosition());
turnPosition = Radians.of(turnEncoder.getPosition());
driveVelocity = LinearVelocity.ofBaseUnits(driveEncoder.getVelocity(), Units.MetersPerSecond);
turnVelocity = AngularVelocity.ofBaseUnits(turnEncoder.getVelocity(), Units.RadiansPerSecond);
}

public void simulationPeriodic() {
drivePosition = drivePosition.plus(driveSetpoint.times(RobotConstants.ROBOT_CLOCK_SPEED));
turnPosition = turnSetpoint;
driveVelocity = driveSetpoint;
}

public SwerveModuleState getModuleState(){
return new SwerveModuleState(driveEncoder.getVelocity(), new Rotation2d(turnEncoder.getPosition()));
return new SwerveModuleState(driveVelocity, new Rotation2d(turnPosition));
}

public SwerveModulePosition getPosition(){
return new SwerveModulePosition(drivePosition, new Rotation2d(turnPosition));
}
}
Loading