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.

Sign up for GitHub

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

Code quality and radians for swerve #22

Merged
merged 11 commits into from
Sep 14, 2023
Merged

Code quality and radians for swerve #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
615aa61
starting of, module is mostly radians now
an-prata May 27, 2023
3994193
remove newline
an-prata May 28, 2023
993242d
fix a couple mistakes
an-prata May 28, 2023
7351923
clean up `SwerveDrive`
an-prata May 28, 2023
7e77786
add and use angle class
an-prata May 28, 2023
decab73
have modules id by position rather than id
an-prata May 28, 2023
142b0e9
rename `SwerveMode` variants to match naming convention
an-prata May 29, 2023
1eeec43
see "The Tau Manifesto" for why Pi is a mistake
an-prata Sep 1, 2023
c3bc40c
rework rock mode interface
an-prata Sep 2, 2023
a7459d0
fix null ref
an-prata Sep 8, 2023
6dc703a
bug fixes
an-prata Sep 8, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 10 additions & 10 deletions src/main/java/frc/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,7 @@ public void autonomousPeriodic() {
@Override
public void teleopInit() {
controlMode = ControlMode.TELEOPERATED;
SwerveDrive.setMode(SwerveMode.Headless);
SwerveDrive.setMode(SwerveMode.HEADLESS);
ElevFourbar.init();
Intake.init();
ElevFourbar.enableSafetyMode(false);
Expand All @@ -170,14 +170,14 @@ public void teleopPeriodic() {
}

SwerveDrive.run(x, y, controllerOne.getRightX(), controllerOne.getPOV());
SwerveDrive.setRockMode(controllerOne.getRightTriggerAxis() > 0.25);
SwerveDrive.conditionalTempMode(SwerveMode.ROCK, controllerOne.getRightTriggerAxis() > 0.25);

// Left stick changes between headless and relative control modes.
if (controllerOne.getLeftStickButtonReleased()) {
if (SwerveDrive.getMode() == SwerveMode.Headless) {
SwerveDrive.setMode(SwerveMode.Relative);
if (SwerveDrive.getMode() == SwerveMode.HEADLESS) {
SwerveDrive.setMode(SwerveMode.RELATIVE);
} else {
SwerveDrive.setMode(SwerveMode.Headless);
SwerveDrive.setMode(SwerveMode.HEADLESS);
}
}

Expand Down Expand Up @@ -225,7 +225,7 @@ public void disabledPeriodic() {
@Override
public void testInit() {
controlMode = ControlMode.SAFETY;
SwerveDrive.setMode(SwerveMode.Headless);
SwerveDrive.setMode(SwerveMode.HEADLESS);
ElevFourbar.init();
Intake.init();
ElevFourbar.enableSafetyMode(true);
Expand All @@ -247,14 +247,14 @@ public void testPeriodic() {

//SCALES DOWN DRIVE SPEED FOR
SwerveDrive.run(x, y, controllerOne.getRightX(), controllerOne.getPOV());
SwerveDrive.setRockMode(controllerOne.getRightTriggerAxis() > 0.25);
SwerveDrive.conditionalTempMode(SwerveMode.ROCK, controllerOne.getRightTriggerAxis() > 0.25);

// Left stick changes between headless and relative control modes.
if (controllerOne.getLeftStickButtonReleased()) {
if (SwerveDrive.getMode() == SwerveMode.Headless) {
SwerveDrive.setMode(SwerveMode.Relative);
if (SwerveDrive.getMode() == SwerveMode.HEADLESS) {
SwerveDrive.setMode(SwerveMode.RELATIVE);
} else {
SwerveDrive.setMode(SwerveMode.Headless);
SwerveDrive.setMode(SwerveMode.HEADLESS);
}
}

Expand Down
39 changes: 39 additions & 0 deletions src/main/java/frc/robot/subsystems/Angle.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.subsystems;

public final class Angle {
public static final double TAU = Math.PI * 2;

private double dec = 0.0;

public double radians() {
return dec * TAU;
}

public double degrees() {
return dec * 360.0;
}

public Angle setRadians(double radians) {
dec = radians / TAU;
return this;
}

public Angle setDegrees(double degrees) {
dec = degrees / 360.0;
return this;
}

/**
* Creates a new identical angle so that modification of the original will not
* effect the value returned from this function.
*/
public Angle clone() {
var ang = new Angle();
ang.dec = this.dec;
return ang;
}
}
9 changes: 7 additions & 2 deletions src/main/java/frc/robot/subsystems/SwerveMode.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,16 @@ public enum SwerveMode {
* Moves relative to the driver station, pressing the joystick away from
* you should always move the robot away from you.
*/
Headless,
HEADLESS,

/**
* Pressing the joystick forward moves the robot froward, regardless of
* position relative to driver.
*/
Relative
RELATIVE,

/**
* Rock mode, resists movement and holds position.
*/
ROCK
}
181 changes: 131 additions & 50 deletions src/main/java/frc/robot/subsystems/SwerveModule.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,35 +6,37 @@

import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.SensorInitializationStrategy;

import com.ctre.phoenix.sensors.SensorTimeBase;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.RelativeEncoder;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import frc.robot.SmartPrintable;

/**
* Class for managing and manipulating a swerve module.
*/
public class SwerveModule extends SmartPrintable {
private static final double CONVERSION_FACTOR_ROTATION = 150 / 7; // Rotations to degrees.
private static final double CONVERSION_FACTOR_ROTATION = Math.toRadians(150 / 7); // Rotations to radians.
private static final double CONVERSION_FACTOR_MOVEMENT = 6.75; // Rotations to meters.
private static final double CONVERSION_FACTOR_ROTATION_VELOCITY = CONVERSION_FACTOR_ROTATION * (1 / 60); // RPM to degrees per second.
private static final double CONVERSION_FACTOR_ROTATION_VELOCITY = CONVERSION_FACTOR_ROTATION * (1 / 60); // RPM to radians per second.
private static final double CONVERSION_FACTOR_MOVEMENT_VELOCITY = CONVERSION_FACTOR_MOVEMENT * (1 / 60); // RPM to meters per second.

private static double PID_P = 0.01;
private static double PID_I = 0.0;
private static double PID_D = 0.0;
private static final double PID_P = 0.5;
private static final double PID_I = 0.0;
private static final double PID_D = 0.0;

private static double ROCK_PID_P = 0.1;
private static double ROCK_PID_I = 0.0;
private static double ROCK_PID_D = 0.0;
private static final double ROCK_PID_P = 0.1;
private static final double ROCK_PID_I = 0.0;
private static final double ROCK_PID_D = 0.0;

private final SwerveModulePosition position;

Expand All @@ -48,25 +50,80 @@ public class SwerveModule extends SmartPrintable {
private final PIDController rotationController;
private final PIDController rockController;

private final double canCoderOffset;
private final RelativePosition physicalPosition;
private final Angle canCoderOffset;
private final double coefficient;

/**
* Set to NaN if not in rock mode, NaN does not equal itself by definition
* (see some IEEE standard or something) and so this is how rock mode is
* checked.
*/
private double rockPos;
// Set to NaN if not in rock mode, NaN does not equal itself by definition
// (see some IEEE standard or something) and so this is how rock mode is
// checked. Max speed works the same way.
private double rockPos = Double.NaN;
private double maxSpeed = Double.NaN;

private enum RelativePosition {
FRONT_RIGHT ( 1.0, 1.0 ),
FRONT_LEFT ( -1.0, 1.0 ),
BACK_LEFT ( -1.0, -1.0 ),
BACK_RIGHT ( 1.0, -1.0 );

private boolean front = true;
private boolean right = true;

RelativePosition(double x, double y) {
right = Math.signum(x) > 0.0;
front = Math.signum(y) > 0.0;
}

public String asString() {
String str = "";

if (front) {
str += "Front ";
} else {
str += "Back ";
}

if (right) {
str += "Right";
} else {
str += "Left";
}

return str;
}

public static RelativePosition fromTranslation(Translation2d translation) {
var x_sign = Math.signum(translation.getX()) > 0.0;
var y_sign = Math.signum(translation.getY()) > 0.0;

if (x_sign && y_sign) {
return FRONT_RIGHT;
} else if (x_sign) {
return BACK_RIGHT;
} else if (y_sign) {
return FRONT_LEFT;
}

return BACK_LEFT;
}
}

public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, double canCoderOffset, double coefficient) {
public SwerveModule(
int movementMotorID,
int rotationalMotorID,
int canCoderID,
Angle canCoderOffset,
double coefficient,
Translation2d physicalPosition
) {
super();

this.canCoderOffset = canCoderOffset;
this.physicalPosition = RelativePosition.fromTranslation(physicalPosition);
this.canCoderOffset = canCoderOffset.clone();
this.coefficient = coefficient;

rotationMotor = new CANSparkMax(rotationalMotorID, MotorType.kBrushless);
rotationMotor.setInverted(false);
//rotationMotor.setVoltage(11.0);
rotationMotor.setSmartCurrentLimit(30);

movementMotor = new CANSparkMax(movementMotorID, MotorType.kBrushless);
Expand All @@ -75,7 +132,19 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID,
movementMotor.setSmartCurrentLimit(40);

angularEncoder = new CANCoder(canCoderID);
angularEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition, 1000);
angularEncoder.configFactoryDefault();

// Magic and forbidden config from the code orange wizards. Makes the
// encoder initialize to absolute.
angularEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
angularEncoder.configFeedbackCoefficient(
// Since the default coefficiant used for degrees is not
// particularly intuitive we just grab it and run a deg -> rad
// conversion on it.
Math.toRadians(angularEncoder.configGetFeedbackCoefficient()),
"rad",
SensorTimeBase.PerSecond
);

rotationEncoder = rotationMotor.getEncoder();
rotationEncoder.setPosition(angularEncoder.getPosition());
Expand All @@ -88,35 +157,37 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID,
movementEncoder.setVelocityConversionFactor(CONVERSION_FACTOR_MOVEMENT_VELOCITY);

rotationController = new PIDController(PID_P, PID_I, PID_D);
rotationController.enableContinuousInput(0.0, 360.0);
rotationController.setTolerance(0.5);
rotationController.enableContinuousInput(0.0, Angle.TAU);
rotationController.setTolerance(0.005); // This is more precise than before, TODO: test.

rockController = new PIDController(ROCK_PID_P, ROCK_PID_I, ROCK_PID_D);
rockController.setTolerance(0.5);

position = new SwerveModulePosition(movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT, new Rotation2d(angularEncoder.getPosition() / 180.0 * Math.PI));
position = new SwerveModulePosition(
movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT,
new Rotation2d(angularEncoder.getPosition())
);
}

/**
* Sets the desired module state for this module.
* Sets the desired module state for this module. This must be run
* repeatedly to continue PID calculations.
*/
public void setDesiredState(SwerveModuleState state) {
/* if (Math.abs(state.speedMetersPerSecond) < 0.001) {
movementMotor.set(0.0);
rotationMotor.set(0.0);
return;
} */

double currentPosition = (angularEncoder.getPosition() + canCoderOffset) % 360.0;
state = SwerveModuleState.optimize(state, new Rotation2d(currentPosition * Math.PI / 180.0));
double calculation = rotationController.calculate(currentPosition, (state.angle.getDegrees() + 360.0) % 360.0);
double currentPosition = (angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU;
state = SwerveModuleState.optimize(state, new Rotation2d(currentPosition));

if (rockPos != rockPos) {
movementMotor.set(Math.abs(state.speedMetersPerSecond) > 0.3 ?
Math.signum(state.speedMetersPerSecond) * 0.3 :
state.speedMetersPerSecond);
movementMotor.set(
Math.abs(state.speedMetersPerSecond) > maxSpeed
? Math.signum(state.speedMetersPerSecond) * maxSpeed
: state.speedMetersPerSecond
);
} else {
movementMotor.set(rockController.calculate(getDistanceTraveled(), rockPos));
}

double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Angle.TAU) % Angle.TAU);
rotationMotor.set(calculation * coefficient);

position.angle = new Rotation2d(angularEncoder.getPosition());
Expand All @@ -130,45 +201,55 @@ public void setRockMode(boolean shouldHold) {
if (!shouldHold) {
rockPos = Double.NaN;
return;
} else if (rockPos != rockPos) {
rockPos = getDistanceTraveled();
}

if (shouldHold && rockPos != rockPos) {
rockPos = getPosition();
}

movementMotor.set(rockController.calculate(getPosition(), rockPos));
}

/**
* True if in rock mode.
*/
public boolean getRockMode() {
public boolean inRockMode() {
return rockPos == rockPos;
}

/**
* Sets this module's maximum movement speed. Use NaN for no limit.
*/
public void setMaxSpeed(double maxSpeed) {
this.maxSpeed = maxSpeed;
}

/**
* Gets this module's maximum movement speed
*/
public double getMaxSpeed() {
return maxSpeed;
}

/**
* Gets distance traveled, should not be used for ablsolute distances as
* this function currently makes no guarantee as to the starting position
* of the module. (This can be mitigated if you zero positions, but it will
* interupt odometry).
*/
public double getPosition() {
public double getDistanceTraveled() {
return movementEncoder.getPosition();
}

@Override
public void print() {
SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position", angularEncoder.getPosition());
SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position mod 360", angularEncoder.getPosition() % 360);
SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off", angularEncoder.getPosition() + canCoderOffset);
SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off mod 360", (angularEncoder.getPosition() + canCoderOffset) % 360);
SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position (Distance) ", movementEncoder.getPosition());
SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position", angularEncoder.getPosition());
SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position mod tau", angularEncoder.getPosition() % Angle.TAU);
SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position + off", angularEncoder.getPosition() + canCoderOffset.radians());
SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position + off mod tau", (angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU);
SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position (Distance) ", movementEncoder.getPosition());
}

/**
* Gets the angle of the module.
*/
public SwerveModulePosition getAngle() {
public SwerveModulePosition getPosition() {
return position;
}

Expand Down
Loading