Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Upgrade to 2022

Browse Source 
* Update license
* Remove arcade drive
* Correct indentation
* Disable RobotGyro (new gyro is untested)
This commit is contained in:
nathanrsxtn
2022-01-11 11:05:52 -07:00
parent 731310fbc8
commit 71563e6759
38 changed files with 1507 additions and 1541 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+46 -60
View File
@@ -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;
}
}
src/main/java/frc4388/robot/Robot.java
+10 -6
View File
@@ -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) {
src/main/java/frc4388/robot/RobotContainer.java
+80 -95
View File
@@ -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();
}
}
src/main/java/frc4388/robot/RobotMap.java
+70 -114
View File
@@ -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);
}
}
src/main/java/frc4388/robot/subsystems/ArcadeDrive.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.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);
}
}
src/main/java/frc4388/robot/subsystems/LED.java
+1 -1
View File
@@ -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;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+145 -162
View File
@@ -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*/)
// }
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+23 -24
View File
@@ -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);
}
src/main/java/frc4388/utility/Gains.java
+49 -49
View File
@@ -6,55 +6,55 @@ package frc4388.utility;
/** Add your docs here. */
public class Gains {
public double m_kP;
public double m_kI;
public double m_kD;
public double m_kF;
public int m_kIzone;
public double m_kPeakOutput;
public double m_kmaxOutput;
public double m_kminOutput;
public double m_kP;
public double m_kI;
public double m_kD;
public double m_kF;
public int m_kIzone;
public double m_kPeakOutput;
public double m_kmaxOutput;
public double m_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)
{
m_kP = kP;
m_kI = kI;
m_kD = kD;
m_kF = kF;
m_kIzone = kIzone;
m_kPeakOutput = kPeakOutput;
m_kmaxOutput = m_kPeakOutput;
m_kminOutput = -m_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.
* @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)
{
m_kP = kP;
m_kI = kI;
m_kD = kD;
m_kF = kF;
m_kIzone = kIzone;
m_kPeakOutput = kPeakOutput;
m_kmaxOutput = m_kPeakOutput;
m_kminOutput = -m_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.
* @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 kMinOutput, double kMaxOutput)
{
m_kP = kP;
m_kI = kI;
m_kD = kD;
m_kF = kF;
m_kIzone = kIzone;
m_kminOutput = kMinOutput;
m_kmaxOutput = kMaxOutput;
m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput);
}
/**
* 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 kMinOutput, double kMaxOutput)
{
m_kP = kP;
m_kI = kI;
m_kD = kD;
m_kF = kF;
m_kIzone = kIzone;
m_kminOutput = kMinOutput;
m_kmaxOutput = kMaxOutput;
m_kPeakOutput = (Math.abs(m_kminOutput) > Math.abs(m_kmaxOutput)) ? Math.abs(m_kminOutput) : Math.abs(m_kmaxOutput);
}
}
src/main/java/frc4388/utility/RobotGyro.java
-180
View File
@@ -1,180 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
package frc4388.utility;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.GyroBase;
import edu.wpi.first.wpiutil.math.MathUtil;
/**
* Gyro class that allows for interchangeable use between a pigeon and a navX
*/
public class RobotGyro extends GyroBase {
private RobotTime m_robotTime = RobotTime.getInstance();
private PigeonIMU 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;
/**
* Creates a Gyro based on a pigeon
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(PigeonIMU gyro) {
m_pigeon = gyro;
m_isGyroAPigeon = true;
}
/**
* Creates a Gyro based on a navX
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(AHRS gyro){
m_navX = gyro;
m_isGyroAPigeon = false;
}
/**
* 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.
*/
public void updatePigeonDeltas() {
double currentPigeonAngle = getAngle();
m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
m_lastPigeonAngle = currentPigeonAngle;
}
/**
* <p>NavX:
* <p>Calibrate the gyro by running for a number of samples and computing the center value. Then use
* the center value as the Accumulator center value for subsequent measurements. It's important to
* make sure that the robot is not moving while the centering calculations are in progress, this
* is typically done when the robot is first turned on while it's sitting at rest before the
* competition starts.
*
* <p>Pigeon:
* <p>Calibrate the gyro by collecting data at a range of tempuratures. Allow pigeon to cool, then boot
* into calibration mode. For faster calibration, use a heat lamp to heat up the pigeon. Once the pigeon
* has seen a reasonable range of tempuratures, it will exit calibration mode. It's important to
* make sure that the robot is not moving while the tempurature calculations are in progress, this
* is typically done when the robot is first turned on while it's sitting at rest before the
* competition starts.
*/
@Override
public void calibrate() {
if (m_isGyroAPigeon) {
m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
} else {
m_navX.calibrate();
}
}
@Override
public void reset() {
if (m_isGyroAPigeon) {
m_pigeon.setYaw(0);
} else {
m_navX.reset();
}
}
/**
* Get Yaw, Pitch, and Roll data.
*
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] angles = new double[3];
m_pigeon.getYawPitchRoll(angles);
return angles;
}
@Override
public double getAngle() {
if (m_isGyroAPigeon) {
return getPigeonAngles()[0];
} else {
return m_navX.getAngle();
}
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
*/
public double getHeading() {
return getHeading(getAngle());
}
/**
* Gets an absolute heading of the robot
* @return heading from -180 to 180 degrees
*/
public double getHeading(double angle) {
return Math.IEEEremainder(angle, 360);
}
/**
* Returns the current pitch value (in degrees, from -90 to 90)
* reported by the sensor. Pitch is a measure of rotation around
* the Y Axis.
* @return The current pitch value in degrees (-90 to 90).
*/
public double getPitch() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
} else {
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
}
}
/**
* Returns the current roll value (in degrees, from -90 to 90)
* reported by the sensor. Roll is a measure of rotation around
* the X Axis.
* @return The current roll value in degrees (-90 to 90).
*/
public double getRoll() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
} else {
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
}
}
@Override
public double getRate() {
if (m_isGyroAPigeon) {
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
} else {
return m_navX.getRate();
}
}
public PigeonIMU getPigeon(){
return m_pigeon;
}
public AHRS getNavX(){
return m_navX;
}
@Override
public void close() throws Exception {
}
}
src/main/java/frc4388/utility/RobotGyro.java.old
+245
View File
@@ -0,0 +1,245 @@
// 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;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.interfaces.Gyro;
/**
* Gyro class that allows for interchangeable use between a pigeon and a navX
*/
public class RobotGyro implements Gyro, PIDSource, Sendable {
private RobotTime m_robotTime = RobotTime.getInstance();
private PigeonIMU 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;
/**
* Creates a Gyro based on a pigeon
*
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(PigeonIMU gyro) {
m_pigeon = gyro;
m_isGyroAPigeon = true;
}
/**
* Creates a Gyro based on a navX
*
* @param gyro the gyroscope to use for Gyro
*/
public RobotGyro(AHRS gyro) {
m_navX = gyro;
m_isGyroAPigeon = false;
}
/**
* 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.
*/
public void updatePigeonDeltas() {
double currentPigeonAngle = getAngle();
m_deltaPigeonAngle = currentPigeonAngle - m_lastPigeonAngle;
m_lastPigeonAngle = currentPigeonAngle;
}
/**
* <p>
* NavX:
* <p>
* Calibrate the gyro by running for a number of samples and computing the
* center value. Then use
* the center value as the Accumulator center value for subsequent measurements.
* It's important to
* make sure that the robot is not moving while the centering calculations are
* in progress, this
* is typically done when the robot is first turned on while it's sitting at
* rest before the
* competition starts.
*
* <p>
* Pigeon:
* <p>
* Calibrate the gyro by collecting data at a range of tempuratures. Allow
* pigeon to cool, then boot
* into calibration mode. For faster calibration, use a heat lamp to heat up the
* pigeon. Once the pigeon
* has seen a reasonable range of tempuratures, it will exit calibration mode.
* It's important to
* make sure that the robot is not moving while the tempurature calculations are
* in progress, this
* is typically done when the robot is first turned on while it's sitting at
* rest before the
* competition starts.
*/
@Override
public void calibrate() {
if (m_isGyroAPigeon)
m_pigeon.enterCalibrationMode(CalibrationMode.Temperature);
else
m_navX.calibrate();
}
@Override
public void reset() {
if (m_isGyroAPigeon)
m_pigeon.setYaw(0);
else
m_navX.reset();
}
/**
* Get Yaw, Pitch, and Roll data.
*
* @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data.
* Yaw is within [-368,640, +368,640] degrees.
* Pitch is within [-90,+90] degrees.
* Roll is within [-90,+90] degrees.
*/
private double[] getPigeonAngles() {
double[] angles = new double[3];
m_pigeon.getYawPitchRoll(angles);
return angles;
}
@Override
public double getAngle() {
if (m_isGyroAPigeon) {
return getPigeonAngles()[0];
} else {
return m_navX.getAngle();
}
}
/**
* Gets an absolute heading of the robot
*
* @return heading from -180 to 180 degrees
*/
public double getHeading() {
return getHeading(getAngle());
}
/**
* Gets an absolute heading of the robot
*
* @return heading from -180 to 180 degrees
*/
public double getHeading(double angle) {
return Math.IEEEremainder(angle, 360);
}
/**
* Returns the current pitch value (in degrees, from -90 to 90)
* reported by the sensor. Pitch is a measure of rotation around
* the Y Axis.
*
* @return The current pitch value in degrees (-90 to 90).
*/
public double getPitch() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[1], -90, 90);
} else {
return MathUtil.clamp(m_navX.getPitch(), -90, 90);
}
}
/**
* Returns the current roll value (in degrees, from -90 to 90)
* reported by the sensor. Roll is a measure of rotation around
* the X Axis.
*
* @return The current roll value in degrees (-90 to 90).
*/
public double getRoll() {
if (m_isGyroAPigeon) {
return MathUtil.clamp(getPigeonAngles()[2], -90, 90);
} else {
return MathUtil.clamp(m_navX.getRoll(), -90, 90);
}
}
@Override
public double getRate() {
if (m_isGyroAPigeon) {
return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
} else {
return m_navX.getRate();
}
}
public PigeonIMU getPigeon() {
return m_pigeon;
}
public AHRS getNavX() {
return m_navX;
}
@Override
public void close() throws Exception {
}
// Begin old GyroBase class
private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
/**
* Set which parameter of the gyro you are using as a process control variable.
* The Gyro class
* supports the rate and displacement parameters
*
* @param pidSource An enum to select the parameter.
*/
@Override
public void setPIDSourceType(PIDSourceType pidSource) {
m_pidSource = pidSource;
}
@Override
public PIDSourceType getPIDSourceType() {
return m_pidSource;
}
/**
* Get the output of the gyro for use with PIDControllers. May be the angle or
* rate depending on
* the set PIDSourceType
*
* @return the output according to the gyro
*/
@Override
public double pidGet() {
switch (m_pidSource) {
case kRate:
return getRate();
case kDisplacement:
return getAngle();
default:
return 0.0;
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}
src/main/java/frc4388/utility/RobotTime.java
+53 -56
View File
@@ -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.utility;
@@ -12,68 +9,68 @@ package frc4388.utility;
* <p>All times are in milliseconds
*/
public class RobotTime {
private long m_currTime = System.currentTimeMillis();
public long m_deltaTime = 0;
private long m_currTime = System.currentTimeMillis();
public long m_deltaTime = 0;
private long m_startRobotTime = m_currTime;
public long m_robotTime = 0;
public long m_lastRobotTime = 0;
private long m_startRobotTime = m_currTime;
public long m_robotTime = 0;
public long m_lastRobotTime = 0;
private long m_startMatchTime = 0;
public long m_matchTime = 0;
public long m_lastMatchTime = 0;
private long m_startMatchTime = 0;
public long m_matchTime = 0;
public long m_lastMatchTime = 0;
public long m_frameNumber = 0;
public long m_frameNumber = 0;
/**
* Private constructor prevents other classes from instantiating
*/
private RobotTime(){}
/**
* Private constructor prevents other classes from instantiating
*/
private RobotTime(){}
private static RobotTime instance = null;
private static RobotTime instance = null;
/**
* Gets the instance of Robot Time. If there is no instance running one will be created.
* @return instance of Robot Time
*/
public static RobotTime getInstance() {
if (instance == null) {
instance = new RobotTime();
}
return instance;
/**
* Gets the instance of Robot Time. If there is no instance running one will be created.
* @return instance of Robot Time
*/
public static RobotTime getInstance() {
if (instance == null) {
instance = new RobotTime();
}
return instance;
}
/**
* Call this once per periodic loop.
*/
public void updateTimes() {
m_lastRobotTime = m_robotTime;
m_lastMatchTime = m_matchTime;
/**
* Call this once per periodic loop.
*/
public void updateTimes() {
m_lastRobotTime = m_robotTime;
m_lastMatchTime = m_matchTime;
m_currTime = System.currentTimeMillis();
m_robotTime = m_currTime - m_startRobotTime;
m_deltaTime = m_robotTime - m_lastRobotTime;
m_frameNumber++;
m_currTime = System.currentTimeMillis();
m_robotTime = m_currTime - m_startRobotTime;
m_deltaTime = m_robotTime - m_lastRobotTime;
m_frameNumber++;
if (m_startMatchTime != 0) {
m_matchTime = m_currTime - m_startMatchTime;
}
if (m_startMatchTime != 0) {
m_matchTime = m_currTime - m_startMatchTime;
}
}
/**
* Call this in both the auto and periodic inits
*/
public void startMatchTime() {
if (m_startMatchTime == 0) {
m_startMatchTime = m_currTime;
}
/**
* Call this in both the auto and periodic inits
*/
public void startMatchTime() {
if (m_startMatchTime == 0) {
m_startMatchTime = m_currTime;
}
}
/**
* Call this in disabled init
*/
public void endMatchTime() {
m_startMatchTime = 0;
m_matchTime = 0;
}
/**
* Call this in disabled init
*/
public void endMatchTime() {
m_startMatchTime = 0;
m_matchTime = 0;
}
}
src/main/java/frc4388/utility/controller/IHandController.java
+8 -8
View File
@@ -5,17 +5,17 @@ package frc4388.utility.controller;
*/
public interface IHandController {
public double getLeftXAxis();
public double getLeftXAxis();
public double getLeftYAxis();
public double getLeftYAxis();
public double getRightXAxis();
public double getRightYAxis();
public double getRightXAxis();
public double getLeftTriggerAxis();
public double getRightYAxis();
public double getRightTriggerAxis();
public double getLeftTriggerAxis();
public int getDpadAngle();
public double getRightTriggerAxis();
public int getDpadAngle();
}
src/main/java/frc4388/utility/controller/XboxController.java
+156 -156
View File
@@ -9,210 +9,210 @@ import edu.wpi.first.wpilibj.Joystick;
*/
public class XboxController implements IHandController
{
public static final int LEFT_X_AXIS = 0;
public static final int LEFT_Y_AXIS = 1;
public static final int LEFT_TRIGGER_AXIS = 2;
public static final int RIGHT_TRIGGER_AXIS = 3;
public static final int RIGHT_X_AXIS = 4;
public static final int RIGHT_Y_AXIS = 5;
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
public static final int LEFT_X_AXIS = 0;
public static final int LEFT_Y_AXIS = 1;
public static final int LEFT_TRIGGER_AXIS = 2;
public static final int RIGHT_TRIGGER_AXIS = 3;
public static final int RIGHT_X_AXIS = 4;
public static final int RIGHT_Y_AXIS = 5;
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
public static final int A_BUTTON = 1;
public static final int B_BUTTON = 2;
public static final int X_BUTTON = 3;
public static final int Y_BUTTON = 4;
public static final int LEFT_BUMPER_BUTTON = 5;
public static final int RIGHT_BUMPER_BUTTON = 6;
public static final int BACK_BUTTON = 7;
public static final int START_BUTTON = 8;
public static final int A_BUTTON = 1;
public static final int B_BUTTON = 2;
public static final int X_BUTTON = 3;
public static final int Y_BUTTON = 4;
public static final int LEFT_BUMPER_BUTTON = 5;
public static final int RIGHT_BUMPER_BUTTON = 6;
public static final int BACK_BUTTON = 7;
public static final int START_BUTTON = 8;
public static final int LEFT_JOYSTICK_BUTTON = 9;
public static final int RIGHT_JOYSTICK_BUTTON = 10;
public static final int LEFT_JOYSTICK_BUTTON = 9;
public static final int RIGHT_JOYSTICK_BUTTON = 10;
private static final double LEFT_DPAD_TOLERANCE = -0.9;
private static final double RIGHT_DPAD_TOLERANCE = 0.9;
private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
private static final double TOP_DPAD_TOLERANCE = 0.9;
private static final double LEFT_DPAD_TOLERANCE = -0.9;
private static final double RIGHT_DPAD_TOLERANCE = 0.9;
private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
private static final double TOP_DPAD_TOLERANCE = 0.9;
private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
private static final double DEADZONE = 0.1;
private static final double DEADZONE = 0.1;
private Joystick m_stick;
private Joystick m_stick;
/**
/**
* Add your docs here.
*/
public XboxController(int portNumber){
m_stick = new Joystick(portNumber);
}
public XboxController(int portNumber){
m_stick = new Joystick(portNumber);
}
/**
/**
* Add your docs here.
*/
public Joystick getJoyStick() {
return m_stick;
}
/**
* Checks if the input falls within the deadzone.
* @param input from an axis on the controller
* @return true if input falls in deadzone, false if input falls outside deadzone
*/
private boolean inDeadZone(double input){
return (Math.abs(input) < DEADZONE);
}
public Joystick getJoyStick() {
return m_stick;
}
/**
* Checks if the input falls within the deadzone.
* @param input from an axis on the controller
* @return true if input falls in deadzone, false if input falls outside deadzone
*/
private boolean inDeadZone(double input){
return (Math.abs(input) < DEADZONE);
}
/**
* Updates an input to have a deadzone around the 0 position
* @param input from an axis on the controller
* @return updated input
*/
private double getAxisWithDeadZoneCheck(double input){
if(inDeadZone(input)){
return 0.0;
} else {
return input;
}
}
/**
* Updates an input to have a deadzone around the 0 position
* @param input from an axis on the controller
* @return updated input
*/
private double getAxisWithDeadZoneCheck(double input){
if(inDeadZone(input)){
return 0.0;
} else {
return input;
}
}
public boolean getAButton(){
return m_stick.getRawButton(A_BUTTON);
}
public boolean getAButton(){
return m_stick.getRawButton(A_BUTTON);
}
public boolean getXButton(){
return m_stick.getRawButton(X_BUTTON);
}
public boolean getXButton(){
return m_stick.getRawButton(X_BUTTON);
}
public boolean getBButton(){
return m_stick.getRawButton(B_BUTTON);
}
public boolean getBButton(){
return m_stick.getRawButton(B_BUTTON);
}
public boolean getYButton(){
return m_stick.getRawButton(Y_BUTTON);
}
public boolean getYButton(){
return m_stick.getRawButton(Y_BUTTON);
}
public boolean getBackButton(){
return m_stick.getRawButton(BACK_BUTTON);
}
public boolean getBackButton(){
return m_stick.getRawButton(BACK_BUTTON);
}
public boolean getStartButton(){
return m_stick.getRawButton(START_BUTTON);
}
public boolean getStartButton(){
return m_stick.getRawButton(START_BUTTON);
}
public boolean getLeftBumperButton(){
return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
}
public boolean getLeftBumperButton(){
return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
}
public boolean getRightBumperButton(){
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
}
public boolean getRightBumperButton(){
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
}
public boolean getLeftJoystickButton(){
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
}
public boolean getLeftJoystickButton(){
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
}
public boolean getRightJoystickButton(){
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
}
public boolean getRightJoystickButton(){
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
}
public double getLeftXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
}
public double getLeftXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
}
public double getLeftYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
}
public double getLeftYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
}
public double getRightXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
}
public double getRightXAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
}
public double getRightYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
}
public double getRightYAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
}
public double getLeftTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
}
public double getLeftTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
}
public double getRightTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
}
public double getRightTriggerAxis(){
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
}
/**
* Get the angle input from the dpad.
* @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
*/
public int getDpadAngle() {
return m_stick.getPOV(0);
}
/**
* Get the angle input from the dpad.
* @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
*/
public int getDpadAngle() {
return m_stick.getPOV(0);
}
public boolean getDPadLeft(){
public boolean getDPadLeft(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
}
}
public boolean getDPadRight(){
public boolean getDPadRight(){
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
}
}
public boolean getDPadTop(){
public boolean getDPadTop(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
}
}
public boolean getDPadBottom(){
public boolean getDPadBottom(){
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
}
}
public boolean getLeftTrigger(){
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
}
public boolean getLeftTrigger(){
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
}
public boolean getRightTrigger(){
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
}
public boolean getRightTrigger(){
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
}
public boolean getRightAxisUpTrigger(){
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
}
public boolean getRightAxisUpTrigger(){
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
}
public boolean getRightAxisDownTrigger(){
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
}
public boolean getRightAxisDownTrigger(){
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
}
public boolean getRightAxisLeftTrigger(){
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
}
public boolean getRightAxisLeftTrigger(){
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
}
public boolean getRightAxisRightTrigger(){
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
}
public boolean getRightAxisRightTrigger(){
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
}
public boolean getLeftAxisUpTrigger(){
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
}
public boolean getLeftAxisUpTrigger(){
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
}
public boolean getLeftAxisDownTrigger(){
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
}
public boolean getLeftAxisDownTrigger(){
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
}
public boolean getLeftAxisLeftTrigger(){
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
}
public boolean getLeftAxisLeftTrigger(){
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
}
public boolean getLeftAxisRightTrigger(){
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
}
public boolean getLeftAxisRightTrigger(){
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
}
}
src/main/java/frc4388/utility/controller/XboxTriggerButton.java
+36 -55
View File
@@ -1,7 +1,6 @@
package frc4388.utility.controller;
import edu.wpi.first.wpilibj2.command.button.Button;
import frc4388.utility.controller.XboxController;
/**
* Mapping for the Xbox controller triggers to allow triggers to be defined as
@@ -9,61 +8,43 @@ import frc4388.utility.controller.XboxController;
* exceeds a tolerance defined in {@link XboxController}.
*/
public class XboxTriggerButton extends Button {
public static final int RIGHT_TRIGGER = 0;
public static final int LEFT_TRIGGER = 1;
public static final int RIGHT_AXIS_UP_TRIGGER = 2;
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
public static final int LEFT_AXIS_UP_TRIGGER = 6;
public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
public static final int RIGHT_TRIGGER = 0;
public static final int LEFT_TRIGGER = 1;
public static final int RIGHT_AXIS_UP_TRIGGER = 2;
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
public static final int LEFT_AXIS_UP_TRIGGER = 6;
public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
private XboxController m_controller;
private int m_trigger;
private XboxController m_controller;
private int m_trigger;
/**
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger
*/
public XboxTriggerButton(XboxController controller, int trigger) {
m_controller = controller;
m_trigger = trigger;
}
/**
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger
*/
public XboxTriggerButton(XboxController controller, int trigger) {
m_controller = controller;
m_trigger = trigger;
}
/** {@inheritDoc} */
@Override
public boolean get() {
if (m_trigger == RIGHT_TRIGGER) {
return m_controller.getRightTrigger();
}
else if (m_trigger == LEFT_TRIGGER) {
return m_controller.getLeftTrigger();
}
else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) {
return m_controller.getRightAxisUpTrigger();
}
else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) {
return m_controller.getRightAxisDownTrigger();
}
else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) {
return m_controller.getRightAxisRightTrigger();
}
else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) {
return m_controller.getRightAxisLeftTrigger();
}
else if (m_trigger == LEFT_AXIS_UP_TRIGGER) {
return m_controller.getLeftAxisUpTrigger();
}
else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) {
return m_controller.getLeftAxisDownTrigger();
}
else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) {
return m_controller.getLeftAxisRightTrigger();
}
else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) {
return m_controller.getLeftAxisLeftTrigger();
}
return false;
}
/** {@inheritDoc} */
@Override
public boolean get() {
switch (m_trigger) {
case RIGHT_TRIGGER: return m_controller.getRightTrigger();
case LEFT_TRIGGER: return m_controller.getLeftTrigger();
case RIGHT_AXIS_UP_TRIGGER: return m_controller.getRightAxisUpTrigger();
case RIGHT_AXIS_DOWN_TRIGGER: return m_controller.getRightAxisDownTrigger();
case RIGHT_AXIS_RIGHT_TRIGGER: return m_controller.getRightAxisRightTrigger();
case RIGHT_AXIS_LEFT_TRIGGER: return m_controller.getRightAxisLeftTrigger();
case LEFT_AXIS_UP_TRIGGER: return m_controller.getLeftAxisUpTrigger();
case LEFT_AXIS_DOWN_TRIGGER: return m_controller.getLeftAxisDownTrigger();
case LEFT_AXIS_RIGHT_TRIGGER: return m_controller.getLeftAxisRightTrigger();
case LEFT_AXIS_LEFT_TRIGGER: return m_controller.getLeftAxisLeftTrigger();
default: return false;
}
}
}
src/test/java/frc4388/mocks/MockPigeonIMU.java
+35 -38
View File
@@ -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.mocks;
@@ -14,41 +11,41 @@ import com.ctre.phoenix.sensors.PigeonIMU;
* Add your docs here.
*/
public class MockPigeonIMU extends PigeonIMU {
public int m_deviceNumber;
public double currentYaw;
public double currentPitch;
public double currentRoll;
public int m_deviceNumber;
public double currentYaw;
public double currentPitch;
public double currentRoll;
public MockPigeonIMU(int deviceNumber) {
super(deviceNumber);
m_deviceNumber = deviceNumber;
}
public MockPigeonIMU(int deviceNumber) {
super(deviceNumber);
m_deviceNumber = deviceNumber;
}
@Override
public ErrorCode setYaw(double angleDeg) {
currentYaw = angleDeg;
return ErrorCode.OK;
}
@Override
public ErrorCode setYaw(double angleDeg) {
currentYaw = angleDeg;
return ErrorCode.OK;
}
/**
* @param currentPitch the Pitch to set
*/
public void setCurrentPitch(double currentPitch) {
this.currentPitch = currentPitch;
}
/**
* @param currentPitch the Pitch to set
*/
public void setCurrentPitch(double currentPitch) {
this.currentPitch = currentPitch;
}
/**
* @param currentRoll the Roll to set
*/
public void setCurrentRoll(double currentRoll) {
this.currentRoll = currentRoll;
}
/**
* @param currentRoll the Roll to set
*/
public void setCurrentRoll(double currentRoll) {
this.currentRoll = currentRoll;
}
@Override
public ErrorCode getYawPitchRoll(double[] ypr_deg) {
ypr_deg[0] = currentYaw;
ypr_deg[1] = currentPitch;
ypr_deg[2] = currentRoll;
return ErrorCode.OK;
}
@Override
public ErrorCode getYawPitchRoll(double[] ypr_deg) {
ypr_deg[0] = currentYaw;
ypr_deg[1] = currentPitch;
ypr_deg[2] = currentRoll;
return ErrorCode.OK;
}
}
src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java
+31 -34
View File
@@ -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.subsystems;
@@ -12,7 +9,7 @@ import static org.mockito.Mockito.mock;
import org.junit.Test;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.motorcontrol.Spark;
import frc4388.robot.Constants.LEDConstants;
import frc4388.utility.LEDPatterns;
@@ -20,40 +17,40 @@ import frc4388.utility.LEDPatterns;
* Add your docs here.
*/
public class LEDSubsystemTest {
@Test
public void testConstructor() {
// Arrange
Spark ledController = mock(Spark.class);
@Test
public void testConstructor() {
// Arrange
Spark ledController = mock(Spark.class);
// Act
LED led = new LED(ledController);
// Act
LED led = new LED(ledController);
// Assert
assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
}
// Assert
assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
}
@Test
public void testPatterns() {
// Arrange
Spark ledController = mock(Spark.class);
LED led = new LED(ledController);
@Test
public void testPatterns() {
// Arrange
Spark ledController = mock(Spark.class);
LED led = new LED(ledController);
// Act
led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
// Act
led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
// Assert
assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
// Assert
assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
// Act
led.setPattern(LEDPatterns.BLUE_BREATH);
// Act
led.setPattern(LEDPatterns.BLUE_BREATH);
// Assert
assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
// Assert
assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
// Act
led.setPattern(LEDPatterns.SOLID_BLACK);
// Act
led.setPattern(LEDPatterns.SOLID_BLACK);
// Assert
assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
}
// Assert
assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
}
}
src/test/java/frc4388/utility/RobotGyroUtilityTest.java → src/test/java/frc4388/utility/RobotGyroUtilityTest.java.old
+3 -6
View File
@@ -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.utility;
src/test/java/frc4388/utility/RobotTimeUtilityTest.java
+75 -78
View File
@@ -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.utility;
@@ -15,90 +12,90 @@ import org.junit.*;
* Add your docs here.
*/
public class RobotTimeUtilityTest {
RobotTime robotTime = RobotTime.getInstance();
RobotTime robotTime = RobotTime.getInstance();
@Test
public void testUpdateTimes() {
// Arrange
long lastTime;
robotTime.m_deltaTime = 0;
robotTime.m_robotTime = 0;
robotTime.m_lastRobotTime = 0;
robotTime.m_frameNumber = 0;
robotTime.endMatchTime();
robotTime.m_lastMatchTime = 0;
@Test
public void testUpdateTimes() {
// Arrange
long lastTime;
robotTime.m_deltaTime = 0;
robotTime.m_robotTime = 0;
robotTime.m_lastRobotTime = 0;
robotTime.m_frameNumber = 0;
robotTime.endMatchTime();
robotTime.m_lastMatchTime = 0;
// Assert
assertEquals(0, robotTime.m_deltaTime);
assertEquals(0, robotTime.m_robotTime);
assertEquals(0, robotTime.m_lastRobotTime);
assertEquals(0, robotTime.m_frameNumber);
lastTime = robotTime.m_robotTime;
// Assert
assertEquals(0, robotTime.m_deltaTime);
assertEquals(0, robotTime.m_robotTime);
assertEquals(0, robotTime.m_lastRobotTime);
assertEquals(0, robotTime.m_frameNumber);
lastTime = robotTime.m_robotTime;
// Act
wait(1);
robotTime.updateTimes();
// Act
wait(1);
robotTime.updateTimes();
// Assert
assertEquals(true, robotTime.m_deltaTime > 0);
assertEquals(true, robotTime.m_robotTime > 0);
assertEquals(lastTime, robotTime.m_lastRobotTime);
assertEquals(1, robotTime.m_frameNumber);
lastTime = robotTime.m_robotTime;
// Assert
assertEquals(true, robotTime.m_deltaTime > 0);
assertEquals(true, robotTime.m_robotTime > 0);
assertEquals(lastTime, robotTime.m_lastRobotTime);
assertEquals(1, robotTime.m_frameNumber);
lastTime = robotTime.m_robotTime;
// Act
wait(1);
robotTime.updateTimes();
// Act
wait(1);
robotTime.updateTimes();
// Assert
assertEquals(true, robotTime.m_deltaTime > 0);
assertEquals(true, robotTime.m_robotTime > 0);
assertEquals(lastTime, robotTime.m_lastRobotTime);
assertEquals(2, robotTime.m_frameNumber);
}
// Assert
assertEquals(true, robotTime.m_deltaTime > 0);
assertEquals(true, robotTime.m_robotTime > 0);
assertEquals(lastTime, robotTime.m_lastRobotTime);
assertEquals(2, robotTime.m_frameNumber);
}
@Test
public void testMatchTime() {
// Arrange
long lastTime;
@Test
public void testMatchTime() {
// Arrange
long lastTime;
// Assert
assertEquals(0, robotTime.m_matchTime);
assertEquals(0, robotTime.m_lastMatchTime);
lastTime = robotTime.m_matchTime;
// Assert
assertEquals(0, robotTime.m_matchTime);
assertEquals(0, robotTime.m_lastMatchTime);
lastTime = robotTime.m_matchTime;
// Act
robotTime.startMatchTime();
wait(1);
robotTime.updateTimes();
// Act
robotTime.startMatchTime();
wait(1);
robotTime.updateTimes();
// Assert
assertEquals(true, robotTime.m_matchTime > 0);
assertEquals(lastTime, robotTime.m_lastMatchTime);
lastTime = robotTime.m_matchTime;
// Act
wait(1);
robotTime.updateTimes();
robotTime.endMatchTime();
// Assert
assertEquals(true, robotTime.m_matchTime > 0);
assertEquals(lastTime, robotTime.m_lastMatchTime);
lastTime = robotTime.m_matchTime;
// Act
wait(1);
robotTime.updateTimes();
robotTime.endMatchTime();
// Assert
assertEquals(0, robotTime.m_matchTime);
assertEquals(lastTime, robotTime.m_lastMatchTime);
lastTime = robotTime.m_matchTime;
// Assert
assertEquals(0, robotTime.m_matchTime);
assertEquals(lastTime, robotTime.m_lastMatchTime);
lastTime = robotTime.m_matchTime;
// Act
wait(1);
robotTime.updateTimes();
// Act
wait(1);
robotTime.updateTimes();
// Assert
assertEquals(0, robotTime.m_matchTime);
assertEquals(lastTime, robotTime.m_lastMatchTime);
}
// Assert
assertEquals(0, robotTime.m_matchTime);
assertEquals(lastTime, robotTime.m_lastMatchTime);
}
private void wait(int millis) {
try {
Thread.sleep(millis);
} catch (Exception e) {}
}
private void wait(int millis) {
try {
Thread.sleep(millis);
} catch (Exception e) {}
}
}