Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Addressing issue #7 by reading CANCoder values until successful with 10ms delay between readings. Fall back to reading relative encoder.

Browse Source
This commit is contained in:
thenetworkgrinch
2023-02-20 20:59:31 -06:00
parent 8f9ffdf031
commit dd28a657b2
43 changed files with 570 additions and 363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
swervelib/SwerveController.java
View File

@@ -72,11 +72,14 @@ public class SwerveController
* @param currentHeadingAngleRadians The current robot heading in radians.
* @return {@link ChassisSpeeds} which can be sent to th Swerve Drive.
*/
public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double angle, double currentHeadingAngleRadians)
public ChassisSpeeds getTargetSpeeds(
double xInput, double yInput, double angle, double currentHeadingAngleRadians)
{
// Calculates an angular rate using a PIDController and the commanded angle. This is then scaled by
// Calculates an angular rate using a PIDController and the commanded angle. This is then
// scaled by
// the drivebase's maximum angular velocity.
double omega = thetaController.calculate(currentHeadingAngleRadians, angle) * config.maxAngularVelocity;
double omega =
thetaController.calculate(currentHeadingAngleRadians, angle) * config.maxAngularVelocity;
// Convert joystick inputs to m/s by scaling by max linear speed. Also uses a cubic function
// to allow for precise control and fast movement.
double x = Math.pow(xInput, 3) * config.maxSpeed;
@@ -96,13 +99,20 @@ public class SwerveController
* @param currentHeadingAngleRadians The current robot heading in radians.
* @return {@link ChassisSpeeds} which can be sent to th Swerve Drive.
*/
public ChassisSpeeds getTargetSpeeds(double xInput, double yInput, double headingX, double headingY,
public ChassisSpeeds getTargetSpeeds(
double xInput,
double yInput,
double headingX,
double headingY,
double currentHeadingAngleRadians)
{
// Converts the horizontal and vertical components to the commanded angle, in radians, unless the joystick is near
// the center (i. e. has been released), in which case the angle is held at the last valid joystick input (hold
// Converts the horizontal and vertical components to the commanded angle, in radians, unless
// the joystick is near
// the center (i. e. has been released), in which case the angle is held at the last valid
// joystick input (hold
// position when stick released).
double angle = withinHypotDeadband(headingX, headingY) ? lastAngle : Math.atan2(headingX, headingY);
double angle =
withinHypotDeadband(headingX, headingY) ? lastAngle : Math.atan2(headingX, headingY);
ChassisSpeeds speeds = getTargetSpeeds(xInput, yInput, angle, currentHeadingAngleRadians);
// Used for the position hold feature

110
swervelib/SwerveDrive.java
View File

@@ -11,10 +11,10 @@ import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
import java.util.ArrayList;
import java.util.List;
import swervelib.imu.SwerveIMU;
@@ -69,7 +69,7 @@ public class SwerveDrive
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
* {@link SwerveDrive#setModuleStates} method. The {@link SwerveDrive#drive} method incorporates kinematics— it takes
* {@link SwerveDrive#setModuleStates} method. The {@link SwerveDrive#drive} method incorporates kinematics-- it takes
* a translation and rotation, as well as parameters for field-centric and closed-loop velocity control.
* {@link SwerveDrive#setModuleStates} takes a list of SwerveModuleStates and directly passes them to the modules.
* This subsystem also handles odometry.
@@ -78,7 +78,8 @@ public class SwerveDrive
* @param controllerConfig The {@link SwerveControllerConfiguration} to use when creating the
* {@link SwerveController}.
*/
public SwerveDrive(SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig)
public SwerveDrive(
SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig)
{
swerveDriveConfiguration = config;
swerveController = new SwerveController(controllerConfig);
@@ -87,7 +88,7 @@ public class SwerveDrive
// Create an integrator for angle if the robot is being simulated to emulate an IMU
// If the robot is real, instantiate the IMU instead.
if (!Robot.isReal())
if (RobotBase.isSimulation())
{
simIMU = new SwerveIMUSimulation();
} else
@@ -98,14 +99,18 @@ public class SwerveDrive
this.swerveModules = config.modules;
// odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
swerveDrivePoseEstimator = new SwerveDrivePoseEstimator(
// odometry = new SwerveDriveOdometry(kinematics, getYaw(), getModulePositions());
swerveDrivePoseEstimator =
new SwerveDrivePoseEstimator(
kinematics,
getYaw(),
getModulePositions(),
new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0)),
VecBuilder.fill(0.1, 0.1, 0.1), // x,y,heading in radians; state std dev, higher=less weight
VecBuilder.fill(0.9, 0.9, 0.9)); // x,y,heading in radians; Vision measurement std dev, higher=less weight
VecBuilder.fill(
0.1, 0.1, 0.1), // x,y,heading in radians; state std dev, higher=less weight
VecBuilder.fill(
0.9, 0.9,
0.9)); // x,y,heading in radians; Vision measurement std dev, higher=less weight
zeroGyro();
SmartDashboard.putData("Field", field);
@@ -118,24 +123,24 @@ public class SwerveDrive
*
* @param translation {@link Translation2d} that is the commanded linear velocity of the robot, in meters per
* second. In robot-relative mode, positive x is torwards the bow (front) and positive y is
* torwards port (left). In field-relative mode, positive x is away from the alliance wall
* (field North) and positive y is torwards the left wall when looking through the driver station
* glass (field West).
* torwards port (left). In field-relative mode, positive x is away from the alliance wall (field
* North) and positive y is torwards the left wall when looking through the driver station glass
* (field West).
* @param rotation Robot angular rate, in radians per second. CCW positive. Unaffected by field/robot
* relativity.
* @param fieldRelative Drive mode. True for field-relative, false for robot-relative.
* @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
*/
public void drive(Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)
public void drive(
Translation2d translation, double rotation, boolean fieldRelative, boolean isOpenLoop)
{
// Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if necessary.
ChassisSpeeds velocity = fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(translation.getX(),
translation.getY(),
rotation,
getYaw()) : new ChassisSpeeds(
translation.getX(),
translation.getY(),
rotation);
// Creates a robot-relative ChassisSpeeds object, converting from field-relative speeds if
// necessary.
ChassisSpeeds velocity =
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(
translation.getX(), translation.getY(), rotation, getYaw())
: new ChassisSpeeds(translation.getX(), translation.getY(), rotation);
// Display commanded speed for testing
SmartDashboard.putString("RobotVelocity", velocity.toString());
@@ -161,9 +166,11 @@ public class SwerveDrive
for (SwerveModule module : swerveModules)
{
module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop);
SmartDashboard.putNumber("Module " + module.moduleNumber + " Speed Setpoint: ",
SmartDashboard.putNumber(
"Module " + module.moduleNumber + " Speed Setpoint: ",
desiredStates[module.moduleNumber].speedMetersPerSecond);
SmartDashboard.putNumber("Module " + module.moduleNumber + " Angle Setpoint: ",
SmartDashboard.putNumber(
"Module " + module.moduleNumber + " Angle Setpoint: ",
desiredStates[module.moduleNumber].angle.getDegrees());
}
}
@@ -175,8 +182,7 @@ public class SwerveDrive
*/
public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
{
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds),
false);
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
}
/**
@@ -197,9 +203,11 @@ public class SwerveDrive
public ChassisSpeeds getFieldVelocity()
{
// ChassisSpeeds has a method to convert from field-relative to robot-relative speeds,
// but not the reverse. However, because this transform is a simple rotation, negating the angle
// but not the reverse. However, because this transform is a simple rotation, negating the
// angle
// given as the robot angle reverses the direction of rotation, and the conversion is reversed.
return ChassisSpeeds.fromFieldRelativeSpeeds(kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus());
return ChassisSpeeds.fromFieldRelativeSpeeds(
kinematics.toChassisSpeeds(getStates()), getYaw().unaryMinus());
}
/**
@@ -212,7 +220,6 @@ public class SwerveDrive
return kinematics.toChassisSpeeds(getStates());
}
/**
* Resets odometry to the given pose. Gyro angle and module positions do not need to be reset when calling this
* method. However, if either gyro angle or module position is reset, this must be called in order for odometry to
@@ -257,7 +264,8 @@ public class SwerveDrive
*/
public SwerveModulePosition[] getModulePositions()
{
SwerveModulePosition[] positions = new SwerveModulePosition[swerveDriveConfiguration.moduleCount];
SwerveModulePosition[] positions =
new SwerveModulePosition[swerveDriveConfiguration.moduleCount];
for (SwerveModule module : swerveModules)
{
positions[module.moduleNumber] = module.getPosition();
@@ -270,8 +278,9 @@ public class SwerveDrive
*/
public void zeroGyro()
{
// Resets the real gyro or the angle accumulator, depending on whether the robot is being simulated
if (Robot.isReal())
// Resets the real gyro or the angle accumulator, depending on whether the robot is being
// simulated
if (!RobotBase.isSimulation())
{
imu.setYaw(0);
} else
@@ -290,7 +299,7 @@ public class SwerveDrive
public Rotation2d getYaw()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (Robot.isReal())
if (!RobotBase.isSimulation())
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
@@ -309,7 +318,7 @@ public class SwerveDrive
public Rotation2d getPitch()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (Robot.isReal())
if (!RobotBase.isSimulation())
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
@@ -328,7 +337,7 @@ public class SwerveDrive
public Rotation2d getRoll()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (Robot.isReal())
if (!RobotBase.isSimulation())
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
@@ -347,7 +356,7 @@ public class SwerveDrive
public Rotation3d getGyroRotation3d()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
if (Robot.isReal())
if (!RobotBase.isSimulation())
{
double[] ypr = new double[3];
imu.getYawPitchRoll(ypr);
@@ -382,9 +391,8 @@ public class SwerveDrive
{
for (SwerveModule swerveModule : swerveModules)
{
swerveModule.setDesiredState(new SwerveModuleState2(0,
swerveModule.configuration.moduleLocation.getAngle(),
0), true);
swerveModule.setDesiredState(
new SwerveModuleState2(0, swerveModule.configuration.moduleLocation.getAngle(), 0), true);
}
}
@@ -400,7 +408,9 @@ public class SwerveDrive
List<Pose2d> poses = new ArrayList<>();
for (SwerveModule module : swerveModules)
{
poses.add(robotPose.plus(new Transform2d(module.configuration.moduleLocation, module.getState().angle)));
poses.add(
robotPose.plus(
new Transform2d(module.configuration.moduleLocation, module.getState().angle)));
}
return poses.toArray(poseArr);
}
@@ -429,10 +439,13 @@ public class SwerveDrive
swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
// Update angle accumulator if the robot is simulated
if (!Robot.isReal())
if (RobotBase.isSimulation())
{
simIMU.updateOdometry(kinematics, getStates(),
getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()), field);
simIMU.updateOdometry(
kinematics,
getStates(),
getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()),
field);
}
field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition());
@@ -446,19 +459,21 @@ public class SwerveDrive
moduleStates[module.moduleNumber + 1] = moduleState.speedMetersPerSecond;
sumOmega += Math.abs(moduleState.omegaRadPerSecond);
SmartDashboard.putNumber("Module" + module.moduleNumber + "Relative Encoder", module.getRelativePosition());
SmartDashboard.putNumber("Module" + module.moduleNumber + "Absolute Encoder", module.getAbsolutePosition());
SmartDashboard.putNumber(
"Module" + module.moduleNumber + "Relative Encoder", module.getRelativePosition());
SmartDashboard.putNumber(
"Module" + module.moduleNumber + "Absolute Encoder", module.getAbsolutePosition());
}
SmartDashboard.putNumberArray("moduleStates", moduleStates);
// If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS lib)
// If the robot isn't moving synchronize the encoders every 100ms (Inspired by democrat's SDS
// lib)
// To ensure that everytime we initialize it works.
if (sumOmega <= .01 && ++moduleSynchronizationCounter > 5)
{
synchronizeModuleEncoders();
moduleSynchronizationCounter = 0;
}
}
/**
@@ -492,10 +507,11 @@ public class SwerveDrive
swerveDrivePoseEstimator.addVisionMeasurement(robotPose, timestamp);
} else
{
swerveDrivePoseEstimator.resetPosition(robotPose.getRotation(), getModulePositions(), robotPose);
swerveDrivePoseEstimator.resetPosition(
robotPose.getRotation(), getModulePositions(), robotPose);
}
if (Robot.isReal())
if (!RobotBase.isSimulation())
{
imu.setYaw(swerveDrivePoseEstimator.getEstimatedPosition().getRotation().getDegrees());
// Yaw reset recommended by Team 1622

59
swervelib/SwerveModule.java
View File

@@ -4,8 +4,8 @@ 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.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc.robot.Robot;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveModuleState2;
import swervelib.motors.SwerveMotor;
@@ -59,10 +59,10 @@ public class SwerveModule
*/
public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration)
{
// angle = 0;
// speed = 0;
// omega = 0;
// fakePos = 0;
// angle = 0;
// speed = 0;
// omega = 0;
// fakePos = 0;
this.moduleNumber = moduleNumber;
configuration = moduleConfiguration;
angleOffset = moduleConfiguration.angleOffset;
@@ -90,7 +90,8 @@ public class SwerveModule
{
absoluteEncoder.factoryDefault();
absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
angleMotor.configureIntegratedEncoder(moduleConfiguration.getPositionEncoderConversion(false));
angleMotor.configureIntegratedEncoder(
moduleConfiguration.getPositionEncoderConversion(false));
angleMotor.setPosition(absoluteEncoder.getAbsolutePosition() - angleOffset);
}
@@ -109,7 +110,7 @@ public class SwerveModule
driveMotor.burnFlash();
angleMotor.burnFlash();
if (!Robot.isReal())
if (RobotBase.isSimulation())
{
simModule = new SwerveModuleSimulation();
}
@@ -124,7 +125,7 @@ public class SwerveModule
{
if (absoluteEncoder != null)
{
angleMotor.setPosition(absoluteEncoder.getAbsolutePosition() - angleOffset);
angleMotor.setPosition(getAbsolutePosition() - angleOffset);
}
}
@@ -136,14 +137,19 @@ public class SwerveModule
*/
public void setDesiredState(SwerveModuleState2 desiredState, boolean isOpenLoop)
{
SwerveModuleState simpleState = new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
SwerveModuleState simpleState =
new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
simpleState = SwerveModuleState.optimize(simpleState, getState().angle);
desiredState = new SwerveModuleState2(simpleState.speedMetersPerSecond, simpleState.angle,
desiredState.omegaRadPerSecond);
desiredState =
new SwerveModuleState2(
simpleState.speedMetersPerSecond, simpleState.angle, desiredState.omegaRadPerSecond);
SmartDashboard.putNumber("Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond);
SmartDashboard.putNumber("Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees());
SmartDashboard.putNumber("Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond));
SmartDashboard.putNumber(
"Optimized " + moduleNumber + " Speed Setpoint: ", desiredState.speedMetersPerSecond);
SmartDashboard.putNumber(
"Optimized " + moduleNumber + " Angle Setpoint: ", desiredState.angle.getDegrees());
SmartDashboard.putNumber(
"Module " + moduleNumber + " Omega: ", Math.toDegrees(desiredState.omegaRadPerSecond));
if (isOpenLoop)
{
@@ -156,17 +162,18 @@ public class SwerveModule
}
// Prevents module rotation if speed is less than 1%
double angle = (Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01) ?
lastAngle :
desiredState.angle.getDegrees());
angleMotor.setReference(angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
double angle =
(Math.abs(desiredState.speedMetersPerSecond) <= (configuration.maxSpeed * 0.01)
? lastAngle
: desiredState.angle.getDegrees());
angleMotor.setReference(
angle, Math.toDegrees(desiredState.omegaRadPerSecond) * configuration.angleKV);
lastAngle = angle;
if (!Robot.isReal())
if (RobotBase.isSimulation())
{
simModule.updateStateAndPosition(desiredState);
}
}
/**
@@ -190,7 +197,7 @@ public class SwerveModule
double velocity;
Rotation2d azimuth;
double omega;
if (Robot.isReal())
if (!RobotBase.isSimulation())
{
velocity = driveMotor.getVelocity();
azimuth = Rotation2d.fromDegrees(angleMotor.getPosition());
@@ -211,7 +218,7 @@ public class SwerveModule
{
double position;
Rotation2d azimuth;
if (Robot.isReal())
if (!RobotBase.isSimulation())
{
position = driveMotor.getPosition();
azimuth = Rotation2d.fromDegrees(angleMotor.getPosition());
@@ -230,7 +237,13 @@ public class SwerveModule
*/
public double getAbsolutePosition()
{
return absoluteEncoder.getAbsolutePosition();
double angle = absoluteEncoder.getAbsolutePosition();
if (absoluteEncoder.readingError)
{
angle = getRelativePosition();
}
return angle;
}
/**

48
swervelib/encoders/CANCoderSwerve.java
View File

@@ -1,5 +1,6 @@
package swervelib.encoders;
import com.ctre.phoenix.ErrorCode;
import com.ctre.phoenix.sensors.AbsoluteSensorRange;
import com.ctre.phoenix.sensors.CANCoderConfiguration;
import com.ctre.phoenix.sensors.MagnetFieldStrength;
@@ -69,24 +70,61 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
canCoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360;
canCoderConfiguration.sensorDirection = inverted;
canCoderConfiguration.initializationStrategy = SensorInitializationStrategy.BootToAbsolutePosition;
canCoderConfiguration.initializationStrategy =
SensorInitializationStrategy.BootToAbsolutePosition;
canCoderConfiguration.sensorTimeBase = SensorTimeBase.PerSecond;
encoder.configAllSettings(canCoderConfiguration);
}
/**
* Get the absolute position of the encoder.
* Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on erroneous readings.
*
* @return Absolute position in degrees from [0, 360).
*/
@Override
public double getAbsolutePosition()
{
if (encoder.getMagnetFieldStrength() != MagnetFieldStrength.Good_GreenLED)
readingError = false;
MagnetFieldStrength strength = encoder.getMagnetFieldStrength();
if (strength != MagnetFieldStrength.Good_GreenLED)
{
DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false);
DriverStation.reportWarning(
"CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.\n", false);
}
return encoder.getAbsolutePosition();
if (strength == MagnetFieldStrength.Invalid_Unknown || strength == MagnetFieldStrength.BadRange_RedLED)
{
readingError = true;
return 0;
}
double angle = encoder.getAbsolutePosition();
// Taken from democat's library.
// Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74
ErrorCode code = encoder.getLastError();
int ATTEMPTS = 3;
for (int i = 0; i < ATTEMPTS; i++)
{
if (code == ErrorCode.OK)
{
break;
}
try
{
Thread.sleep(10);
} catch (InterruptedException e)
{
}
angle = encoder.getAbsolutePosition();
code = encoder.getLastError();
}
if (code != ErrorCode.OK)
{
readingError = true;
DriverStation.reportWarning("CANCoder " + encoder.getDeviceID() + " reading was faulty, ignoring.\n", false);
}
return angle;
}
/**

5
swervelib/encoders/SwerveAbsoluteEncoder.java
View File

@@ -6,6 +6,11 @@ package swervelib.encoders;
public abstract class SwerveAbsoluteEncoder
{
/**
* Last angle reading was faulty.
*/
public boolean readingError = false;
/**
* Reset the encoder to factory defaults.
*/

1
swervelib/imu/ADIS16448Swerve.java
View File

@@ -81,4 +81,3 @@ public class ADIS16448Swerve extends SwerveIMU
return imu;
}
}

1
swervelib/imu/ADXRS450Swerve.java
View File

@@ -80,5 +80,4 @@ public class ADXRS450Swerve extends SwerveIMU
{
return imu;
}
}

3
swervelib/imu/AnalogGyroSwerve.java
View File

@@ -27,7 +27,8 @@ public class AnalogGyroSwerve extends SwerveIMU
{
if (!(channel == 0 || channel == 1))
{
throw new RuntimeException("Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n");
throw new RuntimeException(
"Analog Gyroscope must be attached to port 0 or 1 on the roboRIO.\n");
}
gyro = new AnalogGyro(channel);
SmartDashboard.putData(gyro);

1
swervelib/imu/Pigeon2Swerve.java
View File

@@ -15,7 +15,6 @@ public class Pigeon2Swerve extends SwerveIMU
*/
WPI_Pigeon2 imu;
/**
* Generate the SwerveIMU for pigeon.
*

42
swervelib/math/SwerveKinematics2.java
View File

@@ -76,8 +76,10 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
{
m_inverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
bigInverseKinematics.setRow(i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY());
bigInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX());
bigInverseKinematics.setRow(
i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getX(), -m_modules[i].getY());
bigInverseKinematics.setRow(
i * 2 + 1, 0, /* Start Data */ 0, 1, -m_modules[i].getY(), +m_modules[i].getX());
}
m_forwardKinematics = m_inverseKinematics.pseudoInverse();
@@ -197,11 +199,7 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
for (int i = 0; i < m_numModules; i++)
{
m_inverseKinematics.setRow(
i * 2,
0, /* Start Data */
1,
0,
-m_modules[i].getY() + centerOfRotationMeters.getY());
i * 2, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotationMeters.getY());
m_inverseKinematics.setRow(
i * 2 + 1,
0, /* Start Data */
@@ -237,14 +235,7 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
var moduleVelocityStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
var accelerationVector = new SimpleMatrix(4, 1);
accelerationVector.setColumn(
0,
0,
0,
0,
Math.pow(chassisSpeeds.omegaRadiansPerSecond, 2),
0
);
accelerationVector.setColumn(0, 0, 0, 0, Math.pow(chassisSpeeds.omegaRadiansPerSecond, 2), 0);
var moduleAccelerationStatesMatrix = bigInverseKinematics.mult(accelerationVector);
@@ -260,26 +251,11 @@ public class SwerveKinematics2 extends SwerveDriveKinematics
Rotation2d angle = new Rotation2d(x, y);
var trigThetaAngle = new SimpleMatrix(2, 2);
trigThetaAngle.setColumn(
0,
0,
angle.getCos(),
-angle.getSin()
);
trigThetaAngle.setColumn(
1,
0,
angle.getSin(),
angle.getCos()
);
trigThetaAngle.setColumn(0, 0, angle.getCos(), -angle.getSin());
trigThetaAngle.setColumn(1, 0, angle.getSin(), angle.getCos());
var accelVector = new SimpleMatrix(2, 1);
accelVector.setColumn(
0,
0,
ax,
ay
);
accelVector.setColumn(0, 0, ax, ay);
var omegaVector = trigThetaAngle.mult(accelVector);

111
swervelib/math/SwerveMath.java
View File

@@ -19,14 +19,15 @@ public class SwerveMath
/**
* Calculate the angle kV which will be multiplied by the radians per second for the feedforward. Volt * seconds /
* degree <=> (maxVolts) / (maxSpeed)
* degree == (maxVolts) / (maxSpeed)
*
* @param optimalVoltage Optimal voltage to use when calculating the angle kV.
* @param motorFreeSpeedRPM Motor free speed in Rotations per Minute.
* @param angleGearRatio Angle gear ratio, the amount of times the motor as to turn for the wheel rotation.
* @return angle kV for feedforward.
*/
public static double calculateAngleKV(double optimalVoltage, double motorFreeSpeedRPM, double angleGearRatio)
public static double calculateAngleKV(
double optimalVoltage, double motorFreeSpeedRPM, double angleGearRatio)
{
double maxAngularVelocity = 360 * (motorFreeSpeedRPM / angleGearRatio) / 60; // deg/s
return optimalVoltage / maxAngularVelocity;
@@ -41,7 +42,8 @@ public class SwerveMath
* @param pulsePerRotation The number of encoder pulses per rotation. 1 if using an integrated encoder.
* @return Meters per rotation for the drive motor.
*/
public static double calculateMetersPerRotation(double wheelDiameter, double driveGearRatio, double pulsePerRotation)
public static double calculateMetersPerRotation(
double wheelDiameter, double driveGearRatio, double pulsePerRotation)
{
return (Math.PI * wheelDiameter) / (driveGearRatio * pulsePerRotation);
}
@@ -69,7 +71,9 @@ public class SwerveMath
public static double applyDeadband(double value, boolean scaled, double deadband)
{
value = Math.abs(value) > deadband ? value : 0;
return scaled ? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value) : value;
return scaled
? ((1 / (1 - deadband)) * (Math.abs(value) - deadband)) * Math.signum(value)
: value;
}
/**
@@ -80,7 +84,8 @@ public class SwerveMath
* @param pulsePerRotation The number of pulses in a complete rotation for the encoder, 1 if integrated.
* @return Degrees per steering rotation for the angle motor.
*/
public static double calculateDegreesPerSteeringRotation(double angleGearRatio, double pulsePerRotation)
public static double calculateDegreesPerSteeringRotation(
double angleGearRatio, double pulsePerRotation)
{
return 360 / (angleGearRatio * pulsePerRotation);
}
@@ -93,7 +98,8 @@ public class SwerveMath
* @param furthestModuleY Y of the furthest module in meters.
* @return Maximum angular velocity in rad/s.
*/
public static double calculateMaxAngularVelocity(double maxSpeed, double furthestModuleX, double furthestModuleY)
public static double calculateMaxAngularVelocity(
double maxSpeed, double furthestModuleX, double furthestModuleY)
{
return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY);
}
@@ -119,8 +125,12 @@ public class SwerveMath
* @param robotMass Mass of the robot in kg.
* @return Theoretical maximum acceleration in m/s/s.
*/
public static double calculateMaxAcceleration(double stallTorqueNm, double gearRatio, double moduleCount,
double wheelDiameter, double robotMass)
public static double calculateMaxAcceleration(
double stallTorqueNm,
double gearRatio,
double moduleCount,
double wheelDiameter,
double robotMass)
{
return (stallTorqueNm * gearRatio * moduleCount) / ((wheelDiameter / 2) * robotMass);
}
@@ -137,50 +147,60 @@ public class SwerveMath
* @param config The swerve drive configuration.
* @return Maximum acceleration allowed in the robot direction.
*/
private static double calcMaxAccel(Rotation2d angle, double chassisMass, double robotMass,
Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config)
private static double calcMaxAccel(
Rotation2d angle,
double chassisMass,
double robotMass,
Translation3d chassisCenterOfGravity,
SwerveDriveConfiguration config)
{
double xMoment = (chassisCenterOfGravity.getX() * chassisMass);
double yMoment = (chassisCenterOfGravity.getY() * chassisMass);
// Calculate the vertical mass moment using the floor as the datum. This will be used later to calculate max
// Calculate the vertical mass moment using the floor as the datum. This will be used later to
// calculate max
// acceleration
double zMoment = (chassisCenterOfGravity.getZ() * (chassisMass));
Translation3d robotCG = new Translation3d(xMoment, yMoment, zMoment).div(robotMass);
Translation2d horizontalCG = robotCG.toTranslation2d();
Translation2d projectedHorizontalCg = new Translation2d(
(angle.getSin() * angle.getCos() * horizontalCG.getY()) + (Math.pow(angle.getCos(), 2) * horizontalCG.getX()),
(angle.getSin() * angle.getCos() * horizontalCG.getX()) + (Math.pow(angle.getSin(), 2) * horizontalCG.getY())
);
Translation2d projectedHorizontalCg =
new Translation2d(
(angle.getSin() * angle.getCos() * horizontalCG.getY())
+ (Math.pow(angle.getCos(), 2) * horizontalCG.getX()),
(angle.getSin() * angle.getCos() * horizontalCG.getX())
+ (Math.pow(angle.getSin(), 2) * horizontalCG.getY()));
// Projects the edge of the wheelbase onto the direction line. Assumes the wheelbase is rectangular.
// Because a line is being projected, rather than a point, one of the coordinates of the projected point is
// Projects the edge of the wheelbase onto the direction line. Assumes the wheelbase is
// rectangular.
// Because a line is being projected, rather than a point, one of the coordinates of the
// projected point is
// already known.
Translation2d projectedWheelbaseEdge;
double angDeg = angle.getDegrees();
if (angDeg <= 45 && angDeg >= -45)
{
SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true);
projectedWheelbaseEdge = new Translation2d(conf.moduleLocation.getX(),
conf.moduleLocation.getX() * angle.getTan());
projectedWheelbaseEdge =
new Translation2d(
conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan());
} else if (135 >= angDeg && angDeg > 45)
{
SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, true);
projectedWheelbaseEdge = new Translation2d(
conf.moduleLocation.getY() / angle.getTan(),
conf.moduleLocation.getY());
projectedWheelbaseEdge =
new Translation2d(
conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY());
} else if (-135 <= angDeg && angDeg < -45)
{
SwerveModuleConfiguration conf = getSwerveModule(config.modules, true, false);
projectedWheelbaseEdge = new Translation2d(
conf.moduleLocation.getY() / angle.getTan(),
conf.moduleLocation.getY());
projectedWheelbaseEdge =
new Translation2d(
conf.moduleLocation.getY() / angle.getTan(), conf.moduleLocation.getY());
} else
{
SwerveModuleConfiguration conf = getSwerveModule(config.modules, false, true);
projectedWheelbaseEdge = new Translation2d(
conf.moduleLocation.getX(),
conf.moduleLocation.getX() * angle.getTan());
projectedWheelbaseEdge =
new Translation2d(
conf.moduleLocation.getX(), conf.moduleLocation.getX() * angle.getTan());
}
double horizontalDistance = projectedHorizontalCg.plus(projectedWheelbaseEdge).getNorm();
@@ -207,9 +227,15 @@ public class SwerveMath
* @return The limited velocity. This is either the commanded velocity, if attainable, or the closest attainable
* velocity.
*/
public static Translation2d limitVelocity(Translation2d commandedVelocity, ChassisSpeeds fieldVelocity,
Pose2d robotPose, double loopTime, double chassisMass, double robotMass,
Translation3d chassisCenterOfGravity, SwerveDriveConfiguration config)
public static Translation2d limitVelocity(
Translation2d commandedVelocity,
ChassisSpeeds fieldVelocity,
Pose2d robotPose,
double loopTime,
double chassisMass,
double robotMass,
Translation3d chassisCenterOfGravity,
SwerveDriveConfiguration config)
{
// Get the robot's current field-relative velocity
Translation2d currentVelocity = SwerveController.getTranslation2d(fieldVelocity);
@@ -222,11 +248,17 @@ public class SwerveMath
// Creates an acceleration vector with the direction of delta V and a magnitude
// of the maximum allowed acceleration in that direction
Translation2d maxAccel = new Translation2d(
calcMaxAccel(deltaV
Translation2d maxAccel =
new Translation2d(
calcMaxAccel(
deltaV
// Rotates the velocity vector to convert from field-relative to robot-relative
.rotateBy(robotPose.getRotation().unaryMinus())
.getAngle(), chassisMass, robotMass, chassisCenterOfGravity, config),
.getAngle(),
chassisMass,
robotMass,
chassisCenterOfGravity,
config),
deltaV.getAngle());
// Calculate the maximum achievable velocity by the next loop cycle.
@@ -251,16 +283,21 @@ public class SwerveMath
* @param left True = furthest left, False = furthest right.
* @return Module location which is the furthest from center and abides by parameters.
*/
public static SwerveModuleConfiguration getSwerveModule(SwerveModule[] modules, boolean front, boolean left)
public static SwerveModuleConfiguration getSwerveModule(
SwerveModule[] modules, boolean front, boolean left)
{
Translation2d target = modules[0].configuration.moduleLocation, current, temp;
SwerveModuleConfiguration configuration = modules[0].configuration;
for (SwerveModule module : modules)
{
current = module.configuration.moduleLocation;
temp = front ? (target.getY() >= current.getY() ? current : target)
temp =
front
? (target.getY() >= current.getY() ? current : target)
: (target.getY() <= current.getY() ? current : target);
target = left ? (target.getX() >= temp.getX() ? temp : target)
target =
left
? (target.getX() >= temp.getX() ? temp : target)
: (target.getX() <= temp.getX() ? temp : target);
configuration = current.equals(target) ? module.configuration : configuration;
}

3
swervelib/math/SwerveModuleState2.java
View File

@@ -36,7 +36,8 @@ public class SwerveModuleState2 extends SwerveModuleState
* @param angle The angle of the module.
* @param omegaRadPerSecond The angular velocity of the module.
*/
public SwerveModuleState2(double speedMetersPerSecond, Rotation2d angle, double omegaRadPerSecond)
public SwerveModuleState2(
double speedMetersPerSecond, Rotation2d angle, double omegaRadPerSecond)
{
this.speedMetersPerSecond = speedMetersPerSecond;
this.angle = angle;

26
swervelib/motors/SparkMaxSwerve.java
View File

@@ -38,7 +38,6 @@ public class SparkMaxSwerve extends SwerveMotor
*/
private boolean factoryDefaultOccurred = false;
/**
* Initialize the swerve motor.
*
@@ -54,7 +53,8 @@ public class SparkMaxSwerve extends SwerveMotor
encoder = motor.getEncoder();
pid = motor.getPIDController();
pid.setFeedbackDevice(encoder); // Configure feedback of the PID controller as the integrated encoder.
pid.setFeedbackDevice(
encoder); // Configure feedback of the PID controller as the integrated encoder.
motor.setCANTimeout(0); // Spin off configurations in a different thread.
}
@@ -179,12 +179,17 @@ public class SparkMaxSwerve extends SwerveMotor
encoder.setPositionConversionFactor(positionConversionFactor);
encoder.setVelocityConversionFactor(positionConversionFactor / 60);
// Taken from https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
configureCANStatusFrames(10, 20, 20, 500, 500);
} else
{
absoluteEncoder.setPositionConversionFactor(positionConversionFactor);
absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60);
if (!isAttachedAbsoluteEncoder())
{
configureCANStatusFrames(10, 20, 20, 500, 500);
}
}
}
@@ -196,7 +201,8 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void configurePIDF(PIDFConfig config)
{
int pidSlot = isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
int pidSlot =
isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
pid.setP(config.p, pidSlot);
pid.setI(config.i, pidSlot);
pid.setD(config.d, pidSlot);
@@ -228,7 +234,8 @@ public class SparkMaxSwerve extends SwerveMotor
* @param CANStatus3 Analog Sensor Voltage, Analog Sensor Velocity, Analog Sensor Position
* @param CANStatus4 Alternate Encoder Velocity, Alternate Encoder Position
*/
public void configureCANStatusFrames(int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
public void configureCANStatusFrames(
int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
{
motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0);
motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1);
@@ -290,8 +297,13 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward)
{
int pidSlot = isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
pid.setReference(setpoint, isDriveMotor ? ControlType.kVelocity : ControlType.kPosition, pidSlot, feedforward);
int pidSlot =
isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
pid.setReference(
setpoint,
isDriveMotor ? ControlType.kVelocity : ControlType.kPosition,
pidSlot,
feedforward);
}
/**

28
swervelib/motors/TalonFXSwerve.java
View File

@@ -5,7 +5,7 @@ import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import frc.robot.Robot;
import edu.wpi.first.wpilibj.RobotBase;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.simulation.ctre.PhysicsSim;
@@ -55,7 +55,7 @@ public class TalonFXSwerve extends SwerveMotor
factoryDefaults();
clearStickyFaults();
if (!Robot.isReal())
if (RobotBase.isSimulation())
{
PhysicsSim.getInstance().addTalonFX(motor, .25, 6800);
}
@@ -106,7 +106,6 @@ public class TalonFXSwerve extends SwerveMotor
motor.clearStickyFaults();
}
/**
* Set the absolute encoder to be a compatible absolute encoder.
*
@@ -122,15 +121,17 @@ public class TalonFXSwerve extends SwerveMotor
/**
* Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
*
* @param positionConversionFactor The conversion factor to apply for position. <p><br /> Degrees: <br />
* @param positionConversionFactor The conversion factor to apply for position.
* <p><br>
* Degrees: <br>
* <code>
* 360 / (angleGearRatio * encoderTicksPerRotation)
* </code><br /></p>
* <p><br />Meters:<br />
* </code><br>
* <p><br>
* Meters:<br>
* <code>
* (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation)
* </code>
* </p>
*/
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
@@ -265,9 +266,8 @@ public class TalonFXSwerve extends SwerveMotor
*/
public double convertToNativeSensorUnits(double setpoint)
{
setpoint = isDriveMotor ?
setpoint * .1 :
placeInAppropriate0To360Scope(getRawPosition(), setpoint);
setpoint =
isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(getRawPosition(), setpoint);
return setpoint / positionConversionFactor;
}
@@ -280,14 +280,15 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward)
{
if (!Robot.isReal())
if (RobotBase.isSimulation())
{
PhysicsSim.getInstance().run();
}
burnFlash();
motor.set(isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
motor.set(
isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
convertToNativeSensorUnits(setpoint),
DemandType.ArbitraryFeedForward,
feedforward * -0.3);
@@ -334,7 +335,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setPosition(double position)
{
if (!absoluteEncoder && Robot.isReal())
if (!absoluteEncoder && !RobotBase.isSimulation())
{
motor.setSelectedSensorPosition(convertToNativeSensorUnits(position));
}
@@ -400,5 +401,4 @@ public class TalonFXSwerve extends SwerveMotor
{
return absoluteEncoder;
}
}

21
swervelib/motors/TalonSRXSwerve.java
View File

@@ -6,7 +6,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import frc.robot.Robot;
import edu.wpi.first.wpilibj.RobotBase;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
@@ -20,14 +20,14 @@ public class TalonSRXSwerve extends SwerveMotor
* Factory default already occurred.
*/
private final boolean factoryDefaultOccurred = false;
/**
* TalonSRX motor controller.
*/
WPI_TalonSRX motor;
/**
* Whether the absolute encoder is integrated.
*/
private final boolean absoluteEncoder = false;
/**
* TalonSRX motor controller.
*/
WPI_TalonSRX motor;
/**
* The position conversion factor.
*/
@@ -82,7 +82,6 @@ public class TalonSRXSwerve extends SwerveMotor
motor.clearStickyFaults();
}
/**
* Set the absolute encoder to be a compatible absolute encoder.
*
@@ -187,7 +186,8 @@ public class TalonSRXSwerve extends SwerveMotor
{
burnFlash();
motor.set(isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
motor.set(
isDriveMotor ? ControlMode.Velocity : ControlMode.Position,
convertToNativeSensorUnits(setpoint),
DemandType.ArbitraryFeedForward,
feedforward * -0.3);
@@ -234,7 +234,7 @@ public class TalonSRXSwerve extends SwerveMotor
@Override
public void setPosition(double position)
{
if (!absoluteEncoder && Robot.isReal())
if (!absoluteEncoder && !RobotBase.isSimulation())
{
motor.setSelectedSensorPosition(convertToNativeSensorUnits(position));
}
@@ -354,9 +354,8 @@ public class TalonSRXSwerve extends SwerveMotor
*/
public double convertToNativeSensorUnits(double setpoint)
{
setpoint = isDriveMotor ?
setpoint * .1 :
placeInAppropriate0To360Scope(getRawPosition(), setpoint);
setpoint =
isDriveMotor ? setpoint * .1 : placeInAppropriate0To360Scope(getRawPosition(), setpoint);
return setpoint / positionConversionFactor;
}
}

1
swervelib/parser/PIDFConfig.java
View File

@@ -106,4 +106,3 @@ public class PIDFConfig
return new PIDController(p, i, d);
}
}

12
swervelib/parser/SwerveControllerConfiguration.java
View File

@@ -23,7 +23,8 @@ public class SwerveControllerConfiguration
/**
* hypotenuse deadband for the robot angle control joystick.
*/
public final double angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick.
public final double
angleJoyStickRadiusDeadband; // Deadband for the minimum hypot for the heading joystick.
/**
* Construct the swerve controller configuration.
@@ -32,11 +33,15 @@ public class SwerveControllerConfiguration
* @param headingPIDF Heading PIDF configuration.
* @param angleJoyStickRadiusDeadband Deadband on radius of angle joystick.
*/
public SwerveControllerConfiguration(SwerveDriveConfiguration driveCfg, PIDFConfig headingPIDF,
public SwerveControllerConfiguration(
SwerveDriveConfiguration driveCfg,
PIDFConfig headingPIDF,
double angleJoyStickRadiusDeadband)
{
this.maxSpeed = driveCfg.maxSpeed;
this.maxAngularVelocity = calculateMaxAngularVelocity(driveCfg.maxSpeed,
this.maxAngularVelocity =
calculateMaxAngularVelocity(
driveCfg.maxSpeed,
Math.abs(driveCfg.moduleLocationsMeters[0].getX()),
Math.abs(driveCfg.moduleLocationsMeters[0].getY()));
this.headingPIDF = headingPIDF;
@@ -54,5 +59,4 @@ public class SwerveControllerConfiguration
{
this(driveCfg, headingPIDF, 0.5);
}
}

5
swervelib/parser/SwerveDriveConfiguration.java
View File

@@ -43,7 +43,10 @@ public class SwerveDriveConfiguration
* @param maxSpeed Max speed of the robot in meters per second.
* @param invertedIMU Invert the IMU.
*/
public SwerveDriveConfiguration(SwerveModuleConfiguration[] moduleConfigs, SwerveIMU swerveIMU, double maxSpeed,
public SwerveDriveConfiguration(
SwerveModuleConfiguration[] moduleConfigs,
SwerveIMU swerveIMU,
double maxSpeed,
boolean invertedIMU)
{
this.moduleCount = moduleConfigs.length;

53
swervelib/parser/SwerveModuleConfiguration.java
View File

@@ -65,7 +65,6 @@ public class SwerveModuleConfiguration
*/
public SwerveAbsoluteEncoder absoluteEncoder;
/**
* Construct a configuration object for swerve modules.
*
@@ -83,12 +82,19 @@ public class SwerveModuleConfiguration
* @param maxSpeed Maximum speed in meters per second.
* @param physicalCharacteristics Physical characteristics of the swerve module.
*/
public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor,
SwerveAbsoluteEncoder absoluteEncoder, double angleOffset,
public SwerveModuleConfiguration(
SwerveMotor driveMotor,
SwerveMotor angleMotor,
SwerveAbsoluteEncoder absoluteEncoder,
double angleOffset,
double xMeters,
double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF, double maxSpeed,
double yMeters,
PIDFConfig anglePIDF,
PIDFConfig velocityPIDF,
double maxSpeed,
SwerveModulePhysicalCharacteristics physicalCharacteristics,
boolean absoluteEncoderInverted, boolean driveMotorInverted,
boolean absoluteEncoderInverted,
boolean driveMotorInverted,
boolean angleMotorInverted)
{
this.driveMotor = driveMotor;
@@ -102,11 +108,12 @@ public class SwerveModuleConfiguration
this.anglePIDF = anglePIDF;
this.velocityPIDF = velocityPIDF;
this.maxSpeed = maxSpeed;
this.angleKV = calculateAngleKV(physicalCharacteristics.optimalVoltage,
this.angleKV =
calculateAngleKV(
physicalCharacteristics.optimalVoltage,
physicalCharacteristics.angleMotorFreeSpeedRPM,
physicalCharacteristics.angleGearRatio);
this.physicalCharacteristics = physicalCharacteristics;
}
/**
@@ -124,12 +131,20 @@ public class SwerveModuleConfiguration
* @param maxSpeed Maximum robot speed in meters per second.
* @param physicalCharacteristics Physical characteristics of the swerve module.
*/
public SwerveModuleConfiguration(SwerveMotor driveMotor, SwerveMotor angleMotor,
SwerveAbsoluteEncoder absoluteEncoder, double angleOffset,
double xMeters, double yMeters, PIDFConfig anglePIDF, PIDFConfig velocityPIDF,
double maxSpeed, SwerveModulePhysicalCharacteristics physicalCharacteristics)
public SwerveModuleConfiguration(
SwerveMotor driveMotor,
SwerveMotor angleMotor,
SwerveAbsoluteEncoder absoluteEncoder,
double angleOffset,
double xMeters,
double yMeters,
PIDFConfig anglePIDF,
PIDFConfig velocityPIDF,
double maxSpeed,
SwerveModulePhysicalCharacteristics physicalCharacteristics)
{
this(driveMotor,
this(
driveMotor,
angleMotor,
absoluteEncoder,
angleOffset,
@@ -144,7 +159,6 @@ public class SwerveModuleConfiguration
false);
}
/**
* Create the drive feedforward for swerve modules.
*
@@ -153,10 +167,11 @@ public class SwerveModuleConfiguration
public SimpleMotorFeedforward createDriveFeedforward()
{
double kv = physicalCharacteristics.optimalVoltage / maxSpeed;
///^ Volt-seconds per meter (max voltage divided by max speed)
double ka = physicalCharacteristics.optimalVoltage /
calculateMaxAcceleration(physicalCharacteristics.wheelGripCoefficientOfFriction);
///^ Volt-seconds^2 per meter (max voltage divided by max accel)
/// ^ Volt-seconds per meter (max voltage divided by max speed)
double ka =
physicalCharacteristics.optimalVoltage
/ calculateMaxAcceleration(physicalCharacteristics.wheelGripCoefficientOfFriction);
/// ^ Volt-seconds^2 per meter (max voltage divided by max accel)
return new SimpleMotorFeedforward(0, kv, ka);
}
@@ -168,7 +183,9 @@ public class SwerveModuleConfiguration
*/
public double getPositionEncoderConversion(boolean isDriveMotor)
{
return isDriveMotor ? calculateMetersPerRotation(physicalCharacteristics.wheelDiameter,
return isDriveMotor
? calculateMetersPerRotation(
physicalCharacteristics.wheelDiameter,
physicalCharacteristics.driveGearRatio,
physicalCharacteristics.driveEncoderPulsePerRotation)
: calculateDegreesPerSteeringRotation(

42
swervelib/parser/SwerveModulePhysicalCharacteristics.java
View File

@@ -66,12 +66,18 @@ public class SwerveModulePhysicalCharacteristics
* @param driveEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders.
* @param angleEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders.
*/
public SwerveModulePhysicalCharacteristics(double driveGearRatio, double angleGearRatio,
public SwerveModulePhysicalCharacteristics(
double driveGearRatio,
double angleGearRatio,
double angleMotorFreeSpeedRPM,
double wheelDiameter, double wheelGripCoefficientOfFriction,
double optimalVoltage, int driveMotorCurrentLimit,
int angleMotorCurrentLimit, double driveMotorRampRate,
double angleMotorRampRate, int driveEncoderPulsePerRotation,
double wheelDiameter,
double wheelGripCoefficientOfFriction,
double optimalVoltage,
int driveMotorCurrentLimit,
int angleMotorCurrentLimit,
double driveMotorRampRate,
double angleMotorRampRate,
int driveEncoderPulsePerRotation,
int angleEncoderPulsePerRotation)
{
this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction;
@@ -106,12 +112,28 @@ public class SwerveModulePhysicalCharacteristics
* @param driveEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders.
* @param angleEncoderPulsePerRotation The number of encoder pulses per motor rotation, 1 for integrated encoders.
*/
public SwerveModulePhysicalCharacteristics(double driveGearRatio, double angleGearRatio,
public SwerveModulePhysicalCharacteristics(
double driveGearRatio,
double angleGearRatio,
double angleMotorFreeSpeedRPM,
double wheelDiameter, double driveMotorRampRate, double angleMotorRampRate,
int driveEncoderPulsePerRotation, int angleEncoderPulsePerRotation)
double wheelDiameter,
double driveMotorRampRate,
double angleMotorRampRate,
int driveEncoderPulsePerRotation,
int angleEncoderPulsePerRotation)
{
this(driveGearRatio, angleGearRatio, angleMotorFreeSpeedRPM, wheelDiameter, 1.19, 12,
40, 20, driveMotorRampRate, angleMotorRampRate, driveEncoderPulsePerRotation, angleEncoderPulsePerRotation);
this(
driveGearRatio,
angleGearRatio,
angleMotorFreeSpeedRPM,
wheelDiameter,
1.19,
12,
40,
20,
driveMotorRampRate,
angleMotorRampRate,
driveEncoderPulsePerRotation,
angleEncoderPulsePerRotation);
}
}

44
swervelib/parser/SwerveParser.java
View File

@@ -45,7 +45,6 @@ public class SwerveParser
*/
public static ModuleJson[] moduleJsons;
/**
* Construct a swerve parser. Will throw an error if there is a missing file.
*
@@ -55,12 +54,21 @@ public class SwerveParser
public SwerveParser(File directory) throws IOException
{
checkDirectory(directory);
swerveDriveJson = new ObjectMapper().readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class);
controllerPropertiesJson = new ObjectMapper().readValue(new File(directory, "controllerproperties.json"),
ControllerPropertiesJson.class);
pidfPropertiesJson = new ObjectMapper().readValue(new File(directory, "modules/pidfproperties.json"),
PIDFPropertiesJson.class);
physicalPropertiesJson = new ObjectMapper().readValue(new File(directory, "modules/physicalproperties.json"),
swerveDriveJson =
new ObjectMapper()
.readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class);
controllerPropertiesJson =
new ObjectMapper()
.readValue(
new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class);
pidfPropertiesJson =
new ObjectMapper()
.readValue(
new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class);
physicalPropertiesJson =
new ObjectMapper()
.readValue(
new File(directory, "modules/physicalproperties.json"),
PhysicalPropertiesJson.class);
moduleJsons = new ModuleJson[swerveDriveJson.modules.length];
for (int i = 0; i < moduleJsons.length; i++)
@@ -79,8 +87,8 @@ public class SwerveParser
* @param driveConfiguration {@link SwerveDriveConfiguration} to pull from.
* @return {@link SwerveModuleConfiguration} based on the file.
*/
public static SwerveModule getModuleConfigurationByName(String name,
SwerveDriveConfiguration driveConfiguration)
public static SwerveModule getModuleConfigurationByName(
String name, SwerveDriveConfiguration driveConfiguration)
{
return driveConfiguration.modules[moduleConfigs.get(name + ".json")];
}
@@ -124,21 +132,27 @@ public class SwerveParser
public SwerveDrive createSwerveDrive()
{
double maxSpeedMPS = Units.feetToMeters(swerveDriveJson.maxSpeed);
SwerveModuleConfiguration[] moduleConfigurations = new SwerveModuleConfiguration[moduleJsons.length];
SwerveModuleConfiguration[] moduleConfigurations =
new SwerveModuleConfiguration[moduleJsons.length];
for (int i = 0; i < moduleConfigurations.length; i++)
{
ModuleJson module = moduleJsons[i];
moduleConfigurations[i] = module.createModuleConfiguration(pidfPropertiesJson.angle, pidfPropertiesJson.drive,
moduleConfigurations[i] =
module.createModuleConfiguration(
pidfPropertiesJson.angle,
pidfPropertiesJson.drive,
maxSpeedMPS,
physicalPropertiesJson.createPhysicalProperties(
swerveDriveJson.optimalVoltage));
physicalPropertiesJson.createPhysicalProperties(swerveDriveJson.optimalVoltage));
}
SwerveDriveConfiguration swerveDriveConfiguration = new SwerveDriveConfiguration(moduleConfigurations,
SwerveDriveConfiguration swerveDriveConfiguration =
new SwerveDriveConfiguration(
moduleConfigurations,
swerveDriveJson.imu.createIMU(),
maxSpeedMPS,
swerveDriveJson.invertedIMU);
return new SwerveDrive(swerveDriveConfiguration,
return new SwerveDrive(
swerveDriveConfiguration,
controllerPropertiesJson.createControllerConfiguration(swerveDriveConfiguration));
}
}

6
swervelib/parser/json/ControllerPropertiesJson.java
View File

@@ -25,8 +25,10 @@ public class ControllerPropertiesJson
* @param driveConfiguration {@link SwerveDriveConfiguration} parsed configuration.
* @return {@link SwerveControllerConfiguration} object based on parsed data.
*/
public SwerveControllerConfiguration createControllerConfiguration(SwerveDriveConfiguration driveConfiguration)
public SwerveControllerConfiguration createControllerConfiguration(
SwerveDriveConfiguration driveConfiguration)
{
return new SwerveControllerConfiguration(driveConfiguration, heading, angleJoystickRadiusDeadband);
return new SwerveControllerConfiguration(
driveConfiguration, heading, angleJoystickRadiusDeadband);
}
}

4
swervelib/parser/json/DeviceJson.java
View File

@@ -102,7 +102,6 @@ public class DeviceJson
return new TalonSRXSwerve(id, isDriveMotor);
default:
throw new RuntimeException(type + " is not a recognized absolute encoder type.");
}
}
@@ -121,6 +120,7 @@ public class DeviceJson
case "none":
return null;
}
throw new RuntimeException("Could not create absolute encoder from data port of " + type + " id " + id);
throw new RuntimeException(
"Could not create absolute encoder from data port of " + type + " id " + id);
}
}

21
swervelib/parser/json/ModuleJson.java
View File

@@ -53,7 +53,9 @@ public class ModuleJson
* @param physicalCharacteristics Physical characteristics of the swerve module.
* @return {@link SwerveModuleConfiguration} based on the provided data and parsed data.
*/
public SwerveModuleConfiguration createModuleConfiguration(PIDFConfig anglePIDF, PIDFConfig velocityPIDF,
public SwerveModuleConfiguration createModuleConfiguration(
PIDFConfig anglePIDF,
PIDFConfig velocityPIDF,
double maxSpeed,
SwerveModulePhysicalCharacteristics physicalCharacteristics)
{
@@ -67,10 +69,19 @@ public class ModuleJson
angleMotor.setAbsoluteEncoder(absEncoder);
}
return new SwerveModuleConfiguration(drive.createMotor(true), angleMotor, absEncoder,
absoluteEncoderOffset, Units.inchesToMeters(location.x),
Units.inchesToMeters(location.y), anglePIDF, velocityPIDF, maxSpeed,
physicalCharacteristics, absoluteEncoderInverted, inverted.drive,
return new SwerveModuleConfiguration(
drive.createMotor(true),
angleMotor,
absEncoder,
absoluteEncoderOffset,
Units.inchesToMeters(location.x),
Units.inchesToMeters(location.y),
anglePIDF,
velocityPIDF,
maxSpeed,
physicalCharacteristics,
absoluteEncoderInverted,
inverted.drive,
inverted.angle);
}
}

16
swervelib/parser/json/PhysicalPropertiesJson.java
View File

@@ -46,11 +46,18 @@ public class PhysicalPropertiesJson
*/
public SwerveModulePhysicalCharacteristics createPhysicalProperties(double optimalVoltage)
{
return new SwerveModulePhysicalCharacteristics(gearRatio.drive, gearRatio.angle, angleMotorFreeSpeedRPM,
Units.inchesToMeters(wheelDiameter), wheelGripCoefficientOfFriction,
return new SwerveModulePhysicalCharacteristics(
gearRatio.drive,
gearRatio.angle,
angleMotorFreeSpeedRPM,
Units.inchesToMeters(wheelDiameter),
wheelGripCoefficientOfFriction,
optimalVoltage,
currentLimit.drive, currentLimit.angle, rampRate.drive,
rampRate.angle, encoderPulsePerRotation.drive,
currentLimit.drive,
currentLimit.angle,
rampRate.drive,
rampRate.angle,
encoderPulsePerRotation.drive,
encoderPulsePerRotation.angle);
}
}
@@ -88,7 +95,6 @@ class MotorConfigDouble
this.angle = angle;
this.drive = drive;
}
}
/**

1
swervelib/parser/json/modules/LocationJson.java
View File

@@ -4,7 +4,6 @@ package swervelib.parser.json.modules;
* Location JSON parsed class. Used to access the JSON data. Module locations, in inches, as distances to the center of
* the robot. Positive x is torwards the robot front, and * +y is torwards robot left.
*/
public class LocationJson
{

5
swervelib/simulation/SwerveIMUSimulation.java
View File

@@ -86,7 +86,10 @@ public class SwerveIMUSimulation
* @param modulePoses {@link Pose2d} representing the swerve modules.
* @param field {@link Field2d} to update.
*/
public void updateOdometry(SwerveKinematics2 kinematics, SwerveModuleState2[] states, Pose2d[] modulePoses,
public void updateOdometry(
SwerveKinematics2 kinematics,
SwerveModuleState2[] states,
Pose2d[] modulePoses,
Field2d field)
{
angle += kinematics.toChassisSpeeds(states).omegaRadiansPerSecond * (timer.get() - lastTime);

3
swervelib/simulation/SwerveModuleSimulation.java
View File

@@ -73,7 +73,8 @@ public class SwerveModuleSimulation
*/
public SwerveModulePosition getPosition()
{
return new SwerveModulePosition(fakePos, state.angle.plus(new Rotation2d(state.omegaRadPerSecond * dt)));
return new SwerveModulePosition(
fakePos, state.angle.plus(new Rotation2d(state.omegaRadPerSecond * dt)));
}
/**

23
swervelib/simulation/ctre/PhysicsSim.java
View File

@@ -16,6 +16,8 @@ public class PhysicsSim
/**
* Gets the robot simulator instance.
*
* @return {@link PhysicsSim} instance.
*/
public static PhysicsSim getInstance()
{
@@ -25,7 +27,8 @@ public class PhysicsSim
/* scales a random domain of [0, 2pi] to [min, max] while prioritizing the peaks */
static double random(double min, double max)
{
return (max - min) / 2 * Math.sin(Math.IEEEremainder(Math.random(), 2 * 3.14159)) + (max + min) / 2;
return (max - min) / 2 * Math.sin(Math.IEEEremainder(Math.random(), 2 * 3.14159))
+ (max + min) / 2;
}
static double random(double max)
@@ -53,11 +56,16 @@ public class PhysicsSim
* @param fullVel The maximum motor velocity, in ticks per 100ms
* @param sensorPhase The phase of the TalonSRX sensors
*/
public void addTalonSRX(TalonSRX talon, final double accelToFullTime, final double fullVel, final boolean sensorPhase)
public void addTalonSRX(
TalonSRX talon,
final double accelToFullTime,
final double fullVel,
final boolean sensorPhase)
{
if (talon != null)
{
TalonSRXSimProfile simTalon = new TalonSRXSimProfile(talon, accelToFullTime, fullVel, sensorPhase);
TalonSRXSimProfile simTalon =
new TalonSRXSimProfile(talon, accelToFullTime, fullVel, sensorPhase);
_simProfiles.add(simTalon);
}
}
@@ -82,11 +90,16 @@ public class PhysicsSim
* @param fullVel The maximum motor velocity, in ticks per 100ms
* @param sensorPhase The phase of the TalonFX sensors
*/
public void addTalonFX(TalonFX falcon, final double accelToFullTime, final double fullVel, final boolean sensorPhase)
public void addTalonFX(
TalonFX falcon,
final double accelToFullTime,
final double fullVel,
final boolean sensorPhase)
{
if (falcon != null)
{
TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, accelToFullTime, fullVel, sensorPhase);
TalonFXSimProfile simFalcon =
new TalonFXSimProfile(falcon, accelToFullTime, fullVel, sensorPhase);
_simProfiles.add(simFalcon);
}
}

12
swervelib/simulation/ctre/TalonFXSimProfile.java
View File

@@ -31,7 +31,10 @@ class TalonFXSimProfile extends SimProfile
* @param fullVel The maximum motor velocity, in ticks per 100ms
* @param sensorPhase The phase of the TalonFX sensors
*/
public TalonFXSimProfile(final TalonFX falcon, final double accelToFullTime, final double fullVel,
public TalonFXSimProfile(
final TalonFX falcon,
final double accelToFullTime,
final double fullVel,
final boolean sensorPhase)
{
this._falcon = falcon;
@@ -42,9 +45,10 @@ class TalonFXSimProfile extends SimProfile
/**
* Runs the simulation profile.
* <p>
* This uses very rudimentary physics simulation and exists to allow users to test features of our products in
* simulation using our examples out of the box. Users may modify this to utilize more accurate physics simulation.
*
* <p>This uses very rudimentary physics simulation and exists to allow users to test features of
* our products in simulation using our examples out of the box. Users may modify this to utilize more accurate
* physics simulation.
*/
public void run()
{

12
swervelib/simulation/ctre/TalonSRXSimProfile.java
View File

@@ -31,7 +31,10 @@ class TalonSRXSimProfile extends SimProfile
* @param fullVel The maximum motor velocity, in ticks per 100ms
* @param sensorPhase The phase of the TalonSRX sensors
*/
public TalonSRXSimProfile(final TalonSRX talon, final double accelToFullTime, final double fullVel,
public TalonSRXSimProfile(
final TalonSRX talon,
final double accelToFullTime,
final double fullVel,
final boolean sensorPhase)
{
this._talon = talon;
@@ -42,9 +45,10 @@ class TalonSRXSimProfile extends SimProfile
/**
* Runs the simulation profile.
* <p>
* This uses very rudimentary physics simulation and exists to allow users to test features of our products in
* simulation using our examples out of the box. Users may modify this to utilize more accurate physics simulation.
*
* <p>This uses very rudimentary physics simulation and exists to allow users to test features of
* our products in simulation using our examples out of the box. Users may modify this to utilize more accurate
* physics simulation.
*/
public void run()
{

8
swervelib/simulation/ctre/VictorSPXSimProfile.java
View File

@@ -1,6 +1,5 @@
package swervelib.simulation.ctre;
import static swervelib.simulation.ctre.PhysicsSim.random;
import com.ctre.phoenix.motorcontrol.can.VictorSPX;
@@ -26,9 +25,10 @@ class VictorSPXSimProfile extends SimProfile
/**
* Runs the simulation profile.
* <p>
* This uses very rudimentary physics simulation and exists to allow users to test features of our products in
* simulation using our examples out of the box. Users may modify this to utilize more accurate physics simulation.
*
* <p>This uses very rudimentary physics simulation and exists to allow users to test features of
* our products in simulation using our examples out of the box. Users may modify this to utilize more accurate
* physics simulation.
*/
public void run()
{