Skip to content

Commit

Permalink
Add SwerveDrive SysID Support
Browse files Browse the repository at this point in the history
  • Loading branch information
@anivanchen
anivanchen committed Jan 24, 2024
1 parent 77944c2 commit faaed3a
Show file tree
Hide file tree
Showing 5 changed files with 508 additions and 10 deletions.
36 changes: 32 additions & 4 deletions src/main/java/com/stuypulse/robot/constants/Ports.java
View file
Original file line number Diff line number Diff line change
@@ -1,15 +1,43 @@
/************************ PROJECT PHIL ************************/
/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/
/* This work is licensed under the terms of the MIT license. */
/**************************************************************/
/************************ PROJECT SYSID ************************/
/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */
/* Use of this source code is governed by an MIT-style license */
/* that can be found in the repository LICENSE file. */
/***************************************************************/

package com.stuypulse.robot.constants;

/** This file contains the different ports of motors, solenoids and sensors */
public interface Ports {

public interface Gamepad {
int DRIVER = 0;
int OPERATOR = 1;
int DEBUGGER = 2;
}

public interface Swerve {
public interface BackLeft {
int DRIVE = 10;
int TURN = 11;
int ENCODER = 2;
}

public interface BackRight {
int DRIVE = 12;
int TURN = 13;
int ENCODER = 1;
}

public interface FrontRight {
int DRIVE = 14;
int TURN = 15;
int ENCODER = 4;
}

public interface FrontLeft {
int DRIVE = 16;
int TURN = 17;
int ENCODER = 3;
}
}
}
107 changes: 101 additions & 6 deletions src/main/java/com/stuypulse/robot/constants/Settings.java
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,112 @@
/************************ PROJECT PHIL ************************/
/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved.*/
/* This work is licensed under the terms of the MIT license. */
/**************************************************************/
/************************ PROJECT SYSID ************************/
/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */
/* Use of this source code is governed by an MIT-style license */
/* that can be found in the repository LICENSE file. */
/***************************************************************/

package com.stuypulse.robot.constants;

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.util.Units;

import com.pathplanner.lib.util.PIDConstants;

/*-
* File containing tunable settings for every subsystem on the robot.
*
* We use StuyLib's SmartNumber / SmartBoolean in order to have tunable
* values that we can edit on Shuffleboard.
*/
public interface Settings {}
public interface Settings {

public interface Routine {
public enum Mechanism {
SWERVE_TURN,
SWERVE_DRIVE
}

public Mechanism ROUTINE = Mechanism.SWERVE_TURN;
}

public interface Swerve {

double WIDTH = Units.inchesToMeters(26);
double LENGTH = Units.inchesToMeters(26);

SmartNumber MODULE_VELOCITY_DEADBAND =
new SmartNumber("Swerve/Module velocity deadband (m per s)", 0.02);
SmartNumber MAX_MODULE_SPEED =
new SmartNumber("Swerve/Maximum module speed (m per s)", 5.06);
SmartNumber MAX_MODULE_TURN =
new SmartNumber("Swerve/Maximum module turn (rad per s)", 6.28);

public interface Turn {
SmartNumber kP = new SmartNumber("Swerve/Turn/kP", 6.0);
SmartNumber kI = new SmartNumber("Swerve/Turn/kI", 0.0);
SmartNumber kD = new SmartNumber("Swerve/Turn/kD", 0.15);

SmartNumber kS = new SmartNumber("Swerve/Turn/kS", 0.44076);
SmartNumber kV = new SmartNumber("Swerve/Turn/kV", 0.0056191);
SmartNumber kA = new SmartNumber("Swerve/Turn/kA", 0.00042985);
}

public interface Drive {
SmartNumber kP = new SmartNumber("Swerve/Drive/kP", 0.00019162);
SmartNumber kI = new SmartNumber("Swerve/Drive/kI", 0.0);
SmartNumber kD = new SmartNumber("Swerve/Drive/kD", 0.0);

SmartNumber kS = new SmartNumber("Swerve/Drive/kS", 0.36493);
SmartNumber kV = new SmartNumber("Swerve/Drive/kV", 2.448);
SmartNumber kA = new SmartNumber("Swerve/Drive/kA", 0.16408);
}

public interface Motion {

PIDConstants XY = new PIDConstants(0.7, 0, 0.02);
PIDConstants THETA = new PIDConstants(10, 0, 0.1);
}

public interface FrontRight {
String ID = "Front Right";
Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(-153.984375);
Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * -0.5);
}

public interface FrontLeft {
String ID = "Front Left";
Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(149.326172);
Translation2d MODULE_OFFSET = new Translation2d(WIDTH * +0.5, LENGTH * +0.5);
}

public interface BackLeft {
String ID = "Back Left";
Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(71.191406);
Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * +0.5);
}

