mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Upgrade to 2022
* Update license * Remove arcade drive * Correct indentation * Disable RobotGyro (new gyro is untested)
This commit is contained in:
nathanrsxtn
@@ -1,9 +1,6 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
// 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;
|
||||
|
||||
@@ -19,62 +16,51 @@ import frc4388.utility.LEDPatterns;
|
||||
* constants are needed, to reduce verbosity.
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final class ArcadeDriveConstants {
|
||||
public static final int DRIVE_LEFT_FRONT_CAN_ID = 2;
|
||||
public static final int DRIVE_RIGHT_FRONT_CAN_ID = 4;
|
||||
public static final int DRIVE_LEFT_BACK_CAN_ID = 3;
|
||||
public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
|
||||
|
||||
public static final int DRIVE_PIGEON_ID = 6;
|
||||
public static final class SwerveDriveConstants {
|
||||
public static final double ROTATION_SPEED = 0.1;
|
||||
public static final double WHEEL_SPEED = 0.1;
|
||||
public static final double WIDTH = 22;
|
||||
public static final double HEIGHT = 22;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
|
||||
public static final double MAX_SPEED_FEET_PER_SEC = 16;
|
||||
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
|
||||
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
|
||||
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
|
||||
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
|
||||
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
|
||||
public static final int LEFT_BACK_STEER_CAN_ID = 6;
|
||||
public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
|
||||
public static final int RIGHT_BACK_STEER_CAN_ID = 8;
|
||||
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
|
||||
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
|
||||
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
|
||||
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
|
||||
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
|
||||
//ofsets are in degrees
|
||||
public static final float LEFT_FRONT_ENCODER_OFFSET = 0;
|
||||
public static final float RIGHT_FRONT_ENCODER_OFFSET = 0;
|
||||
public static final float LEFT_BACK_ENCODER_OFFSET = 0;
|
||||
public static final float RIGHT_BACK_ENCODER_OFFSET = 0;
|
||||
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
}
|
||||
// swerve PID constants
|
||||
public static final int SWERVE_SLOT_IDX = 0;
|
||||
public static final int SWERVE_PID_LOOP_IDX = 1;
|
||||
public static final int SWERVE_TIMEOUT_MS = 30;
|
||||
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
|
||||
public static final class SwerveDriveConstants {
|
||||
public static final double ROTATION_SPEED = 0.1;
|
||||
public static final double WHEEL_SPEED = 0.1;
|
||||
public static final double WIDTH = 22;
|
||||
public static final double HEIGHT = 22;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
|
||||
public static final double MAX_SPEED_FEET_PER_SEC = 16;
|
||||
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
|
||||
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
|
||||
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
|
||||
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
|
||||
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
|
||||
public static final int LEFT_BACK_STEER_CAN_ID = 6;
|
||||
public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
|
||||
public static final int RIGHT_BACK_STEER_CAN_ID = 8;
|
||||
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
|
||||
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
|
||||
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
|
||||
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
|
||||
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
|
||||
//ofsets are in degrees
|
||||
public static final float LEFT_FRONT_ENCODER_OFFSET = 0;
|
||||
public static final float RIGHT_FRONT_ENCODER_OFFSET = 0;
|
||||
public static final float LEFT_BACK_ENCODER_OFFSET = 0;
|
||||
public static final float RIGHT_BACK_ENCODER_OFFSET = 0;
|
||||
// swerve configuration
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
|
||||
public static final int REMOTE_0 = 0;
|
||||
}
|
||||
public static final class LEDConstants {
|
||||
public static final int LED_SPARK_ID = 0;
|
||||
|
||||
// swerve PID constants
|
||||
public static final int SWERVE_SLOT_IDX = 0;
|
||||
public static final int SWERVE_PID_LOOP_IDX = 1;
|
||||
public static final int SWERVE_TIMEOUT_MS = 30;
|
||||
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES;
|
||||
}
|
||||
|
||||
// swerve configuration
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
|
||||
public static final int REMOTE_0 = 0;
|
||||
}
|
||||
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;
|
||||
}
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -75,12 +75,16 @@ 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; }
|
||||
*/
|
||||
/*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) {
|
||||
|
||||
@@ -1,9 +1,6 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
// 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;
|
||||
|
||||
@@ -13,7 +10,6 @@ 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.ArcadeDrive;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
@@ -28,105 +24,94 @@ import frc4388.utility.controller.XboxController;
|
||||
* commands, and button mappings) should be declared here.
|
||||
*/
|
||||
public class RobotContainer {
|
||||
/* RobotMap */
|
||||
private final RobotMap m_robotMap = new RobotMap();
|
||||
/* RobotMap */
|
||||
private final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
//private final ArcadeDrive m_robotArcadeDrive = new ArcadeDrive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor,
|
||||
// m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.driveTrain, m_robotMap.gyroDrive);
|
||||
/* Subsystems */
|
||||
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
|
||||
m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
|
||||
m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
|
||||
m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor,
|
||||
m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor,
|
||||
m_robotMap.leftFrontEncoder,
|
||||
m_robotMap.rightFrontEncoder,
|
||||
m_robotMap.leftBackEncoder,
|
||||
m_robotMap.rightBackEncoder
|
||||
);
|
||||
|
||||
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
|
||||
m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor,
|
||||
m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor,
|
||||
m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor,
|
||||
m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor,
|
||||
m_robotMap.leftFrontEncoder,
|
||||
m_robotMap.rightFrontEncoder,
|
||||
m_robotMap.leftBackEncoder,
|
||||
m_robotMap.rightBackEncoder
|
||||
);
|
||||
private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
|
||||
private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
/* Controllers */
|
||||
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
|
||||
/* Controllers */
|
||||
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
/* Default Commands */
|
||||
// drives the swerve drive with a two-axis input from the driver controller
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getLeftXAxis(),
|
||||
getDriverController().getLeftYAxis(), getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
|
||||
|
||||
/* Default Commands */
|
||||
// drives the arcade drive with a two-axis input from the driver controller
|
||||
/*m_robotArcadeDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotArcadeDrive.driveWithInput(getDriverController().getLeftYAxis(),
|
||||
getDriverController().getRightXAxis()), m_robotArcadeDrive));*/
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
}
|
||||
|
||||
// drives the swerve drive with a two-axis input from the driver controller
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(getDriverController().getLeftXAxis(),
|
||||
getDriverController().getLeftYAxis(), getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
|
||||
}
|
||||
/* 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));
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
// test command to spin the robot while pressing A on the driver controller
|
||||
//new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
//.whileHeld(() -> m_robotArcadeDrive.driveWithInput(0, 1));
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// no auto
|
||||
return new InstantCommand();
|
||||
}
|
||||
|
||||
/* 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));
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public IHandController getDriverController() {
|
||||
return m_driverXbox;
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// no auto
|
||||
return new InstantCommand();
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public IHandController getOperatorController() {
|
||||
return m_operatorXbox;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public IHandController getDriverController() {
|
||||
return m_driverXbox;
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getOperatorJoystick() {
|
||||
return m_operatorXbox.getJoyStick();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public IHandController getOperatorController() {
|
||||
return m_operatorXbox;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getOperatorJoystick() {
|
||||
return m_operatorXbox.getJoyStick();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getDriverJoystick() {
|
||||
return m_driverXbox.getJoyStick();
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getDriverJoystick() {
|
||||
return m_driverXbox.getJoyStick();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,26 +1,15 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018-2019 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. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
// 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;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.InvertType;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU;
|
||||
import com.ctre.phoenix.sensors.AbsoluteSensorRange;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
|
||||
import edu.wpi.first.wpilibj.Spark;
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
import frc4388.robot.Constants.ArcadeDriveConstants;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
@@ -28,112 +17,79 @@ import frc4388.utility.RobotGyro;
|
||||
*/
|
||||
public class RobotMap {
|
||||
|
||||
public RobotMap() {
|
||||
configureLEDMotorControllers();
|
||||
//configureArcadeDriveMotorControllers();
|
||||
configureSwerveMotorControllers();
|
||||
}
|
||||
public RobotMap() {
|
||||
configureLEDMotorControllers();
|
||||
configureSwerveMotorControllers();
|
||||
}
|
||||
|
||||
/* LED Subsystem */
|
||||
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
/* LED Subsystem */
|
||||
public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||
|
||||
void configureLEDMotorControllers() {
|
||||
|
||||
}
|
||||
void configureLEDMotorControllers() {
|
||||
|
||||
/* ArcadeDrive Subsystem */
|
||||
//public final WPI_TalonFX leftFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
|
||||
//public final WPI_TalonFX rightFrontMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
|
||||
//public final WPI_TalonFX leftBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_LEFT_BACK_CAN_ID);
|
||||
//public final WPI_TalonFX rightBackMotor = new WPI_TalonFX(ArcadeDriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
|
||||
//public final DifferentialDrive driveTrain = new DifferentialDrive(leftFrontMotor, rightFrontMotor);
|
||||
//public final RobotGyro gyroDrive = new RobotGyro(new PigeonIMU(ArcadeDriveConstants.DRIVE_PIGEON_ID));
|
||||
}
|
||||
|
||||
/* Swerve Subsystem */
|
||||
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
|
||||
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
|
||||
|
||||
void configureSwerveMotorControllers() {
|
||||
leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
/*void configureArcadeDriveMotorControllers() {
|
||||
leftFrontSteerMotor.configFactoryDefault();
|
||||
leftFrontWheelMotor.configFactoryDefault();
|
||||
rightFrontSteerMotor.configFactoryDefault();
|
||||
rightFrontWheelMotor.configFactoryDefault();
|
||||
leftBackSteerMotor.configFactoryDefault();
|
||||
leftBackWheelMotor.configFactoryDefault();
|
||||
rightBackSteerMotor.configFactoryDefault();
|
||||
rightBackWheelMotor.configFactoryDefault();
|
||||
|
||||
// factory default values
|
||||
leftFrontMotor.configFactoryDefault();
|
||||
rightFrontMotor.configFactoryDefault();
|
||||
leftBackMotor.configFactoryDefault();
|
||||
rightBackMotor.configFactoryDefault();
|
||||
leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
// set back motors as followers
|
||||
leftBackMotor.follow(leftFrontMotor);
|
||||
rightBackMotor.follow(rightFrontMotor);
|
||||
leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
// set neutral mode
|
||||
leftFrontMotor.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontMotor.setNeutralMode(NeutralMode.Brake);
|
||||
leftFrontMotor.setNeutralMode(NeutralMode.Brake);
|
||||
rightFrontMotor.setNeutralMode(NeutralMode.Brake);
|
||||
leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
// flip input so forward becomes back, etc
|
||||
leftFrontMotor.setInverted(false);
|
||||
rightFrontMotor.setInverted(false);
|
||||
leftBackMotor.setInverted(InvertType.FollowMaster);
|
||||
rightBackMotor.setInverted(InvertType.FollowMaster);
|
||||
}*/
|
||||
/* Swerve Subsystem */
|
||||
public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID);
|
||||
public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID);
|
||||
public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID);
|
||||
public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID);
|
||||
public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
|
||||
|
||||
void configureSwerveMotorControllers() {
|
||||
leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
leftFrontSteerMotor.configFactoryDefault();
|
||||
leftFrontWheelMotor.configFactoryDefault();
|
||||
rightFrontSteerMotor.configFactoryDefault();
|
||||
rightFrontWheelMotor.configFactoryDefault();
|
||||
leftBackSteerMotor.configFactoryDefault();
|
||||
leftBackWheelMotor.configFactoryDefault();
|
||||
rightBackSteerMotor.configFactoryDefault();
|
||||
rightBackWheelMotor.configFactoryDefault();
|
||||
|
||||
leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
// config cancoder as remote encoder for swerve steer motors
|
||||
//leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
//leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
//rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
//rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
}
|
||||
// config cancoder as remote encoder for swerve steer motors
|
||||
//leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
//leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
//rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
//rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -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.ArcadeDriveConstants;
|
||||
import frc4388.utility.RobotGyro;
|
||||
import frc4388.utility.RobotTime;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class ArcadeDrive 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 ArcadeDrive(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 % ArcadeDriveConstants.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);
|
||||
}
|
||||
}
|
||||
@@ -7,7 +7,7 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.Spark;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
|
||||
@@ -1,194 +1,177 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
|
||||
/* 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. */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
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.wpilibj.interfaces.Gyro;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase
|
||||
{
|
||||
SwerveDriveKinematics m_kinematics;
|
||||
private WPI_TalonFX m_leftFrontSteerMotor;
|
||||
private WPI_TalonFX m_leftFrontWheelMotor;
|
||||
private WPI_TalonFX m_rightFrontSteerMotor;
|
||||
private WPI_TalonFX m_rightFrontWheelMotor;
|
||||
private WPI_TalonFX m_leftBackSteerMotor;
|
||||
private WPI_TalonFX m_leftBackWheelMotor;
|
||||
private WPI_TalonFX m_rightBackSteerMotor;
|
||||
private WPI_TalonFX m_rightBackWheelMotor;
|
||||
private CANCoder m_leftFrontEncoder;
|
||||
private CANCoder m_rightFrontEncoder;
|
||||
private CANCoder m_leftBackEncoder;
|
||||
private CANCoder m_rightBackEncoder;
|
||||
double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
|
||||
double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
public class SwerveDrive extends SubsystemBase {
|
||||
SwerveDriveKinematics m_kinematics;
|
||||
private WPI_TalonFX m_leftFrontSteerMotor;
|
||||
private WPI_TalonFX m_leftFrontWheelMotor;
|
||||
private WPI_TalonFX m_rightFrontSteerMotor;
|
||||
private WPI_TalonFX m_rightFrontWheelMotor;
|
||||
private WPI_TalonFX m_leftBackSteerMotor;
|
||||
private WPI_TalonFX m_leftBackWheelMotor;
|
||||
private WPI_TalonFX m_rightBackSteerMotor;
|
||||
private WPI_TalonFX m_rightBackWheelMotor;
|
||||
private CANCoder m_leftFrontEncoder;
|
||||
private CANCoder m_rightFrontEncoder;
|
||||
private CANCoder m_leftBackEncoder;
|
||||
private CANCoder m_rightBackEncoder;
|
||||
double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
|
||||
double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
|
||||
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
|
||||
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
|
||||
// setSwerveGains();
|
||||
|
||||
Translation2d m_frontLeftLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(halfHeight),
|
||||
Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_frontRightLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(halfHeight),
|
||||
Units.inchesToMeters(-halfWidth));
|
||||
Translation2d m_backLeftLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(-halfHeight),
|
||||
Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_backRightLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(-halfHeight),
|
||||
Units.inchesToMeters(-halfWidth));
|
||||
//setSwerveGains();
|
||||
|
||||
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
|
||||
public SwerveModule[] modules;
|
||||
public RobotGyro gyro; //TODO Add Gyro Lol
|
||||
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
|
||||
public SwerveModule[] modules;
|
||||
public Gyro gyro; // TODO Add Gyro Lol
|
||||
|
||||
public SwerveDrive(WPI_TalonFX leftFrontSteerMotor, WPI_TalonFX leftFrontWheelMotor,
|
||||
WPI_TalonFX rightFrontSteerMotor, WPI_TalonFX rightFrontWheelMotor,
|
||||
WPI_TalonFX leftBackSteerMotor, WPI_TalonFX leftBackWheelMotor,
|
||||
WPI_TalonFX rightBackSteerMotor, WPI_TalonFX rightBackWheelMotor,
|
||||
CANCoder leftFrontEncoder, CANCoder rightFrontEncoder,
|
||||
CANCoder leftBackEncoder, CANCoder rightBackEncoder) {
|
||||
m_leftFrontSteerMotor = leftFrontSteerMotor;
|
||||
m_leftFrontWheelMotor = leftFrontWheelMotor;
|
||||
m_rightFrontSteerMotor = rightFrontSteerMotor;
|
||||
m_rightFrontWheelMotor = rightFrontWheelMotor;
|
||||
m_leftBackSteerMotor = leftBackSteerMotor;
|
||||
m_leftBackWheelMotor = leftBackWheelMotor;
|
||||
m_rightBackSteerMotor = rightBackSteerMotor;
|
||||
m_rightBackWheelMotor = rightBackWheelMotor;
|
||||
m_leftFrontEncoder = leftFrontEncoder;
|
||||
m_rightFrontEncoder = rightFrontEncoder;
|
||||
m_leftBackEncoder = leftBackEncoder;
|
||||
m_rightBackEncoder = rightBackEncoder;
|
||||
|
||||
public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
|
||||
WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder,
|
||||
CANCoder rightFrontEncoder,
|
||||
CANCoder leftBackEncoder,
|
||||
CANCoder rightBackEncoder)
|
||||
{
|
||||
m_leftFrontSteerMotor = leftFrontSteerMotor;
|
||||
m_leftFrontWheelMotor = leftFrontWheelMotor;
|
||||
m_rightFrontSteerMotor = rightFrontSteerMotor;
|
||||
m_rightFrontWheelMotor = rightFrontWheelMotor;
|
||||
m_leftBackSteerMotor = leftBackSteerMotor;
|
||||
m_leftBackWheelMotor = leftBackWheelMotor;
|
||||
m_rightBackSteerMotor = rightBackSteerMotor;
|
||||
m_rightBackWheelMotor = rightBackWheelMotor;
|
||||
m_leftFrontEncoder = leftFrontEncoder;
|
||||
m_rightFrontEncoder = rightFrontEncoder;
|
||||
m_leftBackEncoder = leftBackEncoder;
|
||||
m_rightBackEncoder = rightBackEncoder;
|
||||
modules = new SwerveModule[] {
|
||||
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left
|
||||
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right
|
||||
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left
|
||||
new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder) // Back Right
|
||||
};
|
||||
// gyro.reset();
|
||||
}
|
||||
|
||||
modules = new SwerveModule[] {
|
||||
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder), // Front Left
|
||||
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder), // Front Right
|
||||
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder), // Back Left
|
||||
new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder) // Back Right
|
||||
};
|
||||
//gyro.reset();
|
||||
}
|
||||
//https://github.com/ZachOrr/MK3-Swerve-Example
|
||||
/**
|
||||
// https://github.com/ZachOrr/MK3-Swerve-Example
|
||||
/**
|
||||
* Method to drive the robot using joystick info.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction (forward).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
|
||||
* @param xSpeed Speed of the robot in the x direction (forward).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to the
|
||||
* field.
|
||||
*/
|
||||
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative)
|
||||
{
|
||||
/*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
|
||||
driveFromSpeeds(speeds);*/
|
||||
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
|
||||
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
|
||||
SwerveModuleState[] states =
|
||||
kinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot));
|
||||
SwerveDriveKinematics.normalizeWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
|
||||
for (int i = 0; i < states.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = states[i];
|
||||
module.setDesiredState(state);
|
||||
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
|
||||
// var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
|
||||
// driveFromSpeeds(speeds);
|
||||
|
||||
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
|
||||
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
|
||||
SwerveModuleState[] states = kinematics.toSwerveModuleStates(
|
||||
fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot, gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot));
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
|
||||
for (int i = 0; i < states.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = states[i];
|
||||
module.setDesiredState(state);
|
||||
}
|
||||
}
|
||||
//Converts a ChassisSpeed to SwerveModuleStates (targets)
|
||||
public void driveFromSpeeds(ChassisSpeeds speeds)
|
||||
{
|
||||
//https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
|
||||
// Convert to module states
|
||||
SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
|
||||
}
|
||||
|
||||
// Front left module state
|
||||
SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition()));
|
||||
// Front right module state
|
||||
SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition()));
|
||||
// Back left module state
|
||||
SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition()));
|
||||
// Back right module state
|
||||
SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
|
||||
|
||||
//Set the motors
|
||||
setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
|
||||
}
|
||||
// Converts a ChassisSpeed to SwerveModuleStates (targets)
|
||||
public void driveFromSpeeds(ChassisSpeeds speeds) {
|
||||
// https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
|
||||
// Convert to module states
|
||||
SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
|
||||
|
||||
//Sets steering motors to PID values
|
||||
public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack)
|
||||
{
|
||||
/*//Set the Wheel motor speeds
|
||||
m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
// Front left module state
|
||||
SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition()));
|
||||
// Front right module state
|
||||
SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition()));
|
||||
// Back left module state
|
||||
SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition()));
|
||||
// Back right module state
|
||||
SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
|
||||
|
||||
//PID
|
||||
m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000);
|
||||
m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000);
|
||||
m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000);
|
||||
m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
|
||||
// Set the motors
|
||||
setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
|
||||
}
|
||||
//Sets steering motors to PID values
|
||||
public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack)
|
||||
{/*
|
||||
//Set the Wheel motor speeds
|
||||
m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
|
||||
System.out.println("Target: " + leftFront.angle.getDegrees());*/
|
||||
}
|
||||
//PID
|
||||
m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000);
|
||||
m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000);
|
||||
m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000);
|
||||
m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
|
||||
|
||||
System.out.println("Target: " + leftFront.angle.getDegrees());
|
||||
*/}
|
||||
|
||||
/*public void setSwerveGains(){
|
||||
m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
/*public void setSwerveGains(){
|
||||
m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
}*/
|
||||
}*/
|
||||
|
||||
|
||||
|
||||
// public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
|
||||
// {
|
||||
// var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary,
|
||||
// rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
|
||||
// }
|
||||
// public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
|
||||
// {
|
||||
// var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary, rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
|
||||
// }
|
||||
}
|
||||
@@ -12,22 +12,20 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.CANCoderConfiguration;
|
||||
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.wpilibj.simulation.EncoderSim;
|
||||
import edu.wpi.first.wpilibj.util.Units;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
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 {
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
private CANCoder canCoder;
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
|
||||
private static double kEncoderTicksPerRotation = 4096;
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
private CANCoder canCoder;
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
|
||||
private static double kEncoderTicksPerRotation = 4096;
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder) {
|
||||
@@ -47,30 +45,32 @@ public class SwerveModule extends SubsystemBase {
|
||||
angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
|
||||
angleMotor.configAllSettings(angleTalonFXConfiguration);
|
||||
|
||||
/*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
|
||||
|
||||
driveTalonFXConfiguration.slot0.kP = kDriveP;
|
||||
driveTalonFXConfiguration.slot0.kI = kDriveI;
|
||||
driveTalonFXConfiguration.slot0.kD = kDriveD;
|
||||
driveTalonFXConfiguration.slot0.kF = kDriveF;
|
||||
|
||||
driveMotor.configAllSettings(driveTalonFXConfiguration);*/
|
||||
/*
|
||||
* TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
|
||||
*
|
||||
* driveTalonFXConfiguration.slot0.kP = kDriveP;
|
||||
* driveTalonFXConfiguration.slot0.kI = kDriveI;
|
||||
* driveTalonFXConfiguration.slot0.kD = kDriveD;
|
||||
* driveTalonFXConfiguration.slot0.kF = kDriveF;
|
||||
*
|
||||
* driveMotor.configAllSettings(driveTalonFXConfiguration);
|
||||
*/
|
||||
|
||||
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
|
||||
//CANCODER CONFIG
|
||||
// CANCODER CONFIG
|
||||
canCoder.configAllSettings(canCoderConfiguration);
|
||||
}
|
||||
|
||||
|
||||
public Rotation2d getAngle() {
|
||||
// Note: This assumes the CANCoders are setup with the default feedback coefficient
|
||||
// and the sesnor value reports degrees.
|
||||
// Note: This assumes the CANCoders are setup with the default feedback coefficient and the sesnor value reports degrees.
|
||||
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the speed + rotation of the swerve module from a SwerveModuleState object
|
||||
* @param desiredState - A SwerveModuleState representing the desired new state of the module
|
||||
*
|
||||
* @param desiredState - A SwerveModuleState representing the desired new state
|
||||
* of the module
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
Rotation2d currentRotation = getAngle();
|
||||
@@ -86,7 +86,6 @@ public class SwerveModule extends SubsystemBase {
|
||||
double desiredTicks = currentTicks + deltaTicks;
|
||||
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
|
||||
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user