Skip to content

Commit

Permalink
Merge branch 'main' into se/7-10
Browse files Browse the repository at this point in the history
  • Loading branch information
@IanShiii
IanShiii authored Jul 11, 2024
2 parents 1917fb4 + 2ca5efa commit e0da655
Show file tree
Hide file tree
Showing 5 changed files with 222 additions and 101 deletions.
67 changes: 34 additions & 33 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
View file
Original file line number Diff line number Diff line change
@@ -1,15 +1,13 @@
package com.stuypulse.robot.constants;

import com.pathplanner.lib.path.PathConstraints;
import com.pathplanner.lib.util.PIDConstants;

import com.stuypulse.robot.util.ShooterSpeeds;
import com.stuypulse.stuylib.network.SmartBoolean;

import com.stuypulse.stuylib.network.SmartNumber;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;

/*-
Expand Down Expand Up @@ -145,49 +143,51 @@ public interface FF {
}

public interface Swerve {
// between wheel centers
double WIDTH = Units.inchesToMeters(20.75);
double LENGTH = Units.inchesToMeters(20.75);
double WIDTH = Units.inchesToMeters(32.5);
double LENGTH = Units.inchesToMeters(27.375);
double CENTER_TO_INTAKE_FRONT = Units.inchesToMeters(13.0);

double MAX_MODULE_SPEED = 4.9;
double MAX_MODULE_ACCEL = 15.0;

double MAX_LINEAR_VELOCITY = 15.0;
double MAX_ANGULAR_VELOCITY = 12.0;

double MODULE_VELOCITY_DEADBAND = 0.05;

SmartNumber ALIGN_OMEGA_DEADBAND = new SmartNumber("Swerve/Align Omega Deadband", 0.05); // TODO: make 0.25 and test

public interface Assist {
SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); //change
// public interface Assist {
// SmartNumber ALIGN_MIN_SPEAKER_DIST = new SmartNumber("SwerveAssist/Minimum Distance to Speaker", 4); //change

double BUZZ_INTENSITY = 1;
// double BUZZ_INTENSITY = 1;

// angle PID
SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0);
SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0);
SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0);
// // angle PID
// SmartNumber kP = new SmartNumber("SwerveAssist/kP", 6.0);
// SmartNumber kI = new SmartNumber("SwerveAssist/kI", 0.0);
// SmartNumber kD = new SmartNumber("SwerveAssist/kD", 0.0);

double ANGLE_DERIV_RC = 0.05;
double REDUCED_FF_DIST = 0.75;
}
// double ANGLE_DERIV_RC = 0.05;
// double REDUCED_FF_DIST = 0.75;
// }

// TODO: Tune these values
public interface Motion {
SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity", 3.0);
SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration", 4.0);
SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity", Units.degreesToRadians(540));
SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration", Units.degreesToRadians(720));

PathConstraints DEFAULT_CONSTRAINTS =
new PathConstraints(
MAX_VELOCITY.get(),
MAX_ACCELERATION.get(),
MAX_ANGULAR_VELOCITY.get(),
MAX_ANGULAR_ACCELERATION.get());

PIDConstants XY = new PIDConstants(2.5, 0, 0.02);
PIDConstants THETA = new PIDConstants(4, 0, 0.1);
}
// // TODO: Tune these values
// public interface Motion {
// SmartNumber MAX_VELOCITY = new SmartNumber("Swerve/Motion/Max Velocity", 3.0);
// SmartNumber MAX_ACCELERATION = new SmartNumber("Swerve/Motion/Max Acceleration", 4.0);
// SmartNumber MAX_ANGULAR_VELOCITY = new SmartNumber("Swerve/Motion/Max Angular Velocity", Units.degreesToRadians(540));
// SmartNumber MAX_ANGULAR_ACCELERATION = new SmartNumber("Swerve/Motion/Max Angular Acceleration", Units.degreesToRadians(720));

// PathConstraints DEFAULT_CONSTRAINTS =
// new PathConstraints(
// MAX_VELOCITY.get(),
// MAX_ACCELERATION.get(),
// MAX_ANGULAR_VELOCITY.get(),
// MAX_ANGULAR_ACCELERATION.get());

// PIDConstants XY = new PIDConstants(2.5, 0, 0.02);
// PIDConstants THETA = new PIDConstants(4, 0, 0.1);
// }

public interface Encoder {
public interface Drive {
Expand All @@ -213,6 +213,7 @@ public interface Turn {
SmartNumber kS = new SmartNumber("Swerve/Turn/FF/kS", 0.25582);
SmartNumber kV = new SmartNumber("Swerve/Turn/FF/kV", 0.00205);
SmartNumber kA = new SmartNumber("Swerve/Turn/FF/kA", 0.00020123);
double kT = 1.0 / DCMotor.getKrakenX60Foc(1).KtNMPerAmp;

SmartBoolean INVERTED = new SmartBoolean("Swerve/Turn/INVERTED", true);

Expand Down
83 changes: 36 additions & 47 deletions src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,9 @@
import com.stuypulse.robot.constants.Settings.Swerve.BackRight;
import com.stuypulse.robot.constants.Settings.Swerve.FrontLeft;
import com.stuypulse.robot.constants.Settings.Swerve.FrontRight;

import com.stuypulse.robot.subsystems.odometry.Odometry;
import com.stuypulse.robot.subsystems.swerve.controllers.DriveController;
import com.stuypulse.robot.subsystems.swerve.modules.KrakenSwerveModule;
import com.stuypulse.robot.subsystems.swerve.modules.SwerveModule;

Expand All @@ -27,46 +29,11 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;

import com.ctre.phoenix6.BaseStatusSignal;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.Pigeon2Configuration;
import com.ctre.phoenix6.hardware.Pigeon2;

/*
* Fields:
* - swerveModules: AbstractSwerveModule...
* - kinematics: SwerveDriveKinematics
* - gyro: AHRS
* - modules2D: FieldObject2D[]
*
* Tasks:
* - drive
* - followDrive
* - trackingDrive
* - aligning (Trap, Speaker, Amp)
* - GTADrive (shooting while driving)
*
* Methods:
* + singleton
* + initFieldObject(Field2D field): void
* + getModulePositions(): Translation2D[]
* + getModuleStates(): SwerveModuleStates[]
* + getModuleOffsets(): Rotation2D[]
* + getChassisSpeeds(): ChassisSpeed[]
* + setModuleStates(SwerveModuleState... states): void
* + setChassisSpeed(ChassisSpeed): void
* + drive(double, Rotation2D)
* + stop(double, Rotation2D)
*
* SwerveDrive.java
* Methods:
* - getGyroAngle(): Rotation2D
* - getGyroYaw(): Rotation2D
* - getGyroPitch(): Rotation2D
* - getGyroRoll(): Rotation2D
* - getKinematics(): SwerveDriveKinematics
* + periodic(): void
*
*
*/
public class SwerveDrive extends SubsystemBase {

private static final SwerveDrive instance;
Expand All @@ -89,8 +56,16 @@ public static SwerveDrive getInstance() {
private final Pigeon2 gyro;
private final FieldObject2d[] modules2D;

private ChassisSpeeds desiredSpeeds = new ChassisSpeeds();
// TODO: Add SSG
private final DriveController driveController;

private final StructArrayPublisher<SwerveModuleState> statesPub;

// Status Signals
private final StatusSignal<Double> yaw;
private final StatusSignal<Double> yawVelocity;

/**
* Creates a new Swerve Drive using the provided modules
*
Expand All @@ -99,11 +74,21 @@ public static SwerveDrive getInstance() {
protected SwerveDrive(SwerveModule... modules) {
this.modules = modules;
kinematics = new SwerveDriveKinematics(getModuleOffsets());
gyro = new Pigeon2(Ports.Gyro.PIGEON2, "*");
modules2D = new FieldObject2d[modules.length];
gyro = new Pigeon2(Ports.Gyro.PIGEON2, "*");

driveController = new DriveController();

statesPub = NetworkTableInstance.getDefault()
.getStructArrayTopic("SmartDashboard/Swerve/States", SwerveModuleState.struct).publish();

yaw = gyro.getYaw();
yawVelocity = gyro.getAngularVelocityZWorld();
gyro.getConfigurator().apply(new Pigeon2Configuration());
gyro.getConfigurator().setYaw(0.0);
yaw.setUpdateFrequency(250);
yawVelocity.setUpdateFrequency(100);
gyro.optimizeBusUtilization();
}

public void initFieldObject(Field2d field) {
Expand Down Expand Up @@ -133,12 +118,14 @@ public SwerveModulePosition[] getModulePositions() {
return offsets;
}

// TODO: Offset should be to the center of wheels, not corner of robot
public Translation2d[] getModuleOffsets() {
Translation2d[] offsets = new Translation2d[modules.length];
for (int i = 0; i < modules.length; i++) {
offsets[i] = modules[i].getModuleOffset();
}
return offsets;
return new Translation2d[] {
new Translation2d(Swerve.WIDTH / 2.0, Swerve.LENGTH / 2.0),
new Translation2d(-Swerve.WIDTH / 2.0, Swerve.LENGTH / 2.0),
new Translation2d(-Swerve.WIDTH / 2.0, -Swerve.LENGTH / 2.0),
new Translation2d(Swerve.WIDTH / 2.0, -Swerve.LENGTH / 2.0)
};
}

public ChassisSpeeds getChassisSpeeds() {
Expand Down Expand Up @@ -213,20 +200,22 @@ public Rotation2d getGyroAngle() {
}

public StatusSignal<Double> getGyroYaw() {
return gyro.getYaw();
return yaw;
}

public StatusSignal<Double> getGyroYawVelocity() {
return gyro.getAngularVelocityZWorld();
return yawVelocity;
}

@Override
public void periodic() {
statesPub.set(getModuleStates());

BaseStatusSignal.refreshAll(yaw, yawVelocity);

SmartDashboard.putNumber("Swerve/Gyro/Angle (deg)", getGyroAngle().getDegrees());
SmartDashboard.putNumber("Swerve/Gyro/Yaw (deg)", getGyroYaw().getValueAsDouble());
SmartDashboard.putNumber("Swerve/Gyro/Yaw Velocity (deg)", getGyroYawVelocity().getValueAsDouble());
SmartDashboard.putNumber("Swerve/Gyro/Yaw (deg)", yaw.getValueAsDouble());
SmartDashboard.putNumber("Swerve/Gyro/Yaw Velocity (deg)", yawVelocity.getValueAsDouble());

SmartDashboard.putNumber("Swerve/X Acceleration (Gs)", gyro.getAccelerationX().getValueAsDouble());
SmartDashboard.putNumber("Swerve/Y Acceleration (Gs)", gyro.getAccelerationY().getValueAsDouble());
Expand Down
68 changes: 68 additions & 0 deletions src/main/java/com/stuypulse/robot/subsystems/swerve/controllers/DriveController.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
package com.stuypulse.robot.subsystems.swerve.controllers;

import com.stuypulse.robot.constants.Settings.Swerve;
import com.stuypulse.stuylib.network.SmartNumber;

import edu.wpi.first.math.MathUtil;
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.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;

public class DriveController {
private static final SmartNumber controllerDeadband =
new SmartNumber("Controller/DriveController/Deadband", 0.1);
private static final SmartNumber maxAngularVelocityScalar =
new SmartNumber("Controller/DriveController/MaxAngularVelocityScalar", 0.65);


private double controllerX = 0;
private double controllerY = 0;
private double controllerOmega = 0;

/**
* Accepts new drive input from joysticks.
*
* @param x Desired x velocity scalar, -1..1
* @param y Desired y velocity scalar, -1..1
* @param omega Desired omega velocity scalar, -1..1
*/
public void acceptDriveInput(double x, double y, double omega) {
controllerX = x;
controllerY = y;
controllerOmega = omega;
}

public ChassisSpeeds update() {
Translation2d linearVelocity = calcLinearVelocity(controllerX, controllerY);
double omega = MathUtil.applyDeadband(controllerOmega, controllerDeadband.get());
omega = Math.copySign(omega * omega, omega);

final double maxLinearVelocity = Swerve.MAX_LINEAR_VELOCITY;
final double maxAngularVelocity =
Swerve.MAX_ANGULAR_VELOCITY * (maxAngularVelocityScalar.get());

return new ChassisSpeeds(
linearVelocity.getX() * maxLinearVelocity,
linearVelocity.getY() * maxLinearVelocity,
omega * maxAngularVelocity);
}


public static Translation2d calcLinearVelocity(double x, double y) {
// Apply deadband
double linearMagnitude = MathUtil.applyDeadband(Math.hypot(x, y), controllerDeadband.get());
Rotation2d linearDirection = new Rotation2d(x, y);

// Square magnitude
linearMagnitude = linearMagnitude * linearMagnitude;

// Calcaulate new linear velocity
Translation2d linearVelocity =
new Pose2d(new Translation2d(), linearDirection)
.transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
.getTranslation();
return linearVelocity;
}
}
Loading

0 comments on commit e0da655

Please sign in to comment.