public interface BackRight {
String ID = "Back Right";
Rotation2d ABSOLUTE_OFFSET = Rotation2d.fromDegrees(45.351562);
Translation2d MODULE_OFFSET = new Translation2d(WIDTH * -0.5, LENGTH * -0.5);
}

public interface Encoder {
public interface Drive {
double WHEEL_DIAMETER = Units.inchesToMeters(4);
double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI;
double GEAR_RATIO = 1.0 / 6.12;

double POSITION_CONVERSION = WHEEL_CIRCUMFERENCE * GEAR_RATIO;
double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0;
}

public interface Turn {
double POSITION_CONVERSION = 21.4285714286;
double VELOCITY_CONVERSION = POSITION_CONVERSION / 60.0;
}
}
}
}
98 changes: 98 additions & 0 deletions src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDriveSysID.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
/************************ PROJECT SYSID ************************/
/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */
/* Use of this source code is governed by an MIT-style license */
/* that can be found in the repository LICENSE file. */
/***************************************************************/

package com.stuypulse.robot.subsystems.swerve;

import static com.stuypulse.robot.constants.Settings.Swerve.*;

import com.stuypulse.robot.constants.Ports;
import com.stuypulse.robot.subsystems.AbstractSysID;

import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Units;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;

public class SwerveDriveSysID extends AbstractSysID {

private final SwerveModule[] modules;
private final SysIdRoutine driveRoutine;

public SwerveDriveSysID() {

modules =
new SwerveModule[] {
new SwerveModule(
FrontRight.ID,
FrontRight.ABSOLUTE_OFFSET,
Ports.Swerve.FrontRight.TURN,
Ports.Swerve.FrontRight.DRIVE,
Ports.Swerve.FrontRight.ENCODER),
new SwerveModule(
FrontLeft.ID,
FrontLeft.ABSOLUTE_OFFSET,
Ports.Swerve.FrontLeft.TURN,
Ports.Swerve.FrontLeft.DRIVE,
Ports.Swerve.FrontLeft.ENCODER),
new SwerveModule(
BackLeft.ID,
BackLeft.ABSOLUTE_OFFSET,
Ports.Swerve.BackLeft.TURN,
Ports.Swerve.BackLeft.DRIVE,
Ports.Swerve.BackLeft.ENCODER),
new SwerveModule(
BackRight.ID,
BackRight.ABSOLUTE_OFFSET,
Ports.Swerve.BackRight.TURN,
Ports.Swerve.BackRight.DRIVE,
Ports.Swerve.BackRight.ENCODER)
};

this.driveRoutine =
new SysIdRoutine(
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
(Measure<Voltage> voltage) -> {
for (SwerveModule module : modules) {
module.setMode(true, false);
module.setDriveVoltage(voltage.in(Units.Volts));
}
},
(log) -> {
for (SwerveModule module : modules) {
log.motor(module.getID())
.voltage(Units.Volts.of(module.getDriveVoltage()))
.linearPosition(
Units.Meters.of(
module.getModulePosition()
.distanceMeters))
.linearVelocity(
Units.MetersPerSecond.of(
module.getModuleState()
.speedMetersPerSecond));
}
},
this));
}

public Command quasistaticForward() {
return driveRoutine.quasistatic(Direction.kForward);
}

public Command quasistaticReverse() {
return driveRoutine.quasistatic(Direction.kReverse);
}

public Command dynamicForward() {
return driveRoutine.dynamic(Direction.kForward);
}

public Command dynamicReverse() {
return driveRoutine.dynamic(Direction.kReverse);
}
}
Loading

0 comments on commit faaed3a

Please sign in to comment.