Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #24 from Team4388/swerve-drive

Browse Source 
Swerve Drive --> Master
This commit is contained in:
Aarav Shah
2023-02-02 22:47:01 -07:00
committed by GitHub
parent e8075c93f0 17d8b24b2a
commit 05165d6ca6
16 changed files with 1019 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/LeftBackCANCoder.json
+47
View File
@@ -0,0 +1,47 @@
{
"Configs": [
{
"Name": "Integrated Sensor",
"Type": "IntegratedSensor",
"Description": "Configures how the sensor is processed.",
"Ordinal": 0,
"Values": {
"Absolute Sensor Range": 0,
"Magnet Offset (deg)": -240.0293,
"Sensor Direction": false,
"Sensor Initialization Strategy": 0
}
},
{
"Name": "Unit Coefficient And String",
"Type": "UnitCoeffGroup",
"Description": "Describes the units to report in API and Self-Test.",
"Ordinal": 0,
"Values": {
"Sensor Coefficient": 0.087890625,
"Sensor Time Base": 1,
"Unit String": "deg"
}
},
{
"Name": "Custom Params",
"Type": "CustomParams",
"Description": "These are arbitrary values not used by the firmware.",
"Ordinal": 0,
"Values": {
"Custom Param 0": 0,
"Custom Param 1": 0
}
},
{
"Name": "Advanced Sensor and Meas",
"Type": "AdvancedSensorMeas",
"Description": "Group of configs relating to advanced sensor features and filters",
"Ordinal": 0,
"Values": {
"Velocity Period": 100,
"Velocity Window": 64
}
}
]
}
config/LeftFrontCANCoder.json
+47
View File
@@ -0,0 +1,47 @@
{
"Configs": [
{
"Name": "Integrated Sensor",
"Type": "IntegratedSensor",
"Description": "Configures how the sensor is processed.",
"Ordinal": 0,
"Values": {
"Absolute Sensor Range": 0,
"Magnet Offset (deg)": -181.230469,
"Sensor Direction": false,
"Sensor Initialization Strategy": 0
}
},
{
"Name": "Unit Coefficient And String",
"Type": "UnitCoeffGroup",
"Description": "Describes the units to report in API and Self-Test.",
"Ordinal": 0,
"Values": {
"Sensor Coefficient": 0.087890625,
"Sensor Time Base": 1,
"Unit String": "deg"
}
},
{
"Name": "Custom Params",
"Type": "CustomParams",
"Description": "These are arbitrary values not used by the firmware.",
"Ordinal": 0,
"Values": {
"Custom Param 0": 0,
"Custom Param 1": 0
}
},
{
"Name": "Advanced Sensor and Meas",
"Type": "AdvancedSensorMeas",
"Description": "Group of configs relating to advanced sensor features and filters",
"Ordinal": 0,
"Values": {
"Velocity Period": 100,
"Velocity Window": 64
}
}
]
}
config/RightBackCANCoder.json
+47
View File
@@ -0,0 +1,47 @@
{
"Configs": [
{
"Name": "Integrated Sensor",
"Type": "IntegratedSensor",
"Description": "Configures how the sensor is processed.",
"Ordinal": 0,
"Values": {
"Absolute Sensor Range": 0,
"Magnet Offset (deg)": -40.86914,
"Sensor Direction": false,
"Sensor Initialization Strategy": 0
}
},
{
"Name": "Unit Coefficient And String",
"Type": "UnitCoeffGroup",
"Description": "Describes the units to report in API and Self-Test.",
"Ordinal": 0,
"Values": {
"Sensor Coefficient": 0.087890625,
"Sensor Time Base": 1,
"Unit String": "deg"
}
},
{
"Name": "Custom Params",
"Type": "CustomParams",
"Description": "These are arbitrary values not used by the firmware.",
"Ordinal": 0,
"Values": {
"Custom Param 0": 0,
"Custom Param 1": 0
}
},
{
"Name": "Advanced Sensor and Meas",
"Type": "AdvancedSensorMeas",
"Description": "Group of configs relating to advanced sensor features and filters",
"Ordinal": 0,
"Values": {
"Velocity Period": 100,
"Velocity Window": 64
}
}
]
}
config/RightFrontCANCoder.json
+47
View File
@@ -0,0 +1,47 @@
{
"Configs": [
{
"Name": "Integrated Sensor",
"Type": "IntegratedSensor",
"Description": "Configures how the sensor is processed.",
"Ordinal": 0,
"Values": {
"Absolute Sensor Range": 0,
"Magnet Offset (deg)": -270.615234,
"Sensor Direction": false,
"Sensor Initialization Strategy": 0
}
},
{
"Name": "Unit Coefficient And String",
"Type": "UnitCoeffGroup",
"Description": "Describes the units to report in API and Self-Test.",
"Ordinal": 0,
"Values": {
"Sensor Coefficient": 0.087890625,
"Sensor Time Base": 1,
"Unit String": "deg"
}
},
{
"Name": "Custom Params",
"Type": "CustomParams",
"Description": "These are arbitrary values not used by the firmware.",
"Ordinal": 0,
"Values": {
"Custom Param 0": 0,
"Custom Param 1": 0
}
},
{
"Name": "Advanced Sensor and Meas",
"Type": "AdvancedSensorMeas",
"Description": "Group of configs relating to advanced sensor features and filters",
"Ordinal": 0,
"Values": {
"Velocity Period": 100,
"Velocity Window": 64
}
}
]
}
src/main/java/frc4388/robot/Constants.java
+115 -15
View File
@@ -7,6 +7,10 @@
package frc4388.robot;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import frc4388.utility.Gains;
import frc4388.utility.LEDPatterns;
/**
@@ -18,24 +22,120 @@ import frc4388.utility.LEDPatterns;
* constants are needed, to reduce verbosity.
*/
public final class Constants {
public static final class DriveConstants {
public static final int DRIVE_PIGEON_ID = 6;
public static final class SwerveDriveConstants {
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class ArmConstants {
public static final int MIN_ARM_LEN = 0;
public static final int MAX_ARM_LEN = 1;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class LEDConstants {
public static final int LED_SPARK_ID = 0;
public static final double ROTATION_SPEED = 2.0;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = 2;
public static final int LEFT_FRONT_STEER_ID = 3;
public static final int LEFT_FRONT_ENCODER_ID = 10;
public static final int RIGHT_FRONT_WHEEL_ID = 4;
public static final int RIGHT_FRONT_STEER_ID = 5;
public static final int RIGHT_FRONT_ENCODER_ID = 11;
public static final int LEFT_BACK_WHEEL_ID = 6;
public static final int LEFT_BACK_STEER_ID = 7;
public static final int LEFT_BACK_ENCODER_ID = 12;
public static final int RIGHT_BACK_WHEEL_ID = 8;
public static final int RIGHT_BACK_STEER_ID = 9;
public static final int RIGHT_BACK_ENCODER_ID = 13;
}
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
public static final class PIDConstants {
public static final int SWERVE_SLOT_IDX = 0;
public static final int SWERVE_PID_LOOP_IDX = 1;
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
}
public static final class AutoConstants {
public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0);
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0,
new TrapezoidProfile.Constraints(0.0, 0.0)
);
public static final double PATH_MAX_VEL = -1; // TODO: find the actual value
public static final double PATH_MAX_ACC = -1; // TODO: find the actual value
}
public static final class Conversions {
public static final int CANCODER_TICKS_PER_ROTATION = 4096;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2.0; // TODO: find the actual value
public static final double MOTOR_REV_PER_WHEEL_REV = 6.12; // TODO: find the actual value
public static final double MOTOR_REV_PER_STEER_REV = 12.8; // TODO: find the actual value
public static final double TICKS_PER_MOTOR_REV = 2048;
public static final double WHEEL_DIAMETER_INCHES = 4.0; // TODO: the actual value
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV;
public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH;
public static final double TICK_TIME_TO_SECONDS = 10;
public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS;
}
public static final class Configurations {
public static final double OPEN_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // TODO: find the actual value
public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value
// public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2022 SwerveDrive values
// public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2022 SwerveDrive values
// public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2022 SwerveDrive values
// public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2022 SwerveDrive values
// public static final double LEFT_FRONT_ENCODER_OFFSET = (4 * 360. - 152.1265 - 180 - 90) % 360.; // * 2023 translated values (don't work)
// public static final double RIGHT_FRONT_ENCODER_OFFSET = (4 * 360. - 9.3156 - 180 - 90) % 360.; // * 2023 translated values (don't work)
// public static final double LEFT_BACK_ENCODER_OFFSET = (4 * 360. - 232.6466 + 180 - 90) % 360.; // * 2023 translated values (don't work)
// public static final double RIGHT_BACK_ENCODER_OFFSET = (4 * 360. - 189.4834 - 90) % 360.; // * 2023 translated values (don't work)
// public static final double LEFT_FRONT_ENCODER_OFFSET = 0.0 + 90.0; // * 2023 experimentally-derived values (mostly work)
// public static final double RIGHT_FRONT_ENCODER_OFFSET = 0.0; // * 2023 experimentally-derived values (mostly work)
// public static final double LEFT_BACK_ENCODER_OFFSET = 0.0 + 24.0; // * 2023 experimentally-derived values (mostly work)
// public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0 + 45.0 + 180.0; // * 2023 experimentally-derived values (mostly work)
}
public static final double MAX_SPEED_FEET_PER_SECOND = 10; // TODO: find the actual value
// dimensions
public static final double WIDTH = 18.5; // TODO: find the actual value
public static final double HEIGHT = 18.5; // TODO: find the actual value
public static final double HALF_WIDTH = WIDTH / 2.d;
public static final double HALF_HEIGHT = HEIGHT / 2.d;
// misc
public static final int TIMEOUT_MS = 30;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class GyroConstants {
public static final int ID = 14; // TODO: find the actual ID
}
public static final class ArmConstants {
public static final int MIN_ARM_LEN = 0;
public static final int MAX_ARM_LEN = 1;
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
}
public static final class LEDConstants {
// public static final int LED_SPARK_ID = 0;
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
}
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
}
}
src/main/java/frc4388/robot/Robot.java
+1 -8
View File
@@ -25,6 +25,7 @@ public class Robot extends TimedRobot {
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
@@ -75,13 +76,6 @@ public class Robot extends TimedRobot {
public void autonomousInit() {
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
*/
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
@@ -113,7 +107,6 @@ public class Robot extends TimedRobot {
*/
@Override
public void teleopPeriodic() {
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+30 -14
View File
@@ -10,11 +10,11 @@ package frc4388.robot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.LED;
import frc4388.utility.LEDPatterns;
import frc4388.robot.commands.AutoBalance;
import frc4388.robot.commands.DriveWithInput;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
@@ -27,10 +27,12 @@ import frc4388.utility.controller.XboxController;
*/
public class RobotContainer {
/* RobotMap */
private final RobotMap m_robotMap = new RobotMap();
public final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
private final LED m_robotLED = new LED(m_robotMap.LEDController);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
@@ -43,11 +45,19 @@ public class RobotContainer {
configureButtonBindings();
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
m_robotSwerveDrive.setDefaultCommand(new DriveWithInput(m_robotSwerveDrive,
() -> getDriverController().getLeftXAxis(),
() -> getDriverController().getLeftYAxis(),
() -> getDriverController().getRightXAxis(),
false));
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
}
/**
* Use this method to define your button->command mappings. Buttons can be
* created by instantiating a {@link GenericHID} or one of its subclasses
@@ -56,13 +66,19 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Driver Buttons */
// test command to spin the robot while pressing A on the driver controller
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotMap.gyro.reset()));
// .onFalse()
/* Operator Buttons */
// activates "Lit Mode"
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
// interrupt button
new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
.onTrue(new InstantCommand());
}
/**
@@ -71,7 +87,7 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// no auto
return new InstantCommand();
}
src/main/java/frc4388/robot/RobotMap.java
+98 -16
View File
@@ -7,14 +7,12 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import frc4388.robot.Constants.LEDConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.SwerveModule;
import frc4388.utility.RobotGyro;
/**
@@ -22,19 +20,103 @@ import frc4388.utility.RobotGyro;
* testing and modularization.
*/
public class RobotMap {
private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14);
public RobotGyro gyro = new RobotGyro(m_pigeon2);
public RobotMap() {
configureLEDMotorControllers();
configureDriveMotorControllers();
}
/* LED Subsystem */
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
public SwerveModule leftFront;
public SwerveModule rightFront;
public SwerveModule leftBack;
public SwerveModule rightBack;
void configureLEDMotorControllers() {
}
public RobotMap() {
configureLEDMotorControllers();
configureDriveMotors();
}
void configureDriveMotorControllers() {
}
/* LED Subsystem */
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
void configureLEDMotorControllers() {
}
// swerve drive subsystem
public final WPI_TalonFX leftFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_WHEEL_ID);
public final WPI_TalonFX leftFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_FRONT_STEER_ID);
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_FRONT_ENCODER_ID);
public final WPI_TalonFX rightFrontWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_WHEEL_ID);
public final WPI_TalonFX rightFrontSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_FRONT_STEER_ID);
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_FRONT_ENCODER_ID);
public final WPI_TalonFX leftBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_WHEEL_ID);
public final WPI_TalonFX leftBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.LEFT_BACK_STEER_ID);
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.IDs.LEFT_BACK_ENCODER_ID);
public final WPI_TalonFX rightBackWheel = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_WHEEL_ID);
public final WPI_TalonFX rightBackSteer = new WPI_TalonFX(SwerveDriveConstants.IDs.RIGHT_BACK_STEER_ID);
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.IDs.RIGHT_BACK_ENCODER_ID);
void configureDriveMotors() {
// config factory default
leftFrontWheel.configFactoryDefault();
leftFrontSteer.configFactoryDefault();
rightFrontWheel.configFactoryDefault();
rightFrontSteer.configFactoryDefault();
leftBackWheel.configFactoryDefault();
leftBackSteer.configFactoryDefault();
rightBackWheel.configFactoryDefault();
rightBackSteer.configFactoryDefault();
// config open loop ramp
leftFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightFrontWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightFrontSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightBackWheel.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightBackSteer.configOpenloopRamp(SwerveDriveConstants.Configurations.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// config closed loop ramp
leftFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightFrontWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightFrontSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
leftBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightBackWheel.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
rightBackSteer.configClosedloopRamp(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE, SwerveDriveConstants.TIMEOUT_MS);
// config neutral deadband
leftFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
leftFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightFrontWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightFrontSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
leftBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
leftBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightBackWheel.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
// initialize SwerveModules
this.leftFront = new SwerveModule(leftFrontWheel, leftFrontSteer, leftFrontEncoder, -181.230469);
this.rightFront = new SwerveModule(rightFrontWheel, rightFrontSteer, rightFrontEncoder, -270.615234);
this.leftBack = new SwerveModule(leftBackWheel, leftBackSteer, leftBackEncoder, -240.029297);
this.rightBack = new SwerveModule(rightBackWheel, rightBackSteer, rightBackEncoder, -40.869142);
}
}
src/main/java/frc4388/robot/commands/AutoBalance.java
+51
View File
@@ -0,0 +1,51 @@
// 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 frc4388.robot.commands;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.RobotGyro;
public class AutoBalance extends PelvicInflammatoryDisease {
RobotGyro gyro;
SwerveDrive drive;
/** Creates a new AutoBalanceTF2. */
public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
super(1.0, 0, 0, 0);
this.gyro = gyro;
this.drive = drive;
addRequirements(drive);
}
@Override
public double getError() {
var pitch = gyro.getPitch();
SmartDashboard.putNumber("pitch", pitch);
return pitch;
}
@Override
public void runWithOutput(double output) {
double out2 = MathUtil.clamp(output / 40, -.5, .5);
if (Math.abs(gyro.getPitch()) < 3) out2 = 0;
drive.drive(out2, 0, 0, false);
}
@Override
public void initialize() {
super.initialize();
// this.gyro.reset();
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/DriveWithInput.java
+75
View File
@@ -0,0 +1,75 @@
// 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 frc4388.robot.commands;
import java.util.function.Supplier;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.SwerveDrive;
public class DriveWithInput extends CommandBase {
private final SwerveDrive swerve;
private final Supplier<Double> xSpeed;
private final Supplier<Double> ySpeed;
private final Supplier<Double> rot;
private final boolean fieldRelative;
private final SlewRateLimiter xLimiter, yLimiter, rotLimiter;
/** Creates a new DriveWithInput. */
public DriveWithInput(SwerveDrive swerve, Supplier<Double> xSpeed, Supplier<Double> ySpeed, Supplier<Double> rot, boolean fieldRelative) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
this.xSpeed = xSpeed;
this.ySpeed = ySpeed;
this.rot = rot;
this.fieldRelative = fieldRelative;
this.xLimiter = new SlewRateLimiter(3);
this.yLimiter = new SlewRateLimiter(3);
this.rotLimiter = new SlewRateLimiter(3);
addRequirements(swerve);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double x = xSpeed.get();
double y = ySpeed.get();
double r = rot.get();
x = -xLimiter.calculate(MathUtil.applyDeadband(x * -0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
y = -yLimiter.calculate(MathUtil.applyDeadband(y * 0.3, 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
r = -rotLimiter.calculate(MathUtil.applyDeadband(r * 0.3, 0.02) * Units.feetToMeters(5*Math.PI));
swerve.drive(x, y, r, fieldRelative);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
System.out.println("----------------------------------------------------------------");
System.out.println("DriveWithInput ended");
System.out.println("Interrupted: " + interrupted);
System.out.println("----------------------------------------------------------------");
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
+58
View File
@@ -0,0 +1,58 @@
// 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 frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.utility.Gains;
public abstract class PelvicInflammatoryDisease extends CommandBase {
protected Gains gains;
private double output = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) {
gains = new Gains(kp, ki, kd, kf, 0);
}
public PelvicInflammatoryDisease(Gains gains) {
this.gains = gains;
}
/** produces the error from the setpoint */
public abstract double getError();
/** figure it out bitch */
public abstract void runWithOutput(double output);
// Called when the command is initially scheduled.
@Override
public void initialize() {
output = 0;
}
private double prevError, cumError = 0;
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
double error = getError();
cumError += error * .02; // 20 ms
double delta = error - prevError;
output = error * gains.kP;
output += cumError * gains.kI;
output += delta * gains.kD;
output += gains.kF;
runWithOutput(output);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() { return false; }
}
src/main/java/frc4388/robot/subsystems/DiffDrive.java
-85
View File
@@ -1,85 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.DriveConstants;
import frc4388.utility.RobotGyro;
import frc4388.utility.RobotTime;
/**
* Add your docs here.
*/
public class DiffDrive extends SubsystemBase {
// Put methods for controlling this subsystem
// here. Call these from Commands.
private RobotTime m_robotTime = RobotTime.getInstance();
private WPI_TalonFX m_leftFrontMotor;
private WPI_TalonFX m_rightFrontMotor;
private WPI_TalonFX m_leftBackMotor;
private WPI_TalonFX m_rightBackMotor;
private DifferentialDrive m_driveTrain;
private RobotGyro m_gyro;
/**
* Add your docs here.
*/
public DiffDrive(WPI_TalonFX leftFrontMotor, WPI_TalonFX rightFrontMotor, WPI_TalonFX leftBackMotor,
WPI_TalonFX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
m_leftFrontMotor = leftFrontMotor;
m_rightFrontMotor = rightFrontMotor;
m_leftBackMotor = leftBackMotor;
m_rightBackMotor = rightBackMotor;
m_driveTrain = driveTrain;
m_gyro = gyro;
}
@Override
public void periodic() {
m_gyro.updatePigeonDeltas();
if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
updateSmartDashboard();
}
}
/**
* Add your docs here.
*/
public void driveWithInput(double move, double steer) {
m_driveTrain.arcadeDrive(move, steer);
}
/**
* Add your docs here.
*/
public void tankDriveWithInput(double leftMove, double rightMove) {
m_leftFrontMotor.set(leftMove);
m_rightFrontMotor.set(rightMove);
}
/**
* Add your docs here.
*/
private void updateSmartDashboard() {
// Examples of the functionality of RobotGyro
SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
// SmartDashboard.putData(m_gyro);
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+152
View File
@@ -0,0 +1,152 @@
// 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 frc4388.robot.subsystems;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
public class SwerveDrive extends SubsystemBase {
public SwerveModule leftFront;
public SwerveModule rightFront;
public SwerveModule leftBack;
public SwerveModule rightBack;
private SwerveModule[] modules;
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation);
// private SwerveDriveOdometry odometry = new SwerveDriveOdometry(
// kinematics,
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// }
// );
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
/** Creates a new SwerveDrive. */
public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack) {//, RobotGyro gyro) {
this.leftFront = leftFront;
this.rightFront = rightFront;
this.leftBack = leftBack;
this.rightBack = rightBack;
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
// WPILib swerve drive example
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(
// fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d())
// : new ChassisSpeeds(xSpeed, ySpeed, rot)
// );
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
/**
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.
*/
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.metersToFeet(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state);
}
}
/**
* Updates the odometry of the SwerveDrive.
*/
// public void updateOdometry() {
// odometry.update(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// }
// );
// }
/**
* Gets the odometry of the SwerveDrive.
* @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
*/
// public Pose2d getOdometry() {
// return odometry.getPoseMeters();
// }
/**
* Sets the odometry of the SwerveDrive.
* @param pose Pose to set the odometry to.
*/
// public void setOdometry(Pose2d pose) {
// odometry.resetPosition(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// },
// pose
// );
// }
/**
* Resets the odometry of the SwerveDrive to 0.
* *NOTE: If you reset your gyroscope or wheel encoders, this method MUST be called with the new gyro angle and wheel encoder positions.
*/
// public void resetOdometry() {
// odometry.resetPosition(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// },
// new Pose2d()
// );
// }
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
}
@Override
public void periodic() {
// This method will be called once per scheduler run
// updateOdometry();
}
/**
* Shifts gear from high to low, or vice versa.
* @param shift true to shift to high, false to shift to low
*/
public void highSpeed(boolean shift) {
this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
}
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+146
View File
@@ -0,0 +1,146 @@
// 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 frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
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.math.util.Units;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
public class SwerveModule extends SubsystemBase {
public WPI_TalonFX driveMotor;
public WPI_TalonFX angleMotor;
private CANCoder encoder;
public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
/** Creates a new SwerveModule. */
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder encoder, double offset) {
this.driveMotor = driveMotor;
this.angleMotor = angleMotor;
this.encoder = encoder;
TalonFXConfiguration angleConfig = new TalonFXConfiguration();
angleConfig.slot0.kP = swerveGains.kP;
angleConfig.slot0.kI = swerveGains.kI;
angleConfig.slot0.kD = swerveGains.kD;
// use the CANcoder as the remote sensor for the primary TalonFX PID
angleConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
angleConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
angleMotor.configAllSettings(angleConfig);
encoder.configMagnetOffset(offset);
}
/**
* Get the drive motor of the SwerveModule
* @return the drive motor of the SwerveModule
*/
public WPI_TalonFX getDriveMotor() {
return this.driveMotor;
}
/**
* Get the angle motor of the SwerveModule
* @return the angle motor of the SwerveModule
*/
public WPI_TalonFX getAngleMotor() {
return this.angleMotor;
}
/**
* Get the CANcoder of the SwerveModule
* @return the CANcoder of the SwerveModule
*/
public CANCoder getEncoder() {
return this.encoder;
}
/**
* Get the angle of a SwerveModule as a Rotation2d
* @return the angle of a SwerveModule as a Rotation2d
*/
public Rotation2d getAngle() {
// * Note: This assumes that the CANCoders are setup with the default feedback coefficient and the sensor value reports degrees.
return Rotation2d.fromDegrees(encoder.getAbsolutePosition());
}
public void stop() {
driveMotor.set(0);
angleMotor.set(0);
}
public void rotateToAngle(double angle) {
angleMotor.set(TalonFXControlMode.Position, angle);
}
/**
* Get state of swerve module
* @return speed in m/s and angle in degrees
*/
public SwerveModuleState getState() {
return new SwerveModuleState(
Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS,
getAngle()
);
}
/**
* Returns the current position of the SwerveModule
* @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor.
*/
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle());
}
/**
* Set the speed and rotation of the SwerveModule from a SwerveModuleState object
* @param desiredState a SwerveModuleState representing the desired new state of the module
*/
public void setDesiredState(SwerveModuleState desiredState) {
Rotation2d currentRotation = this.getAngle();
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
// calculate the difference between our current rotational position and our new rotational position
Rotation2d rotationDelta = state.angle.minus(currentRotation);
// calculate the new absolute position of the SwerveModule based on the difference in rotation
double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION;
// convert the CANCoder from its position reading to ticks
double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient();
angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks);
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
driveMotor.set(-1 * angleMotor.get() + feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND);
// driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME);
}
public void reset(double position) {
encoder.setPositionToAbsolute();
}
public double getCurrent() {
return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
}
public double getVoltage() {
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
}
}
src/main/java/frc4388/utility/Gains.java
+77
View File
@@ -0,0 +1,77 @@
// 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 frc4388.utility;
/** Add your docs here. */
public class Gains {
public double kP;
public double kI;
public double kD;
public double kF;
public int kIZone;
public double kPeakOutput;
public double kMaxOutput;
public double kMinOutput;
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
* @param kPeakOutput The peak output setting the motors to run the gains at, in both forward and reverse directions. By default 1.0.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kPeakOutput) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kPeakOutput = kPeakOutput;
this.kMaxOutput = kPeakOutput;
this.kMinOutput = -kPeakOutput;
}
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kPeakOutput = 1.0;
this.kMaxOutput = 1.0;
this.kMinOutput = -1.0;
}
/**
* Creates Gains object for PIDs
* @param kP The P value.
* @param kI The I value.
* @param kD The D value.
* @param kF The F value.
* @param kIZone The zone of the I value.
* @param kMinOutput The lowest output setting to run the gains at, usually in the reverse direction. By default -1.0.
* @param kMaxOutput The highest output setting to run the gains at, usually in the forward direction. By default 1.0.
*/
public Gains(double kP, double kI, double kD, double kF, int kIZone, double kMaxOutput, double kMinOutput) {
this.kP = kP;
this.kI = kI;
this.kD = kD;
this.kF = kF;
this.kIZone = kIZone;
this.kMaxOutput = kMaxOutput;
this.kMinOutput = kMinOutput;
this.kPeakOutput = (Math.abs(kMinOutput) > Math.abs(kMaxOutput)) ? Math.abs(kMinOutput) : Math.abs(kMaxOutput);
}
}
src/main/java/frc4388/utility/RobotGyro.java
+28 -9
View File
@@ -7,8 +7,7 @@
package frc4388.utility;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import com.kauailabs.navx.frc.AHRS;
// import edu.wpi.first.wpilibj.GyroBase;
@@ -21,18 +20,21 @@ import edu.wpi.first.math.MathUtil;
public class RobotGyro implements Gyro {
private RobotTime m_robotTime = RobotTime.getInstance();
private PigeonIMU m_pigeon = null;
private WPI_Pigeon2 m_pigeon = null;
private AHRS m_navX = null;
public boolean m_isGyroAPigeon; //true if pigeon, false if navX
private double m_lastPigeonAngle;
private double m_deltaPigeonAngle;
private double pitchZero = 0;
private double rollZero = 0;
/**
* Creates a Gyro based on a pigeon
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(PigeonIMU gyro) {
public RobotGyro(WPI_Pigeon2 gyro) {
m_pigeon = gyro;
m_isGyroAPigeon = true;
}
@@ -46,6 +48,16 @@ public class RobotGyro implements Gyro {
m_isGyroAPigeon = false;
}
/**
* Resets yaw, pitch, and roll.
*/
public void resetZeroValues() {
if (!m_isGyroAPigeon) return;
pitchZero = m_pigeon.getPitch();
rollZero = m_pigeon.getRoll();
}
/**
* Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note
* that the getRate() method for a navX will likely be much more accurate than for a pigeon.
@@ -75,7 +87,7 @@ public class RobotGyro implements Gyro {
@Override
public void calibrate() {
if (m_isGyroAPigeon) {
m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
m_pigeon.calibrate();
} else {
m_navX.calibrate();
}
@@ -83,6 +95,8 @@ public class RobotGyro implements Gyro {
@Override
public void reset() {
resetZeroValues();
if (m_isGyroAPigeon) {
m_pigeon.setYaw(0);
} else {
@@ -99,9 +113,10 @@ public class RobotGyro implements Gyro {
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] angles = new double[3];
m_pigeon.getYawPitchRoll(angles);
return angles;
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
}
@Override
@@ -113,6 +128,10 @@ public class RobotGyro implements Gyro {
}
}
public double getYaw() {
return this.getAngle();
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
@@ -166,7 +185,7 @@ public class RobotGyro implements Gyro {
}
}
public PigeonIMU getPigeon(){
public WPI_Pigeon2 getPigeon(){
return m_pigeon;
}