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

Jump to bottom

[#5][#9] Added CTRE Swerve #14

Merged
merged 1 commit into from
Jan 10, 2024
Merged

[#5][#9] Added CTRE Swerve #14

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
132 changes: 130 additions & 2 deletions src/main/java/frc/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,21 +4,149 @@

package frc.robot;

import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants.SteerFeedbackType;
import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstantsFactory;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.util.Units;
import frc.robot.Constants.RobotMap.CAN;
import frc.robot.subsystems.Swerve;

/** Add your docs here. */
public class Constants {
public class drivetrainConstants {

public class drivetrainConstants { //TODO Get new for new robot
public static final double MaxSpeed = 6; // 6 meters per second desired top speed
private static final double WHEELBASE = TunerConstants.kFrontLeftXPosInches*2; //2 * x distance from center of robot to wheel
public static final double MaxAngularRate = 2*Math.PI*( //convert to radians per second
TunerConstants.kSpeedAt12VoltsMps / // free speed
Math.PI*Math.sqrt(2*Math.pow(WHEELBASE, 2)) // circumference of circle with radius of wheelbase
);
public static final double RotationMultipler = 0.5; // TODO tune
}

public class RobotMap {
public class CAN {
// Front Left
private static final int kFrontLeftDriveMotorId = 1;
private static final int kFrontLeftSteerMotorId = 2;
private static final int kFrontLeftEncoderId = 31;

// Front Right
private static final int kFrontRightDriveMotorId = 3;
private static final int kFrontRightSteerMotorId = 4;
private static final int kFrontRightEncoderId = 33;

// Back Left
private static final int kBackLeftDriveMotorId = 7;
private static final int kBackLeftSteerMotorId = 8;
private static final int kBackLeftEncoderId = 34;

// Back Right
private static final int kBackRightDriveMotorId = 5;
private static final int kBackRightSteerMotorId = 6;
private static final int kBackRightEncoderId = 32;

public static final int PigeonId = 23;
}
}

public static class ControllerConstants {
public static final int DriverControllerPort = 0;
public static final int CopilotControllerPort = 1;
}

public class TunerConstants {
// Both sets of gains need to be tuned to your individual robot
// The steer motor uses MotionMagicVoltage control
private static final Slot0Configs steerGains = new Slot0Configs()
.withKP(100).withKI(0).withKD(0.05)
.withKS(0).withKV(1.5).withKA(0);
// When using closed-loop control, the drive motor uses:
// - VelocityVoltage, if DrivetrainConstants.SupportsPro is false (default)
// - VelocityTorqueCurrentFOC, if DrivetrainConstants.SupportsPro is true
private static final Slot0Configs driveGains = new Slot0Configs()
.withKP(3).withKI(0).withKD(0)
.withKS(0).withKV(0).withKA(0);

// The stator current at which the wheels start to slip;
// This needs to be tuned to your individual robot
private static final double kSlipCurrentA = 300.0;

// Theoretical free speed (m/s) at 12v applied output;
// This needs to be tuned to your individual robot
private static final double kSpeedAt12VoltsMps = 6.0;

// Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
// This may need to be tuned to your individual robot
private static final double kCoupleRatio = 3.5714285714285716;

private static final double kDriveGearRatio = 6.746031746031747;
private static final double kSteerGearRatio = 21.428571428571427;
private static final double kWheelRadiusInches = 2;

private static final boolean kSteerMotorReversed = true;
private static final boolean kInvertLeftSide = false;
private static final boolean kInvertRightSide = true;

private static final String kCANbusName = "Canivore";
public static final int kPigeonId = 23;


// These are only used for simulation
private static final double kSteerInertia = 0.00001;
private static final double kDriveInertia = 0.001;

private static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
.withPigeon2Id(kPigeonId)
.withCANbusName(kCANbusName);

private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory()
.withDriveMotorGearRatio(kDriveGearRatio)
.withSteerMotorGearRatio(kSteerGearRatio)
.withWheelRadius(kWheelRadiusInches)
.withSlipCurrent(kSlipCurrentA)
.withSteerMotorGains(steerGains)
.withDriveMotorGains(driveGains)
.withSpeedAt12VoltsMps(kSpeedAt12VoltsMps)
.withSteerInertia(kSteerInertia)
.withDriveInertia(kDriveInertia)
.withFeedbackSource(SteerFeedbackType.FusedCANcoder)
.withCouplingGearRatio(kCoupleRatio)
.withSteerMotorInverted(kSteerMotorReversed);

// OFFSETS
private static final double kFrontLeftEncoderOffset = -0.22314453125;
private static final double kFrontLeftXPosInches = 11.25;
private static final double kFrontLeftYPosInches = 11.25;

private static final double kFrontRightEncoderOffset = 0.379150390625;
private static final double kFrontRightXPosInches = 11.25;
private static final double kFrontRightYPosInches = -11.25;

private static final double kBackLeftEncoderOffset = 0.133056640625;
private static final double kBackLeftXPosInches = -11.25;
private static final double kBackLeftYPosInches = 11.25;

private static final double kBackRightEncoderOffset = -0.173828125;
private static final double kBackRightXPosInches = -11.25;
private static final double kBackRightYPosInches = -11.25;


private static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants(
CAN.kFrontLeftSteerMotorId, CAN.kFrontLeftDriveMotorId, CAN.kFrontLeftEncoderId, kFrontLeftEncoderOffset, Units.inchesToMeters(kFrontLeftXPosInches), Units.inchesToMeters(kFrontLeftYPosInches), kInvertLeftSide);
private static final SwerveModuleConstants FrontRight = ConstantCreator.createModuleConstants(
CAN.kFrontRightSteerMotorId, CAN.kFrontRightDriveMotorId, CAN.kFrontRightEncoderId, kFrontRightEncoderOffset, Units.inchesToMeters(kFrontRightXPosInches), Units.inchesToMeters(kFrontRightYPosInches), kInvertRightSide);
private static final SwerveModuleConstants BackLeft = ConstantCreator.createModuleConstants(
CAN.kBackLeftSteerMotorId, CAN.kBackLeftDriveMotorId, CAN.kBackLeftEncoderId, kBackLeftEncoderOffset, Units.inchesToMeters(kBackLeftXPosInches), Units.inchesToMeters(kBackLeftYPosInches), kInvertLeftSide);
private static final SwerveModuleConstants BackRight = ConstantCreator.createModuleConstants(
CAN.kBackRightSteerMotorId, CAN.kBackRightDriveMotorId, CAN.kBackRightEncoderId, kBackRightEncoderOffset, Units.inchesToMeters(kBackRightXPosInches), Units.inchesToMeters(kBackRightYPosInches), kInvertRightSide);

public static final Swerve DriveTrain = new Swerve(DrivetrainConstants, FrontLeft,
FrontRight, BackLeft, BackRight);
}

public class VisionConstants {
//This is a magic number from gridlock, may need to be changed or removed entirely
public static final double PROCESS_LATENCY = 0.0472; // TODO test
Expand Down
41 changes: 37 additions & 4 deletions src/main/java/frc/robot/RobotContainer.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,13 +1,46 @@
package frc.robot;

import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.ControllerConstants;
import frc.robot.Constants.TunerConstants;
import frc.robot.Constants.drivetrainConstants;
import frc.robot.subsystems.Swerve;
import frc.thunder.LightningContainer;

public class RobotContainer extends LightningContainer {
@Override
protected void configureButtonBindings() {}

@Override
protected void configureDefaultCommands() {}
/* Setting up bindings for necessary control of the swerve drive platform */
XboxController driver = new XboxController(ControllerConstants.DriverControllerPort); // My joystick
Swerve drivetrain = TunerConstants.DriveTrain; // My drivetrain

SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric(); //TODO I want field-centric driving in open loop WE NEED TO FIGURE OUT WHAT Change beacuse with open loop is gone
SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
Telemetry logger = new Telemetry(drivetrainConstants.MaxSpeed);


@Override
protected void configureButtonBindings() {
new Trigger(driver::getAButton).whileTrue(drivetrain.applyRequest(() -> brake));
new Trigger(driver::getBButton).whileTrue(drivetrain.applyRequest(() -> point.withModuleDirection(new Rotation2d(-driver.getLeftY(), -driver.getLeftX()))));
new Trigger(driver::getXButton).onTrue(new InstantCommand(() -> drivetrain.zeroGyro())); // TODO create function to reset Heading
drivetrain.registerTelemetry(logger::telemeterize);
}

@Override
protected void configureDefaultCommands() {
drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
drivetrain.applyRequest(() -> drive.withVelocityX(
-MathUtil.applyDeadband(driver.getLeftY(), 0.1) * drivetrainConstants.MaxSpeed) // Drive forward with negative Y (forward)
.withVelocityY(-MathUtil.applyDeadband(driver.getLeftX(), 0.1) * drivetrainConstants.MaxSpeed) // Drive left with negative X (left)
.withRotationalRate(-MathUtil.applyDeadband(driver.getRightX(), 0.1) * drivetrainConstants.MaxAngularRate) // Drive counterclockwise with negative X (left)
));
}

@Override
protected void configureAutonomousCommands() {}
Expand Down
112 changes: 112 additions & 0 deletions src/main/java/frc/robot/Telemetry.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,112 @@
package frc.robot;

import com.ctre.phoenix6.Utils;
import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.networktables.DoubleArrayPublisher;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.util.Color;
import edu.wpi.first.wpilibj.util.Color8Bit;

public class Telemetry {
private final double MaxSpeed;

/**
* Construct a telemetry object, with the specified max speed of the robot
*
* @param maxSpeed Maximum speed in meters per second
*/
public Telemetry(double maxSpeed) {
MaxSpeed = maxSpeed;
}

/* What to publish over networktables for telemetry */
NetworkTableInstance inst = NetworkTableInstance.getDefault();

/* Robot pose for field positioning */
NetworkTable table = inst.getTable("Pose");
DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
StringPublisher fieldTypePub = table.getStringTopic(".type").publish();

/* Robot speeds for general checking */
NetworkTable driveStats = inst.getTable("Drive");
DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish();

/* Keep a reference of the last pose to calculate the speeds */
Pose2d m_lastPose = new Pose2d();
double lastTime = Utils.getCurrentTimeSeconds();

/* Mechanisms to represent the swerve module states */
Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] {
new Mechanism2d(1, 1),
new Mechanism2d(1, 1),
new Mechanism2d(1, 1),
new Mechanism2d(1, 1),
};
/* A direction and length changing ligament for speed representation */
MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] {
m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
};
/* A direction changing and length constant ligament for module direction */
MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] {
m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5)
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5)
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5)
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5)
.append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
};

/* Accept the swerve drive state and telemeterize it to smartdashboard */
public void telemeterize(SwerveDriveState state) {
/* Telemeterize the pose */
Pose2d pose = state.Pose;
fieldTypePub.set("Field2d");
fieldPub.set(new double[] {
pose.getX(),
pose.getY(),
pose.getRotation().getDegrees()
});

/* Telemeterize the robot's general speeds */
double currentTime = Utils.getCurrentTimeSeconds();
double diffTime = currentTime - lastTime;
lastTime = currentTime;
Translation2d distanceDiff = pose.minus(m_lastPose).getTranslation();
m_lastPose = pose;

Translation2d velocities = distanceDiff.div(diffTime);

speed.set(velocities.getNorm());
velocityX.set(velocities.getX());
velocityY.set(velocities.getY());
odomPeriod.set(state.OdometryPeriod);

/* Telemeterize the module's states */
for (int i = 0; i < 4; ++i) {
m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed));

SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
}
}


}
Loading