From 789f4f6a9bc4ef8f69fe77b73eae4928f9deb4b8 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 14 Jan 2022 17:48:53 -0700 Subject: [PATCH] PID Tuning, Controller Adjust --- src/main/java/frc4388/robot/Constants.java | 8 +++--- src/main/java/frc4388/robot/Main.java | 3 +++ src/main/java/frc4388/robot/Robot.java | 1 + .../java/frc4388/robot/RobotContainer.java | 15 ++++++++--- src/main/java/frc4388/robot/RobotMap.java | 3 +++ .../frc4388/robot/subsystems/SwerveDrive.java | 27 ++++++++++++------- .../robot/subsystems/SwerveModule.java | 1 + .../utility/controller/XboxController.java | 15 +++++++++++ .../robot/subsystems/XboxControllerTest.java | 5 ++++ 9 files changed, 61 insertions(+), 17 deletions(-) create mode 100644 src/test/java/frc4388/robot/subsystems/XboxControllerTest.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 67cb117..ba49c07 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -26,9 +26,10 @@ public final class Constants { 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 JOYSTICK_TO_METERS_PER_SECOND = 2; + public static final double MAX_SPEED_FEET_PER_SEC = 20; public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; + //IDs 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; @@ -41,6 +42,7 @@ public final class Constants { 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; + public static final int GYRO_ID = 14; // ofsets are in degrees // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141; // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281; @@ -55,7 +57,7 @@ public final class 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 Gains SWERVE_GAINS = new Gains(1.0, 0.0, 1.0, 0.0, 0, 1.0); // swerve configuration public static final double NEUTRAL_DEADBAND = 0.04; diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index 39a5e0d..544abf0 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -24,3 +24,6 @@ public final class Main { RobotBase.startRobot(Robot::new); } } + + +// hi ryan \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 119a032..826bf43 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -114,6 +114,7 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { + System.err.println("TEST"); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 38c3459..38ba292 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,7 +36,8 @@ public class RobotContainer { m_robotMap.leftFrontEncoder, m_robotMap.rightFrontEncoder, m_robotMap.leftBackEncoder, - m_robotMap.rightBackEncoder + m_robotMap.rightBackEncoder, + m_robotMap.gyro ); private final LED m_robotLED = new LED(m_robotMap.LEDController); @@ -54,8 +55,13 @@ public class RobotContainer { /* 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)); + new RunCommand(() -> m_robotSwerveDrive.driveWithInput( + XboxController.ClampJoystickAxis( + getDriverController().getLeftXAxis(), + getDriverController().getLeftYAxis()), + -getDriverController().getRightXAxis(), + true), + m_robotSwerveDrive)); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); @@ -69,7 +75,8 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - + /*new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) + .whenPressed(() -> m_robotSwerveDrive.resetGyro());*/ /* Operator Buttons */ // activates "Lit Mode" new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 843323c..c55fae8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -6,7 +6,9 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; +import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; @@ -42,6 +44,7 @@ public class RobotMap { 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); + public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID); void configureSwerveMotorControllers() { leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 4b437f1..b70c839 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -6,6 +6,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.WPI_PigeonIMU; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -46,14 +47,14 @@ public class SwerveDrive extends SubsystemBase { private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation); public SwerveModule[] modules; - public Gyro gyro; //TODO Add Gyro Lol + public WPI_PigeonIMU m_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) + CANCoder rightBackEncoder, WPI_PigeonIMU gyro) { m_leftFrontSteerMotor = leftFrontSteerMotor; m_leftFrontWheelMotor = leftFrontWheelMotor; @@ -67,6 +68,7 @@ public class SwerveDrive extends SubsystemBase { m_rightFrontEncoder = rightFrontEncoder; m_leftBackEncoder = leftBackEncoder; m_rightBackEncoder = rightBackEncoder; + m_gyro = gyro; modules = new SwerveModule[] { new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left @@ -74,35 +76,40 @@ public class SwerveDrive extends SubsystemBase { new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right }; - //gyro.reset(); + m_gyro.reset(); } //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 speeds[0] Speed of the robot in the x direction (forward). + * @param speeds[1] 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) + public void driveWithInput(double[] speeds, double rot, boolean fieldRelative) { + System.out.println("Inputx: " + speeds[0] + " Inputy: " + speeds[1]); /*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; + double xSpeedMetersPerSecond = -speeds[0] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; + double ySpeedMetersPerSecond = speeds[1] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; SwerveModuleState[] states = kinematics.toSwerveModuleStates( fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, gyro.getRotation2d()) + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, m_gyro.getRotation2d()) : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3)); 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); + } } - } + + /*public void resetGyro(){ + m_gyro.reset(); + }*/ //Converts a ChassisSpeed to SwerveModuleStates (targets) public void driveFromSpeeds(ChassisSpeeds speeds) { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 4176e52..0c38acf 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -73,6 +73,7 @@ public class SwerveModule extends SubsystemBase { */ public void setDesiredState(SwerveModuleState desiredState) { Rotation2d currentRotation = getAngle(); + SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java index 25c496a..f14da2a 100644 --- a/src/main/java/frc4388/utility/controller/XboxController.java +++ b/src/main/java/frc4388/utility/controller/XboxController.java @@ -1,6 +1,7 @@ package frc4388.utility.controller; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * This is a wrapper for the WPILib Joystick class that represents an XBox @@ -52,6 +53,20 @@ public class XboxController implements IHandController private Joystick m_stick; + public static double[] ClampJoystickAxis(double x, double y) { + double square_mag = x * x + y * y; + double[] ret = {x,y}; + if (square_mag > 1.d) { + double mag = Math.sqrt(square_mag); + ret[0] = x / mag; + ret[1] = y / mag; + } + double [] to_smart_dashboard = {square_mag, x, y, ret[0], ret[1]}; + SmartDashboard.putNumberArray("Input", to_smart_dashboard); + System.err.println("TEST"); + return ret; + } + /** * Add your docs here. */ diff --git a/src/test/java/frc4388/robot/subsystems/XboxControllerTest.java b/src/test/java/frc4388/robot/subsystems/XboxControllerTest.java new file mode 100644 index 0000000..8298fbf --- /dev/null +++ b/src/test/java/frc4388/robot/subsystems/XboxControllerTest.java @@ -0,0 +1,5 @@ +package frc4388.robot.subsystems; + +public class XboxControllerTest { + +}