From e5a9d0c3647149e5975a3c64338a37e4bed67c5d Mon Sep 17 00:00:00 2001 From: Aarav Shah <91212@psdschools.org> Date: Fri, 21 Feb 2020 17:48:05 -0700 Subject: [PATCH 01/23] Added Wait Command, Works --- .../java/frc4388/robot/RobotContainer.java | 6 +- .../java/frc4388/robot/commands/Wait.java | 61 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 2 +- 3 files changed, 66 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/Wait.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index a254ac3..9f25a30 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,6 +13,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.command.WaitCommand; import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; @@ -49,6 +50,7 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.commands.TrackTarget; +import frc4388.robot.commands.Wait; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.Storage; @@ -89,7 +91,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); + m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController())); // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // drives climber with input from triggers on the opperator controller @@ -159,7 +161,7 @@ public class RobotContainer { // interrupts any running command new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) - .whenPressed(new InstantCommand(() -> System.out.print("Gamer"), m_robotDrive)); + .whenPressed(new Wait(3, m_robotDrive)); // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java new file mode 100644 index 0000000..934b508 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Wait.java @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Wait extends CommandBase { + + long m_startTime; + long m_waitTime; + long m_currentTime; + SubsystemBase m_subsystem; + + /** + * Creates a new WaitCommand. + */ + public Wait(float seconds, SubsystemBase subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + + m_waitTime = (long) (seconds * 1000); + m_subsystem = subsystem; + + addRequirements(m_subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_currentTime = System.currentTimeMillis(); + m_startTime = m_currentTime; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_currentTime = System.currentTimeMillis(); + SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if ((m_currentTime - m_startTime) >= m_waitTime) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 7d395cd..1cac650 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -388,7 +388,7 @@ public class Drive extends SubsystemBase { * using the Differential Drive class to manage the two inputs */ public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); + m_driveTrain.arcadeDrive(steer, move); m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); } From 9f1aff6c8d13c17a9a9bebc878f82d6a04fe8508 Mon Sep 17 00:00:00 2001 From: Aarav Shah <91212@psdschools.org> Date: Fri, 21 Feb 2020 19:44:51 -0700 Subject: [PATCH 02/23] Turn Degrees command works, Keenan did work on Dead Assist Control Mode --- src/main/java/frc4388/robot/Constants.java | 4 +- .../java/frc4388/robot/RobotContainer.java | 5 +- .../DriveWithJoystickUsingDeadAssistPID.java | 43 ++++++++--- .../frc4388/robot/commands/TurnDegrees.java | 73 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 4 +- 5 files changed, 114 insertions(+), 15 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/TurnDegrees.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6205e25..54636c2 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -30,8 +30,8 @@ public final class Constants { /* PID Constants Drive*/ public static final int DRIVE_TIMEOUT_MS = 30; public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.3); - public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.05); - public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.5); + public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.45); + public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.45); //public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); //public static final int DRIVE_CRUISE_VELOCITY = 20000; //public static final int DRIVE_ACCELERATION = 7000; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9f25a30..09ee4a4 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -50,6 +50,7 @@ import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.commands.TrackTarget; +import frc4388.robot.commands.TurnDegrees; import frc4388.robot.commands.Wait; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; @@ -144,7 +145,7 @@ public class RobotContainer { // resets the yaw of the pigeon new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive)); + .whileHeld(new RunCommand(() -> m_robotDrive.runMotionMagicPID(12, 0), m_robotDrive)); // turn 45 degrees new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) @@ -161,7 +162,7 @@ public class RobotContainer { // interrupts any running command new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) - .whenPressed(new Wait(3, m_robotDrive)); + .whenPressed(new TurnDegrees(90, m_robotDrive)); // safety for climber and leveler new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 7756772..dbdd737 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpiutil.math.MathUtil; import frc4388.robot.Constants.DriveConstants; @@ -21,6 +22,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { long m_deadTimeSteer, m_deadTimeMove; long m_deadTimeout = 100; IHandController m_controller; + boolean m_isInterrupted; /** * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller. @@ -42,6 +44,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { @Override public void initialize() { m_currTime = System.currentTimeMillis(); + resetGyroTarget(); } // Called every time the scheduler runs while the command is scheduled. @@ -54,6 +57,11 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { m_deltaTime = System.currentTimeMillis() - m_currTime; m_currTime = System.currentTimeMillis(); + if (m_isInterrupted) { + resetGyroTarget(); + m_isInterrupted = false; + } + /* If move stick is being used */ if (moveInput != 0) { m_deadTimeMove = m_currTime; @@ -90,16 +98,17 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } } - private void runDriveWithInput(double move, double steer) { + private void runDriveWithInput(double move, double steerInput) { double cosMultiplier = .45; double steerOutput = 0; double deadzone = .2; /* Curves the steer output to be similarily gradual */ - if (steer > 0){ - steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone); - } else { - steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone); + if (steerInput > 0){ + steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); + } else if (steerInput < 0) { + steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); } + m_drive.driveWithInput(move, steerOutput); System.out.println("Driving With Input"); } @@ -110,8 +119,23 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } private void runStoppedTurn(double steer) { - updateGyroTarget(steer); - m_drive.runDrivePositionPID(m_stopPos, m_targetGyro); + double cosMultiplier = 0.70; + double steerOutput = 0; + double deadzone = .2; + /* Curves the steer output to be similarily gradual */ + if (steer > 0) { + steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone); + } else if (steer < 0) { + steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone); + } + + updateGyroTarget(steerOutput); + double currentPos = m_drive.m_rightFrontMotorPos; + if (Math.abs(currentPos - m_stopPos) > 100) { + m_drive.runDrivePositionPID(m_stopPos, m_targetGyro); + } else { + m_drive.driveWithInputAux(0, m_targetGyro); + } System.out.println("Turning with Target: " + m_targetGyro); } @@ -121,8 +145,8 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { private void updateGyroTarget(double steerInput) { m_targetGyro -= 5 * steerInput * m_deltaTime; m_targetGyro = MathUtil.clamp( m_targetGyro, - m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8), - m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8)); + m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4), + m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4)); } /** @@ -137,6 +161,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { + m_isInterrupted = interrupted; } // Returns true when the command should end. diff --git a/src/main/java/frc4388/robot/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/TurnDegrees.java new file mode 100644 index 0000000..2b47050 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/TurnDegrees.java @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.subsystems.Drive; + +public class TurnDegrees extends CommandBase { + + double m_targetAngle; + Drive m_drive; + double m_currentYawInTicks; + double m_targetAngleTicksIn; + double m_targetAngleTicksOut; + int i; + + /** + * Creates a new TurnDeg. + */ + public TurnDegrees(double targetAngle, Drive subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + + m_targetAngle = targetAngle; + m_drive = subsystem; + + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_targetAngleTicksIn = (m_targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; + m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV; + m_targetAngleTicksOut = m_targetAngleTicksIn + m_currentYawInTicks; + + i = 0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV; + + m_drive.runTurningPID(m_targetAngleTicksOut); + + SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut)); + SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut); + + i++; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) { + return true; + } + return false; + } +} + diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 1cac650..b9a58b7 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -469,9 +469,9 @@ public class Drive extends SubsystemBase { * @param targetAngle target angle in degrees */ public void runTurningPID(double targetAngle) { - double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; + //double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; - runDriveVelocityPID(0, targetGyro); + runDriveVelocityPID(0, targetAngle); } /** From 5f989fb21a67c9519d27ba126a04ee17d3cc065d Mon Sep 17 00:00:00 2001 From: Aarav Shah <91212@psdschools.org> Date: Fri, 21 Feb 2020 20:46:52 -0700 Subject: [PATCH 03/23] Tweaks to Deadassist and Work on Motion Magic --- src/main/java/frc4388/robot/Constants.java | 10 ++++----- .../java/frc4388/robot/RobotContainer.java | 2 +- .../commands/DriveStraightToPositionMM.java | 7 ++++++- .../robot/commands/DriveWithJoystick.java | 2 +- .../DriveWithJoystickUsingDeadAssistPID.java | 8 +++---- .../java/frc4388/robot/subsystems/Drive.java | 21 ++++++++++--------- 6 files changed, 27 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 54636c2..1de9634 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,12 +29,10 @@ public final class Constants { /* PID Constants Drive*/ public static final int DRIVE_TIMEOUT_MS = 30; - public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.3); - public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 0.45); - public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.45); - //public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); - //public static final int DRIVE_CRUISE_VELOCITY = 20000; - //public static final int DRIVE_ACCELERATION = 7000; + public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); + public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); /* Trajectory Constants */ public static final double MAX_SPEED_METERS_PER_SECOND = 3; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 09ee4a4..b82cda6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -145,7 +145,7 @@ public class RobotContainer { // resets the yaw of the pigeon new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotDrive.runMotionMagicPID(12, 0), m_robotDrive)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 36)); // turn 45 degrees new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java index 259c571..061b6e1 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -18,6 +18,7 @@ public class DriveStraightToPositionMM extends CommandBase { double m_targetPosOut; double m_targetGyro; boolean isGoneFast; + int i; /** * Creates a new DriveToDistancePID. @@ -39,6 +40,7 @@ public class DriveStraightToPositionMM extends CommandBase { m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY); isGoneFast = false; + i = 0; } // Called every time the scheduler runs while the command is scheduled. @@ -48,6 +50,8 @@ public class DriveStraightToPositionMM extends CommandBase { //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro); + SmartDashboard.putBoolean("MM Run", true); + i++; } // Called once the command ends or is interrupted. @@ -59,9 +63,10 @@ public class DriveStraightToPositionMM extends CommandBase { @Override public boolean isFinished() { if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){ + SmartDashboard.putBoolean("MM Run", false); return true; } else { - if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100) { + if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) { isGoneFast = true; } return false; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 5387e8d..375ebd2 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -48,7 +48,7 @@ public class DriveWithJoystick extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - double cosMultiplier = .45; + double cosMultiplier = .55; double deadzone = .2; if (steerInput > 0){ steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index dbdd737..59add12 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -99,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } private void runDriveWithInput(double move, double steerInput) { - double cosMultiplier = .45; + double cosMultiplier = .55; double steerOutput = 0; double deadzone = .2; /* Curves the steer output to be similarily gradual */ @@ -119,7 +119,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { } private void runStoppedTurn(double steer) { - double cosMultiplier = 0.70; + double cosMultiplier = 0.55; double steerOutput = 0; double deadzone = .2; /* Curves the steer output to be similarily gradual */ @@ -145,8 +145,8 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { private void updateGyroTarget(double steerInput) { m_targetGyro -= 5 * steerInput * m_deltaTime; m_targetGyro = MathUtil.clamp( m_targetGyro, - m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/4), - m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/4)); + m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/2), + m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/2)); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index b9a58b7..1faef12 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -74,7 +74,7 @@ public class Drive extends SubsystemBase { public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS; public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS; - //public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS; + public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS; public final DifferentialDriveOdometry m_odometry; @@ -149,15 +149,15 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - //m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); /* PID for Back Motor control in Auto */ m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); @@ -456,6 +456,7 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); From 5015432107b3a6928d334ceb5f29213485db5dc8 Mon Sep 17 00:00:00 2001 From: Aarav Shah <91212@psdschools.org> Date: Fri, 21 Feb 2020 22:03:41 -0700 Subject: [PATCH 04/23] Account for High gear being a nice boy --- src/main/java/frc4388/robot/Constants.java | 9 ++ .../DriveWithJoystickUsingDeadAssistPID.java | 44 +++++-- .../java/frc4388/robot/subsystems/Drive.java | 114 ++++++++++++------ 3 files changed, 121 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1de9634..d146f9c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -33,6 +33,15 @@ public final class Constants { public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55); public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + public static final int DRIVE_CRUISE_VELOCITY = 20000; + public static final int DRIVE_ACCELERATION = 7000; + + public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.5); + public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final int DRIVE_CRUISE_VELOCITY_HIGH = 20000; + public static final int DRIVE_ACCELERATION_HIGH = 7000; /* Trajectory Constants */ public static final double MAX_SPEED_METERS_PER_SECOND = 3; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 59add12..6c60e40 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -24,6 +24,22 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { IHandController m_controller; boolean m_isInterrupted; + /* Deadassist Constants */ + final float stopPosVelCoefLow = 1; + final float stopPosVelCoefHigh = 3; + final float cosMultiplierLow = 0.55f; + final float cosMultiplierHigh = 0.35f; + final float targetAngleCoefLow = 5; + final float targetAngleCoefHigh = 5; + final float gyroVelCoefLow = 1; + final float gyroVelCoefHigh = 3; + + /* Deadassist Coeficients */ + final float stopPosVelCoef = 1; + final float cosMultiplier = 0.55f; + final float targetAngleCoef = 5; + final float gyroVelCoef = 1; + /** * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller. * Applies a curved ramp to the input from the controllers to make the robot less "touchy". @@ -73,15 +89,19 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { m_deadTimeSteer = m_currTime; } - /* If move stick has been pressed within 1 sec */ - if (m_currTime - m_deadTimeMove < m_deadTimeout) { - /* Curves the moveInput to be slightly more gradual at first */ - if (moveInput >= 0) { - moveOutput = -Math.cos(1.571*moveInput)+1; - } else { - moveOutput = Math.cos(1.571*moveInput)-1; - } + /* Curves the moveInput to be slightly more gradual at first */ + if (moveInput >= 0) { + moveOutput = -Math.cos(1.571*moveInput)+1; + } else { + moveOutput = Math.cos(1.571*moveInput)-1; + } + if (m_drive.isSpeedShiftHigh) { + runDriveWithInput(moveOutput, steerInput); + resetGyroTarget(); + } + /* If move stick has been pressed within 1 sec */ + else if (m_currTime - m_deadTimeMove < m_deadTimeout) { /* If steer stick has not been used for less than 1 sec */ if (m_currTime - m_deadTimeSteer < m_deadTimeout) { runDriveWithInput(moveOutput, steerInput); @@ -131,7 +151,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { updateGyroTarget(steerOutput); double currentPos = m_drive.m_rightFrontMotorPos; - if (Math.abs(currentPos - m_stopPos) > 100) { + if (Math.abs(currentPos - m_stopPos) > 200) { m_drive.runDrivePositionPID(m_stopPos, m_targetGyro); } else { m_drive.driveWithInputAux(0, m_targetGyro); @@ -145,15 +165,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { private void updateGyroTarget(double steerInput) { m_targetGyro -= 5 * steerInput * m_deltaTime; m_targetGyro = MathUtil.clamp( m_targetGyro, - m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/2), - m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/2)); + m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/3), + m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/3)); } /** * set target angle to current angle (prevents buildup of gyro error). */ private void resetGyroTarget() { - m_targetGyro = m_currentGyro; + //m_targetGyro = m_currentGyro; m_targetGyro = m_currentGyro + m_drive.getTurnRate(); } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 1faef12..d63e597 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -75,10 +75,17 @@ public class Drive extends SubsystemBase { public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS; public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS; public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS; + + public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH; + public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH; + public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_HIGH; + public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH; public final DifferentialDriveOdometry m_odometry; public DoubleSolenoid m_speedShift; + public boolean isSpeedShiftHigh; + public DoubleSolenoid m_coolFalcon; SendableChooser m_songChooser = new SendableChooser(); @@ -128,36 +135,7 @@ public class Drive extends SubsystemBase { //m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed /* PID for Front Motor Control in Teleop */ - //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - //m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - //m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); + /* PID for Back Motor control in Auto */ m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); @@ -371,6 +349,72 @@ public class Drive extends SubsystemBase { -inchesToMeters(getDistanceInches(m_rightBackMotor))); } + public void setRightMotorGains(boolean isHighGear) { + if (!isHighGear) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); + } else { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); + } + } + /** * Sets Motors to a NeutralMode. * @@ -705,11 +749,13 @@ public class Drive extends SubsystemBase { */ public void setShiftState(boolean state) { if (state == true) { - m_speedShift.set(DoubleSolenoid.Value.kForward); - } - if (state == false) { m_speedShift.set(DoubleSolenoid.Value.kReverse); - } + } + if (state == false) { + m_speedShift.set(DoubleSolenoid.Value.kForward); + } + setRightMotorGains(state); + isSpeedShiftHigh = state; } /** From 56f4e09f8c134ae956172fcc201b4030923fc7b7 Mon Sep 17 00:00:00 2001 From: Sebastian <66741@psdschools.org> Date: Sat, 22 Feb 2020 15:34:10 -0700 Subject: [PATCH 05/23] Added the CSV reading things --- src/main/deploy/Robot Data - Angles.csv | 6 + src/main/deploy/Robot Data - Distances.csv | 6 + .../java/frc4388/robot/RobotContainer.java | 1 - .../frc4388/robot/subsystems/Shooter.java | 15 ++ .../java/frc4388/utility/ShooterTables.java | 149 ++++++++++++++++++ 5 files changed, 176 insertions(+), 1 deletion(-) create mode 100644 src/main/deploy/Robot Data - Angles.csv create mode 100644 src/main/deploy/Robot Data - Distances.csv create mode 100644 src/main/java/frc4388/utility/ShooterTables.java diff --git a/src/main/deploy/Robot Data - Angles.csv b/src/main/deploy/Robot Data - Angles.csv new file mode 100644 index 0000000..273d4ba --- /dev/null +++ b/src/main/deploy/Robot Data - Angles.csv @@ -0,0 +1,6 @@ +Angle (deg),Displacement (deg) +-20,-5 +-10,-2 +0,0 +10,2 +20,5 \ No newline at end of file diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv new file mode 100644 index 0000000..e058b92 --- /dev/null +++ b/src/main/deploy/Robot Data - Distances.csv @@ -0,0 +1,6 @@ +Distance (in),Hood Ext. (u),Drum Velocity (u/ds) +21,10,10000 +100,23,11000 +200,30,14000 +300,56,17000 +480,100,20000 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b82cda6..c9a4ff8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -13,7 +13,6 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.command.WaitCommand; import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index a18aca5..a62be93 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.ShooterTables; import frc4388.utility.controller.IHandController; public class Shooter extends SubsystemBase { @@ -48,6 +49,8 @@ public class Shooter extends SubsystemBase { double velP; double input; + + ShooterTables m_shooterTable; /* * Creates a new Shooter subsystem. @@ -71,6 +74,18 @@ public class Shooter extends SubsystemBase { int closedLoopTimeMs = 1; m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterTable = new ShooterTables(); + + SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10)); + SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200)); + SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250)); + SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500)); + + SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30)); + SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10)); + SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5)); + SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30)); } @Override diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java new file mode 100644 index 0000000..638f140 --- /dev/null +++ b/src/main/java/frc4388/utility/ShooterTables.java @@ -0,0 +1,149 @@ +/*----------------------------------------------------------------------------*/ +/* 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 java.io.BufferedReader; +import java.io.File; +import java.io.FileNotFoundException; +import java.io.FileReader; +import java.io.IOException; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Add your docs here. + */ +public class ShooterTables { + double[][] m_distance = new double[50][3]; + double[][] m_angle = new double[50][2]; + + final int m_columnDistance = 0; + final int m_columnHood = 1; + final int m_columnVel = 2; + final int m_columnAngle = 0; + final int m_columnDisplacement = 1; + + int m_distanceLength; + int m_angleLength; + + public ShooterTables() { + int lineNum = 0; + + File distanceCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Distances.csv"); + File angleCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Angles.csv"); + + try { + + + BufferedReader distanceReader = new BufferedReader(new FileReader(distanceCSV)); + BufferedReader angleReader = new BufferedReader(new FileReader(angleCSV)); + String line = ""; + + while ((line = distanceReader.readLine()) != null) { + + if (lineNum == 0) { + lineNum ++; + } else { + String[] values = line.split(","); + + m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]); + m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]); + m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]); + + lineNum ++; + } + + } + + m_distanceLength = lineNum-1; + lineNum = 0; + + while ((line = angleReader.readLine()) != null) { + + if (lineNum == 0) { + lineNum ++; + } else { + String[] values = line.split(","); + + m_angle[lineNum - 1][m_columnAngle] = Double.parseDouble(values[0]); + m_angle[lineNum - 1][m_columnDisplacement] = Double.parseDouble(values[1]); + + lineNum ++; + } + } + + m_angleLength = lineNum-1; + + } catch (FileNotFoundException e) { + e.printStackTrace(); + } catch (IOException e) { + e.printStackTrace(); + } + SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); + SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); + SmartDashboard.putNumber("m_distanceLength", m_distanceLength); + SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); + SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); + SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); + } + + public double getHood(double distance) { + int i = 0; + while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { + i ++; + } + if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { + return m_distance[i][m_columnHood]; + } else { + if (i >= m_distanceLength) { + i = m_distanceLength - 1; + } + return linearInterpolation(i, distance, m_columnHood, m_distance); + } + } + + public double getVelocity(double distance) { + int i = 0; + while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { + i ++; + } + if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { + return m_distance[i][m_columnVel]; + } else { + if (i >= m_distanceLength) { + i = m_distanceLength - 1; + } + return linearInterpolation(i, distance, m_columnVel, m_distance); + } + } + + public double getAngleDisplacement(double angle) { + int i = 0; + while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) { + i ++; + } + if ((i < m_angleLength) && (m_angle[i][m_columnAngle] == angle)) { + return m_angle[i][m_columnDisplacement]; + } else { + if (i >= m_angleLength) { + i = m_angleLength - 1; + } + return linearInterpolation(i, angle, m_columnDisplacement, m_angle); + } + } + + public double linearInterpolation(int i, double value, int column, double[][] table) { + if (i != 0) { + double slope = (table[i][column] - table[i-1][column]) / (table[i][0] - table[i-1][0]); + value = slope * (value - table[i-1][0]) + table[i-1][column]; + return value; + } + return 0.0; + } +} \ No newline at end of file From ff274f96f41b88edb90dde01c3250f526aff3c64 Mon Sep 17 00:00:00 2001 From: nathanrsxtn Date: Mon, 24 Feb 2020 19:48:38 -0700 Subject: [PATCH 06/23] Initial - Untested --- .../java/frc4388/robot/RobotContainer.java | 4 +- .../robot/commands/DrivePositionMPAux.java | 90 +++++++++++++++++++ 2 files changed, 93 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/robot/commands/DrivePositionMPAux.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c9a4ff8..6d7765f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -28,6 +28,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.DriveStraightToPositionMM; @@ -234,7 +235,8 @@ public class RobotContainer { // Run path following command, then stop at the end. return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/ - return new InstantCommand(); + // return new InstantCommand(); + return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); } /** diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java new file mode 100644 index 0000000..364846a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -0,0 +1,90 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.subsystems.Drive; + +public class DrivePositionMPAux extends CommandBase { + Drive m_drive; + double m_cruiseVel; + double m_rampDist; + double m_targetPos; + double m_currentVel; + double m_currentPos; + double m_targetGyro; + double m_targetVel; + double m_rampAcc; + long m_startTime; + long m_rampRate; + + /** + * Creates a new DrivePositionMPAux. + * + * @param subsystem The drive subsystem + * @param cruiseVel The target velocity for the motors in units + * @param rampDist The distance before cruise velocity is reached in inches + * @param rampRate The time to reach the cruise velocity in seconds + * @param targetPos The target position + */ + public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH / 10; + m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH; + m_rampRate = (long) rampRate * 1000; + m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH; + //m_targetGyro = targetGyro * DriveConstants.TICKS_PER_GYRO_REV / 360; + m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_currentVel = m_drive.m_rightFrontMotorVel; + m_currentPos = m_drive.m_rightFrontMotorPos; + m_targetPos = m_targetPos + m_currentPos; + m_targetVel = m_currentVel; + m_startTime = System.currentTimeMillis(); + m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate; + } + + // Called every m_isRamptime the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_currentVel = m_drive.m_rightFrontMotorVel; + m_currentPos = m_drive.m_rightFrontMotorPos; + if (System.currentTimeMillis() - m_startTime < m_rampRate) { + // Ramping + m_targetVel += m_rampAcc * m_drive.m_deltaTime; + m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro); + } else if (m_targetPos - m_currentPos > m_rampDist) { + // Cruising + m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro); + } else { + // Deramp PID + m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH) { + return true; + } + return false; + } +} From ac82fdad12536a06ede77167ce39720cdf8afc41 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 16:15:48 -0700 Subject: [PATCH 07/23] Fix Gearing Issues --- src/main/java/frc4388/robot/Constants.java | 29 ++-- .../commands/DriveStraightToPositionMM.java | 2 +- .../commands/DriveStraightToPositionPID.java | 2 +- .../DriveWithJoystickUsingDeadAssistPID.java | 2 +- .../java/frc4388/robot/subsystems/Drive.java | 132 ++++++++++-------- 5 files changed, 92 insertions(+), 75 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d146f9c..b2d0534 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,14 +29,14 @@ public final class Constants { /* PID Constants Drive*/ public static final int DRIVE_TIMEOUT_MS = 30; - public static final Gains DRIVE_DISTANCE_GAINS = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); - public static final Gains DRIVE_VELOCITY_GAINS = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55); - public static final Gains DRIVE_MOTION_MAGIC_GAINS = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); + public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); + public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); public static final int DRIVE_CRUISE_VELOCITY = 20000; public static final int DRIVE_ACCELERATION = 7000; - public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.5); + public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5); public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains DRIVE_TURNING_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.55); public static final Gains DRIVE_MOTION_MAGIC_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); @@ -65,20 +65,27 @@ public final class Constants { /* Drive Train Characteristics */ public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double MOTOR_ROT_PER_WHEEL_ROT = 5.13; + public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13; + public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15; public static final double WHEEL_DIAMETER_INCHES = 6; public static final double TICKS_PER_GYRO_REV = 8192; /* Ratio Calculation */ + public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double TICK_TIME_TO_SECONDS = 0.1; public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS; - public static final double WHEEL_ROT_PER_MOTOR_ROT = 1/MOTOR_ROT_PER_WHEEL_ROT; - public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT; - public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; - public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV; - public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH; public static final double INCHES_PER_METER = 39.370; public static final double METERS_PER_INCH = 1/INCHES_PER_METER; + + public static final double WHEEL_ROT_PER_MOTOR_ROT_HIGH = 1/MOTOR_ROT_PER_WHEEL_ROT_HIGH; + public static final double TICKS_PER_WHEEL_REV_HIGH = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_HIGH; + public static final double TICKS_PER_INCH_HIGH = TICKS_PER_WHEEL_REV_HIGH/INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK_HIGH = 1/TICKS_PER_INCH_HIGH; + + public static final double WHEEL_ROT_PER_MOTOR_ROT_LOW = 1/MOTOR_ROT_PER_WHEEL_ROT_LOW; + public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW; + public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV; + public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW; } public static final class IntakeConstants { diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java index 061b6e1..5eba4a1 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -28,7 +28,7 @@ public class DriveStraightToPositionMM extends CommandBase { public DriveStraightToPositionMM(Drive subsystem, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH; addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index 3b74edf..3b73c1b 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -27,7 +27,7 @@ public class DriveStraightToPositionPID extends CommandBase { public DriveStraightToPositionPID(Drive subsystem, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH; addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 6c60e40..12d605c 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -96,7 +96,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - if (m_drive.isSpeedShiftHigh) { + if (m_drive.m_isSpeedShiftHigh) { runDriveWithInput(moveOutput, steerInput); resetGyroTarget(); } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index d63e597..6ff68f3 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -71,20 +71,20 @@ public class Drive extends SubsystemBase { public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); SendableChooser m_chooser = new SendableChooser(); - public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; - public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS; - public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS; - public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS; + public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; + public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW; + public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_LOW; + public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_LOW; - public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH; - public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH; - public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_HIGH; - public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH; + public static Gains m_gainsDistanceHigh = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH; + public static Gains m_gainsVelocityHigh = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH; + public static Gains m_gainsTurningHigh = DriveConstants.DRIVE_TURNING_GAINS_HIGH; + public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH; public final DifferentialDriveOdometry m_odometry; public DoubleSolenoid m_speedShift; - public boolean isSpeedShiftHigh; + public boolean m_isSpeedShiftHigh; public DoubleSolenoid m_coolFalcon; @@ -139,18 +139,18 @@ public class Drive extends SubsystemBase { /* PID for Back Motor control in Auto */ m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); /* Setup Sensors for WPI_TalonFXs */ resetEncoders(); @@ -306,12 +306,14 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); - SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); + SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); @@ -345,43 +347,12 @@ public class Drive extends SubsystemBase { } m_odometry.update(Rotation2d.fromDegrees( getHeading()), - inchesToMeters(getDistanceInches(m_leftBackMotor)), - -inchesToMeters(getDistanceInches(m_rightBackMotor))); + getDistanceInches(m_leftBackMotor), + -getDistanceInches(m_rightBackMotor)); } public void setRightMotorGains(boolean isHighGear) { if (!isHighGear) { - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); - } else { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); @@ -409,6 +380,37 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); + } else { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); @@ -650,8 +652,8 @@ public class Drive extends SubsystemBase { * @return The current wheel speeds. */ public DifferentialDriveWheelSpeeds getWheelSpeeds() { - return new DifferentialDriveWheelSpeeds( inchesToMeters(getVelocityInchesPerSecond(m_leftBackMotor)), - -inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor))); + return new DifferentialDriveWheelSpeeds( getVelocityInchesPerSecond(m_leftBackMotor), + -getVelocityInchesPerSecond(m_rightBackMotor)); } /** @@ -698,7 +700,11 @@ public class Drive extends SubsystemBase { * @return The converted value in inches */ public double ticksToInches(double ticks) { - return ticks * DriveConstants.INCHES_PER_TICK; + if (m_isSpeedShiftHigh) { + return ticks * DriveConstants.INCHES_PER_TICK_HIGH; + } else { + return ticks * DriveConstants.INCHES_PER_TICK_LOW; + } } /** @@ -707,7 +713,11 @@ public class Drive extends SubsystemBase { * @return The converted value in ticks */ public double inchesToTicks(double inches) { - return inches * DriveConstants.TICKS_PER_INCH; + if (m_isSpeedShiftHigh) { + return inches * DriveConstants.TICKS_PER_INCH_HIGH; + } else { + return inches * DriveConstants.TICKS_PER_INCH_LOW; + } } /** @@ -755,7 +765,7 @@ public class Drive extends SubsystemBase { m_speedShift.set(DoubleSolenoid.Value.kForward); } setRightMotorGains(state); - isSpeedShiftHigh = state; + m_isSpeedShiftHigh = state; } /** From b741417e0b8f423d630b059201a1b5c543c519d1 Mon Sep 17 00:00:00 2001 From: Elijah Price Date: Tue, 25 Feb 2020 17:12:56 -0700 Subject: [PATCH 08/23] initial testing --- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 7 +- .../robot/commands/DriveWithJoystick.java | 6 +- .../java/frc4388/robot/subsystems/Drive.java | 69 ++++++++++++------- 4 files changed, 56 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 5ada949..ba82993 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -106,7 +106,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { - m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); + m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); m_robotContainer.setDriveGearState(true); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f14c5d9..f46ff66 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -117,11 +117,14 @@ public class RobotContainer { // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); // sets solenoids into low gear new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whileHeld(new InstantCommand(() -> m_robotDrive.driveWithInput(0, 0), m_robotDrive)); /* Operator Buttons */ diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 5387e8d..42aedfc 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -48,12 +48,14 @@ public class DriveWithJoystick extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - double cosMultiplier = .45; + double cosMultiplier = .55; double deadzone = .2; if (steerInput > 0){ steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); - } else { + } else if (steerInput < 0) { steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); + } else { + steerOutput = 0; } m_drive.driveWithInput(moveOutput, steerOutput); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 1902e0d..d0e7601 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -26,6 +26,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.SensorTerm; import com.ctre.phoenix.motorcontrol.StatusFrame; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.music.Orchestra; @@ -33,6 +34,7 @@ import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -69,6 +71,7 @@ public class Drive extends SubsystemBase { public double m_rightFrontMotorVel; public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + //public RobotDrive m_driveTrain2 = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor) SendableChooser m_chooser = new SendableChooser(); public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; @@ -109,21 +112,31 @@ public class Drive extends SubsystemBase { coolFalcon(false); /* set back motors as followers */ - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); + //m_leftBackMotor.follow(m_leftFrontMotor); + //m_rightBackMotor.follow(m_rightFrontMotor); /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); - m_rightFrontMotor.setInverted(true); + m_rightFrontMotor.setInverted(false); //m_driveTrain.setRightSideInverted(false); - m_leftBackMotor.setInverted(InvertType.FollowMaster); - m_rightBackMotor.setInverted(InvertType.FollowMaster); + //m_leftBackMotor.setInverted(InvertType.FollowMaster); + //m_rightBackMotor.setInverted(InvertType.FollowMaster); + + float rampRate = 0.1f; + m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); + + SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 20, 25, 0.01); + m_rightFrontMotor.configSupplyCurrentLimit(c); + m_leftFrontMotor.configSupplyCurrentLimit(c); setDriveTrainNeutralMode(NeutralMode.Coast); /* deadbands */ - m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE - m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed + //m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE + //m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed //m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE //m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed @@ -322,26 +335,36 @@ public class Drive extends SubsystemBase { m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity(); try { - SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); + SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); - - SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - + /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity()); + */ SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); - //SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); - //SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); - //SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent()); + + + SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent()); + + SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); + SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent()); + SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent()); + SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); @@ -350,10 +373,10 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - SmartDashboard.putString("Odometry Values Meters", getPose().toString()); - SmartDashboard.putNumber("Odometry Heading", getHeading()); + //SmartDashboard.putString("Odometry Values Meters", getPose().toString()); + //SmartDashboard.putNumber("Odometry Heading", getHeading()); - SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); + //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); //SmartDashboard.putNumber("Delta Time", m_deltaTime); if (currentSong != m_songChooser.getSelected()){ @@ -389,8 +412,8 @@ public class Drive extends SubsystemBase { */ public void driveWithInput(double move, double steer) { m_driveTrain.arcadeDrive(move, steer); - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); + m_leftBackMotor.follow(m_leftFrontMotor, FollowerType.PercentOutput); + m_rightBackMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); } /** From 0541a2463c4197fca8f707c3397e5fd6a9a3b9af Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 17:47:47 -0700 Subject: [PATCH 09/23] Odometry fixes (but not really) - Seems to only work on the left motor in low gear...no idea why --- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 2 +- .../robot/commands/DriveWithJoystick.java | 4 ++- .../java/frc4388/robot/subsystems/Drive.java | 34 +++++++++++++------ 4 files changed, 29 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 5ada949..eca8337 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -79,7 +79,7 @@ public class Robot extends TimedRobot { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - m_robotContainer.setDriveGearState(true); + //m_robotContainer.setDriveGearState(true); m_robotContainer.resetOdometry(); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b82cda6..f540f1d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -92,7 +92,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController())); + m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // drives climber with input from triggers on the opperator controller diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 375ebd2..42aedfc 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -52,8 +52,10 @@ public class DriveWithJoystick extends CommandBase { double deadzone = .2; if (steerInput > 0){ steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); - } else { + } else if (steerInput < 0) { steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); + } else { + steerOutput = 0; } m_drive.driveWithInput(moveOutput, steerOutput); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 6ff68f3..edbd73a 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -95,6 +95,11 @@ public class Drive extends SubsystemBase { public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle; + public double m_totalLeftDistanceInches = 0; + public double m_totalRightDistanceInches = 0; + public double m_lastLeftPosTicks = 0; + public double m_lastRightPosTicks = 0; + /** * Add your docs here. */ @@ -106,7 +111,7 @@ public class Drive extends SubsystemBase { m_rightBackMotor.configFactoryDefault(); m_pigeon.configFactoryDefault(); resetGyroYaw(); - + m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), new Pose2d(0, 0, new Rotation2d()) ); @@ -298,6 +303,13 @@ public class Drive extends SubsystemBase { m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition(); m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity(); + + m_totalRightDistanceInches += ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition() - m_lastRightPosTicks); + m_totalLeftDistanceInches += ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition() - m_lastLeftPosTicks); + + m_odometry.update(Rotation2d.fromDegrees( getHeading()), + getDistanceInches(m_rightFrontMotor), + -getDistanceInches(m_rightFrontMotor)); try { SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); @@ -312,8 +324,8 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); - SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); + SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); + SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); @@ -346,9 +358,8 @@ public class Drive extends SubsystemBase { // e.printStackTrace(System.err); } - m_odometry.update(Rotation2d.fromDegrees( getHeading()), - getDistanceInches(m_leftBackMotor), - -getDistanceInches(m_rightBackMotor)); + m_lastRightPosTicks = m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition(); + m_lastLeftPosTicks = m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition(); } public void setRightMotorGains(boolean isHighGear) { @@ -470,7 +481,7 @@ public class Drive extends SubsystemBase { m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); - //m_driveTrain.feedWatchdog(); + m_driveTrain.feedWatchdog(); } /** @@ -488,7 +499,7 @@ public class Drive extends SubsystemBase { m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); - //m_driveTrain.feedWatchdog(); + m_driveTrain.feedWatchdog(); } /** @@ -619,9 +630,9 @@ public class Drive extends SubsystemBase { //lol //sko //ridge - /** //brayden=bad coder - * Returns the heading of the robot + /** + * Returns the heading of the robot * @return The robot's heading in degrees, from -180 to 180 */ public double getHeading() { @@ -664,6 +675,9 @@ public class Drive extends SubsystemBase { m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); + + m_totalLeftDistanceInches = 0; + m_totalRightDistanceInches = 0; } /** From 7339658077b70d486beb1edcb2d10d6197660de8 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 19:15:15 -0700 Subject: [PATCH 10/23] Fix watchdog feeds and steer output curving --- .../java/frc4388/robot/commands/DriveWithJoystick.java | 4 ++-- src/main/java/frc4388/robot/subsystems/Drive.java | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 42aedfc..fe1c8e5 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -51,9 +51,9 @@ public class DriveWithJoystick extends CommandBase { double cosMultiplier = .55; double deadzone = .2; if (steerInput > 0){ - steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone); + steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier; } else if (steerInput < 0) { - steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone); + steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier; } else { steerOutput = 0; } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index d0e7601..9f2639b 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -119,8 +119,8 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.setInverted(false); m_rightFrontMotor.setInverted(false); //m_driveTrain.setRightSideInverted(false); - //m_leftBackMotor.setInverted(InvertType.FollowMaster); - //m_rightBackMotor.setInverted(InvertType.FollowMaster); + m_leftBackMotor.setInverted(InvertType.FollowMaster); + m_rightBackMotor.setInverted(InvertType.FollowMaster); float rampRate = 0.1f; m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); @@ -128,9 +128,9 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); - SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 20, 25, 0.01); - m_rightFrontMotor.configSupplyCurrentLimit(c); - m_leftFrontMotor.configSupplyCurrentLimit(c); + //SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 40, 35, 0.01); + //m_rightFrontMotor.configSupplyCurrentLimit(c); + //m_leftFrontMotor.configSupplyCurrentLimit(c); setDriveTrainNeutralMode(NeutralMode.Coast); From ab6cc48c461052da84743ffeefa1871695ee52bb Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 20:45:29 -0700 Subject: [PATCH 11/23] Reduce deadband for greater range of motion --- src/main/java/frc4388/robot/commands/DriveWithJoystick.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index fe1c8e5..f51621a 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -49,7 +49,7 @@ public class DriveWithJoystick extends CommandBase { } double cosMultiplier = .55; - double deadzone = .2; + double deadzone = .1; if (steerInput > 0){ steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier; } else if (steerInput < 0) { From f7dc572a4463d88d58f47f5ebf5358dd83b87953 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 20:52:20 -0700 Subject: [PATCH 12/23] Minor Cleanup Removed set up of Neutral Mode in the constructor because Neutral Mode is controlled in Robot.java --- src/main/java/frc4388/robot/Robot.java | 1 - src/main/java/frc4388/robot/subsystems/Drive.java | 2 -- 2 files changed, 3 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ba82993..feab056 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -64,7 +64,6 @@ public class Robot extends TimedRobot { @Override public void disabledInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); - //m_robotContainer.setDriveGearState(true); } @Override diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 9f2639b..74b9ef9 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -132,8 +132,6 @@ public class Drive extends SubsystemBase { //m_rightFrontMotor.configSupplyCurrentLimit(c); //m_leftFrontMotor.configSupplyCurrentLimit(c); - setDriveTrainNeutralMode(NeutralMode.Coast); - /* deadbands */ //m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE //m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed From 3e829b4db8e2b13ce0cd94bda172ed84b1b7829b Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 21:12:02 -0700 Subject: [PATCH 13/23] Remove extraneous code --- src/main/java/frc4388/robot/RobotContainer.java | 6 +----- src/main/java/frc4388/robot/subsystems/Drive.java | 9 ++++----- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f46ff66..f57b496 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -123,13 +123,9 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileHeld(new InstantCommand(() -> m_robotDrive.driveWithInput(0, 0), m_robotDrive)); - - /* Operator Buttons */ - //TODO: Shooter Buttons + //TODO: Shooter Buttons // shoots until released //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) // .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5)); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 74b9ef9..0dae7c8 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -71,7 +71,6 @@ public class Drive extends SubsystemBase { public double m_rightFrontMotorVel; public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - //public RobotDrive m_driveTrain2 = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor) SendableChooser m_chooser = new SendableChooser(); public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS; @@ -112,8 +111,8 @@ public class Drive extends SubsystemBase { coolFalcon(false); /* set back motors as followers */ - //m_leftBackMotor.follow(m_leftFrontMotor); - //m_rightBackMotor.follow(m_rightFrontMotor); + m_leftBackMotor.follow(m_leftFrontMotor); + m_rightBackMotor.follow(m_rightFrontMotor); /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); @@ -410,8 +409,8 @@ public class Drive extends SubsystemBase { */ public void driveWithInput(double move, double steer) { m_driveTrain.arcadeDrive(move, steer); - m_leftBackMotor.follow(m_leftFrontMotor, FollowerType.PercentOutput); - m_rightBackMotor.follow(m_rightFrontMotor, FollowerType.PercentOutput); + m_leftBackMotor.follow(m_leftFrontMotor); + m_rightBackMotor.follow(m_rightFrontMotor); } /** From 322e4c7a44e05d2f515f554ae8ac4947ef6a6b0f Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 21:33:12 -0700 Subject: [PATCH 14/23] Change PIDs to expect robot to be in Low Gear Eventually should be changed to be gear agnostic. Maybe have conversion factor be in drive subsytem and change depending on gear shift. --- .../java/frc4388/robot/commands/DrivePositionMPAux.java | 8 ++++---- .../frc4388/robot/commands/DriveStraightToPositionMM.java | 2 +- .../robot/commands/DriveStraightToPositionPID.java | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java index 364846a..6883468 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -36,10 +36,10 @@ public class DrivePositionMPAux extends CommandBase { public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH / 10; - m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH; + m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10; + m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH_LOW; m_rampRate = (long) rampRate * 1000; - m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH; + m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH_LOW; //m_targetGyro = targetGyro * DriveConstants.TICKS_PER_GYRO_REV / 360; m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); addRequirements(m_drive); @@ -82,7 +82,7 @@ public class DrivePositionMPAux extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH) { + if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) { return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java index 5eba4a1..c56a36b 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -28,7 +28,7 @@ public class DriveStraightToPositionMM extends CommandBase { public DriveStraightToPositionMM(Drive subsystem, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW; addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index 3b73c1b..949a4bf 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -27,7 +27,7 @@ public class DriveStraightToPositionPID extends CommandBase { public DriveStraightToPositionPID(Drive subsystem, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_HIGH; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW; addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } From 1b31a6d5082c4ceb888a0dc9fe24b4fb79cf7a29 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 22:25:20 -0700 Subject: [PATCH 15/23] Cleanup Imports --- src/main/java/frc4388/robot/RobotContainer.java | 14 -------------- src/main/java/frc4388/robot/subsystems/Drive.java | 14 -------------- 2 files changed, 28 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 97676e6..9bd371f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -9,7 +9,6 @@ package frc4388.robot; import java.util.List; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.Joystick; @@ -17,7 +16,6 @@ import edu.wpi.first.wpilibj.controller.RamseteController; import edu.wpi.first.wpilibj.geometry.Pose2d; 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.trajectory.Trajectory; import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; @@ -29,17 +27,11 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DrivePositionMPAux; -import frc4388.robot.commands.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.DriveStraightToPositionMM; -import frc4388.robot.commands.DriveStraightToPositionPID; -import frc4388.robot.commands.DriveWithJoystick; -import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID; -import frc4388.robot.commands.DriveWithJoystickDriveStraight; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; -import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.commands.StorageIntakeGroup; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; @@ -47,17 +39,11 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.LED; import frc4388.robot.commands.TrackTarget; -import frc4388.robot.commands.TurnDegrees; -import frc4388.robot.commands.Wait; import frc4388.robot.commands.storageOutake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.Storage; -import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index b690162..fc5e310 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -8,15 +8,6 @@ package frc4388.robot.subsystems; import java.io.File; -import java.io.FilenameFilter; -import java.io.IOException; -import java.nio.file.Files; -import java.nio.file.Path; -import java.nio.file.Paths; -import java.util.ArrayList; -import java.util.List; -import java.util.stream.Collectors; -import java.util.stream.Stream; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; @@ -26,7 +17,6 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.SensorTerm; import com.ctre.phoenix.motorcontrol.StatusFrame; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.music.Orchestra; @@ -34,21 +24,17 @@ import com.ctre.phoenix.sensors.PigeonIMU; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; -import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds; -import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpiutil.math.MathUtil; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Gains; From f7dfe25744ec65e2621f33badf412a48f5c4c235 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 22:25:37 -0700 Subject: [PATCH 16/23] Cleanup Robot Container --- .../java/frc4388/robot/RobotContainer.java | 131 ++++++++++-------- 1 file changed, 75 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9bd371f..8f0e772 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -102,18 +102,24 @@ public class RobotContainer { * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - /* Driver Buttons */ - //new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - // .whileHeld(new DriveWithJoystickAuxPID(m_robotDrive, getDriverController())); - - // resets the yaw of the pigeon - new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 36)); - - // turn 45 degrees + /* Test Buttons */ + // A driver test button + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whenPressed(new InstantCommand()); + + // B driver test button + new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) + .whenPressed(new InstantCommand()); + + // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive)); + .whenPressed(new InstantCommand()); + + // X driver test button + new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) + .whenPressed(new InstantCommand()); + /* Driver Buttons */ // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); @@ -123,7 +129,6 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); /* Operator Buttons */ - //TODO: Shooter Buttons // shoots until released //new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) @@ -159,7 +164,62 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) .whileHeld(new storageOutake(m_robotStorage)); } - + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // Create config for trajectory + TrajectoryConfig config = getTrajectoryConfig(); + Trajectory trajectory = getTrajectory(config); + RamseteCommand ramseteCommand = getRamseteCommand(trajectory); + // Run path following command, then stop at the end. + //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + + // return new InstantCommand(); + return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); + } + + TrajectoryConfig getTrajectoryConfig() { + return new TrajectoryConfig( + DriveConstants.MAX_SPEED_METERS_PER_SECOND, + DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED) + // Add kinematics to ensure max speed is actually obeyed + .setKinematics(DriveConstants.kDriveKinematics); + } + + Trajectory getTrajectory(TrajectoryConfig config) { + Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( + // Start at the origin facing the +X direction + new Pose2d(0, 0, new Rotation2d(0)), + // Pass through these two interior waypoints, making an 's' curve path + List.of( + new Translation2d(10, 0) + ), + // End 3 meters straight ahead of where we started, facing forward + new Pose2d(20, 20, new Rotation2d(0)), + // Pass config + config); + // 10 = 20, 20 = 35, 30 = 53.5 + // (0,10) = (8,22) + + return exampleTrajectory; + } + + RamseteCommand getRamseteCommand(Trajectory trajectory) { + RamseteCommand ramseteCommand = new RamseteCommand( + trajectory, + m_robotDrive::getPose, + new RamseteController(), + DriveConstants.kDriveKinematics, + m_robotDrive::tankDriveVelocity, + m_robotDrive); + + return ramseteCommand; + } + /** * Sets Motors to a NeutralMode. * @param mode NeutralMode to set motors to @@ -176,55 +236,14 @@ public class RobotContainer { m_robotDrive.setShiftState(state); } - public void configDriveTrainSensors(FeedbackDevice type) { - m_robotDrive.configMotorSensor(type); - } - + /** + * + */ public void resetOdometry() { m_robotDrive.resetGyroAngles(); m_robotDrive.setOdometry(new Pose2d()); } - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - - // Create config for trajectory - /*TrajectoryConfig config = new TrajectoryConfig( DriveConstants.MAX_SPEED_METERS_PER_SECOND, - DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED) - // Add kinematics to ensure max speed is actually obeyed - .setKinematics(DriveConstants.kDriveKinematics); - - Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( - // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(0)), - // Pass through these two interior waypoints, making an 's' curve path - List.of( - new Translation2d(10, 0) - ), - // End 3 meters straight ahead of where we started, facing forward - new Pose2d(20, 20, new Rotation2d(0)), - // Pass config - config); - // 10 = 20, 20 = 35, 30 = 53.5 - // (0,10) = (8,22) - RamseteCommand ramseteCommand = new RamseteCommand( - exampleTrajectory, - m_robotDrive::getPose, - new RamseteController(), - DriveConstants.kDriveKinematics, - m_robotDrive::tankDriveVelocity, - m_robotDrive); - - // Run path following command, then stop at the end. - return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/ - // return new InstantCommand(); - return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); - } - /** * Used for analog inputs like triggers and axises. * @return IHandController interface for the Driver Controller. From 1f1d5ed158782b3717a041bf78476011c3df358d Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 22:27:53 -0700 Subject: [PATCH 17/23] Group like default commands together to improve readability --- src/main/java/frc4388/robot/RobotContainer.java | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8f0e772..3f40a5d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -83,15 +83,14 @@ public class RobotContainer { m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives intake with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); - // drives climber with input from triggers on the opperator controller - m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); - // continually sends updates to the Blinkin LED controller to keep the lights on - m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); // runs the drum shooter in idle mode m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter)); + // drives climber with input from triggers on the opperator controller + m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // drives the leveler with an axis input from the driver controller m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); - + // continually sends updates to the Blinkin LED controller to keep the lights on + m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); } From f60e4a8bf9d2c40f11b2ac9bf63136ed42f7a6d0 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 22:39:18 -0700 Subject: [PATCH 18/23] Reorganise Drive Variables --- .../java/frc4388/robot/subsystems/Drive.java | 52 ++++++++++--------- 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fc5e310..0eee452 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -42,51 +42,56 @@ import frc4388.robot.Gains; * Add your docs here. */ public class Drive extends SubsystemBase { - // Put methods for controlling this subsystem - // here. Call these from Commands. - + /* Create Motors, Gyros, Solenoids, etc */ public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID); + public DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1); + public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2); + + /* Drive objects to manage Drive Train */ + public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + public final DifferentialDriveOdometry m_odometry; public Orchestra m_orchestra = new Orchestra(); - public double m_rightFrontMotorPos; - - public double m_rightFrontMotorVel; - - public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - - SendableChooser m_chooser = new SendableChooser(); + /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW; public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_LOW; public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_LOW; + /* High Gear Gains */ public static Gains m_gainsDistanceHigh = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH; public static Gains m_gainsVelocityHigh = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH; public static Gains m_gainsTurningHigh = DriveConstants.DRIVE_TURNING_GAINS_HIGH; public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH; - - public final DifferentialDriveOdometry m_odometry; - - public DoubleSolenoid m_speedShift; - public boolean m_isSpeedShiftHigh; - - public DoubleSolenoid m_coolFalcon; - - SendableChooser m_songChooser = new SendableChooser(); + /* Timey Whimey */ + public long m_lastTime; + public long m_deltaTime = 0; public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000); - public long m_lastTime, m_deltaTime; //in milliseconds - public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle; + /* Position Tracking */ + public double m_rightFrontMotorPos = 0; + public double m_rightFrontMotorVel = 0; public double m_totalLeftDistanceInches = 0; public double m_totalRightDistanceInches = 0; + public double m_lastLeftPosTicks = 0; public double m_lastRightPosTicks = 0; + + public double m_lastAngleYaw = 0; + public double m_currentAngleYaw = 0; + public double m_kinematicsTargetAngle = 0; + + /* Smart Dashboard Objects */ + SendableChooser m_songChooser = new SendableChooser(); + + /* Misc */ + public boolean m_isSpeedShiftHigh; /** * Add your docs here. @@ -101,10 +106,7 @@ public class Drive extends SubsystemBase { resetGyroYaw(); m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), - new Pose2d(0, 0, new Rotation2d()) ); - - m_speedShift = new DoubleSolenoid(7,0,1); - m_coolFalcon = new DoubleSolenoid(7,3,2); + new Pose2d(0, 0, new Rotation2d())); coolFalcon(false); From 5225a98c2c5d4ae496fe0ca6e24b7397a9eb8d06 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 23:18:17 -0700 Subject: [PATCH 19/23] Cleanup Drive Constructor Also move constants to Drive Constants --- src/main/java/frc4388/robot/Constants.java | 13 ++ .../java/frc4388/robot/subsystems/Drive.java | 152 ++++++++---------- 2 files changed, 79 insertions(+), 86 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 50f0f9c..7e01b54 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -7,6 +7,8 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; + import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; import frc4388.utility.LEDPatterns; @@ -86,6 +88,17 @@ public final class Constants { public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW; public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV; public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW; + + public static final double OPEN_LOOP_RAMP_RATE = 0.1; + public static final double NEUTRAL_DEADBAND = 0.04; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = + new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01); + public static final boolean isRightMotorInverted = false; + public static final boolean isLeftMotorInverted = false; + public static final boolean isRightArcadeInverted = false; + public static final boolean isAuxPIDInverted = false; + + public static final int CLOSED_LOOP_TIME_MS = 1; } public static final class IntakeConstants { diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 0eee452..3976c47 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -8,6 +8,7 @@ package frc4388.robot.subsystems; import java.io.File; + import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; @@ -25,7 +26,6 @@ import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.DoubleSolenoid; - import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; @@ -35,12 +35,10 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Gains; -/** - * Add your docs here. - */ public class Drive extends SubsystemBase { /* Create Motors, Gyros, Solenoids, etc */ public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); @@ -52,9 +50,9 @@ public class Drive extends SubsystemBase { public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2); /* Drive objects to manage Drive Train */ - public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + public DifferentialDrive m_driveTrain; public final DifferentialDriveOdometry m_odometry; - public Orchestra m_orchestra = new Orchestra(); + public Orchestra m_orchestra; /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; @@ -82,7 +80,7 @@ public class Drive extends SubsystemBase { public double m_lastLeftPosTicks = 0; public double m_lastRightPosTicks = 0; - + public double m_lastAngleYaw = 0; public double m_currentAngleYaw = 0; public double m_kinematicsTargetAngle = 0; @@ -104,43 +102,36 @@ public class Drive extends SubsystemBase { m_rightBackMotor.configFactoryDefault(); m_pigeon.configFactoryDefault(); resetGyroYaw(); - - m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), - new Pose2d(0, 0, new Rotation2d())); - - coolFalcon(false); - - /* set back motors as followers */ - m_leftBackMotor.follow(m_leftFrontMotor); - m_rightBackMotor.follow(m_rightFrontMotor); /* flip input so forward becomes back, etc */ - m_leftFrontMotor.setInverted(false); - m_rightFrontMotor.setInverted(false); - //m_driveTrain.setRightSideInverted(false); - m_leftBackMotor.setInverted(InvertType.FollowMaster); - m_rightBackMotor.setInverted(InvertType.FollowMaster); + m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted); + m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted); + m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted); + m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted); + m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted); - float rampRate = 0.1f; - m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS); + /* Config Open Loop Ramp so we don't make sudden output changes */ + m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); - //SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 40, 35, 0.01); - //m_rightFrontMotor.configSupplyCurrentLimit(c); - //m_leftFrontMotor.configSupplyCurrentLimit(c); + /* Config Supply Current Limit (Use only for debugging) */ + m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - /* deadbands */ - //m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE - //m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed - //m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE - //m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed + /* Config deadbands so that */ + m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); /* PID for Front Motor Control in Teleop */ - + setRightMotorGains(false); - /* PID for Back Motor control in Auto */ + /* PID for Back Motor Control in Tank Drive Vel */ m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); @@ -155,7 +146,7 @@ public class Drive extends SubsystemBase { m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - /* Setup Sensors for WPI_TalonFXs */ + /* Reset Sensors for WPI_TalonFXs */ resetEncoders(); /* Configure the left Talon's selected sensor as local QuadEncoder */ @@ -175,50 +166,43 @@ public class Drive extends SubsystemBase { /* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */ m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source - RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1] + RemoteSensorSource.TalonSRX_SelectedSensor, + DriveConstants.REMOTE_0, // Source number [0, 1] DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + /* Diff Signal signal to be used for Distance*/ + m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); + + /* Configure Diff [Sum of both QuadEncoders] to be used for Primary PID Index */ + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference, + DriveConstants.PID_PRIMARY, + DriveConstants.DRIVE_TIMEOUT_MS); + /* Configure the Pigeon IMU to the other Remote Slot available on the right Talon */ m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw, DriveConstants.REMOTE_1, DriveConstants.DRIVE_TIMEOUT_MS); - /* Setup Sum signal to be used for Distance */ - m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Diff Signal */ - m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ - configMotorSensor(FeedbackDevice.SensorDifference); - - /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */ - /* - m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, // Coefficient - DriveConstants.PID_PRIMARY, // PID Slot of Source - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout - */ - + /* Config Remote1 to be used for Aux PID Index */ m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); - /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */ - //m_rightFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS); - - /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */ - //m_leftFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS); + /** + * configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local + * output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's + * local output is PID0 - PID1, and other side Talon is PID0 + PID1 + */ + m_rightFrontMotor.configAuxPIDPolarity(DriveConstants.isAuxPIDInverted, DriveConstants.DRIVE_TIMEOUT_MS); /* Set status frame periods to ensure we don't have stale data */ m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS); m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS); - m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS); m_rightBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS); + m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); /** * Max out the peak output (for all modes). However you can limit the output of @@ -240,37 +224,39 @@ public class Drive extends SubsystemBase { * derivative error never gets large enough to be useful. - sensor movement is * very slow causing the derivative error to be near zero. */ - int closedLoopTimeMs = 1; m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, - closedLoopTimeMs, + DriveConstants.CLOSED_LOOP_TIME_MS, DriveConstants.DRIVE_TIMEOUT_MS); m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_TURN, - closedLoopTimeMs, + DriveConstants.CLOSED_LOOP_TIME_MS, DriveConstants.DRIVE_TIMEOUT_MS); m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, - closedLoopTimeMs, + DriveConstants.CLOSED_LOOP_TIME_MS, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, - closedLoopTimeMs, + m_rightBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY, + DriveConstants.CLOSED_LOOP_TIME_MS, DriveConstants.DRIVE_TIMEOUT_MS); - - /** - * configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local - * output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's - * local output is PID0 - PID1, and other side Talon is PID0 + PID1 - */ - m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS); + + /* Set up Differential Drive */ + m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); - m_lastTime = System.currentTimeMillis(); + /* Set up Differential Drive Odometry. */ + m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()), + new Pose2d(0, 0, new Rotation2d())); + + /* Set up Orchestra */ + m_orchestra = new Orchestra(); + /* Set up music for drive train */ m_orchestra.addInstrument(m_leftBackMotor); m_orchestra.addInstrument(m_rightFrontMotor); m_orchestra.addInstrument(m_rightBackMotor); m_orchestra.addInstrument(m_leftFrontMotor); + /* Create chooser to choose song to play */ File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs"); System.err.println(songsDir.getPath()); String[] songsStrings = songsDir.list(); @@ -278,6 +264,9 @@ public class Drive extends SubsystemBase { m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString); } Shuffleboard.getTab("Songs").add(m_songChooser); + + /* Start counting time */ + m_lastTime = System.currentTimeMillis(); } String currentSong = ""; @@ -583,15 +572,6 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } - /** - * Selects the feedback device for the motors. - * @param feedbackDevice The feedback device to set it to, usually SensorDifference or - */ - public void configMotorSensor(FeedbackDevice type) { - m_rightFrontMotor.configSelectedFeedbackSensor( type, DriveConstants.PID_PRIMARY, - DriveConstants.DRIVE_TIMEOUT_MS); - } - /** * Returns the current yaw of the pigeon */ From a36a7f978ee325cb59b9fd5a5f7196e528e0e225 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 23:34:38 -0700 Subject: [PATCH 20/23] Organize Drive Periodic --- .../robot/commands/DrivePositionMPAux.java | 2 +- .../java/frc4388/robot/subsystems/Drive.java | 70 ++++++++++--------- 2 files changed, 39 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java index 6883468..68d9538 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -63,7 +63,7 @@ public class DrivePositionMPAux extends CommandBase { m_currentPos = m_drive.m_rightFrontMotorPos; if (System.currentTimeMillis() - m_startTime < m_rampRate) { // Ramping - m_targetVel += m_rampAcc * m_drive.m_deltaTime; + m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs; m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro); } else if (m_targetPos - m_currentPos > m_rampDist) { // Cruising diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3976c47..8caae27 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -67,9 +67,10 @@ public class Drive extends SubsystemBase { public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH; /* Timey Whimey */ - public long m_lastTime; - public long m_deltaTime = 0; - public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000); + public long m_currentTimeMs = System.currentTimeMillis(); + public long m_lastTimeMs = m_currentTimeMs; + public long m_deltaTimeMs = 0; + public long m_currentTimeSec = m_currentTimeMs / 1000; /* Position Tracking */ public double m_rightFrontMotorPos = 0; @@ -78,6 +79,8 @@ public class Drive extends SubsystemBase { public double m_totalLeftDistanceInches = 0; public double m_totalRightDistanceInches = 0; + public double m_currentLeftPosTicks = 0; + public double m_currentRightPosTicks = 0; public double m_lastLeftPosTicks = 0; public double m_lastRightPosTicks = 0; @@ -90,6 +93,7 @@ public class Drive extends SubsystemBase { /* Misc */ public boolean m_isSpeedShiftHigh; + String m_currentSong = ""; /** * Add your docs here. @@ -266,14 +270,34 @@ public class Drive extends SubsystemBase { Shuffleboard.getTab("Songs").add(m_songChooser); /* Start counting time */ - m_lastTime = System.currentTimeMillis(); - } + m_lastTimeMs = System.currentTimeMillis(); + } - String currentSong = ""; @Override public void periodic() { - m_currentTimeSec = (int)(System.currentTimeMillis() / 1000); - SmartDashboard.putNumber("Time Seconds", System.currentTimeMillis()); + m_lastTimeMs = m_currentTimeMs; + m_currentTimeMs = System.currentTimeMillis(); + m_currentTimeSec = m_currentTimeMs / 1000; + m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs; + //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); + + m_lastAngleYaw = m_currentAngleYaw; + m_currentAngleYaw = getGyroYaw(); + + m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition(); + m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity(); + + m_lastRightPosTicks = m_currentRightPosTicks; + m_lastLeftPosTicks = m_currentLeftPosTicks; + m_currentRightPosTicks = m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition(); + m_currentLeftPosTicks = m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition(); + + m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks); + m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks); + + m_odometry.update(Rotation2d.fromDegrees( getHeading()), + getDistanceInches(m_leftFrontMotor), + -getDistanceInches(m_rightFrontMotor)); if (m_currentTimeSec % 30 == 0) { coolFalcon(true); @@ -282,21 +306,6 @@ public class Drive extends SubsystemBase { coolFalcon(false); SmartDashboard.putBoolean("Solenoid", false); } - - m_deltaTime = System.currentTimeMillis() - m_lastTime; - m_lastTime = System.currentTimeMillis(); - m_lastAngleYaw = m_currentAngleYaw; - m_currentAngleYaw = getGyroYaw(); - - m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition(); - m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity(); - - m_totalRightDistanceInches += ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition() - m_lastRightPosTicks); - m_totalLeftDistanceInches += ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition() - m_lastLeftPosTicks); - - m_odometry.update(Rotation2d.fromDegrees( getHeading()), - getDistanceInches(m_rightFrontMotor), - -getDistanceInches(m_rightFrontMotor)); try { //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); @@ -349,18 +358,15 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); //SmartDashboard.putNumber("Delta Time", m_deltaTime); - if (currentSong != m_songChooser.getSelected()){ - currentSong = m_songChooser.getSelected(); - selectSong(currentSong); - System.err.println(currentSong); + if (m_currentSong != m_songChooser.getSelected()){ + m_currentSong = m_songChooser.getSelected(); + selectSong(m_currentSong); + //System.err.println(m_currentSong); } } catch (Exception e) { - System.err.println("Error in the Drive Subsystem"); + System.err.println("Error while using Drive SmartDashboard"); // e.printStackTrace(System.err); } - - m_lastRightPosTicks = m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition(); - m_lastLeftPosTicks = m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition(); } public void setRightMotorGains(boolean isHighGear) { @@ -639,7 +645,7 @@ public class Drive extends SubsystemBase { */ public double getTurnRate() { double deltaYaw = m_currentAngleYaw - m_lastAngleYaw; - double turnRate = 1000 * deltaYaw / m_deltaTime; + double turnRate = 1000 * deltaYaw / m_deltaTimeMs; return turnRate; } From dc64155c0d402af94af1d0126d1c8d2c7f08fc3c Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 23:47:19 -0700 Subject: [PATCH 21/23] Cleanup Drive Subsystem Methods --- .../java/frc4388/robot/subsystems/Drive.java | 458 +++++++++--------- 1 file changed, 234 insertions(+), 224 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 8caae27..a0e49ae 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -13,7 +13,6 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.FollowerType; -import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.SensorTerm; @@ -279,7 +278,6 @@ public class Drive extends SubsystemBase { m_currentTimeMs = System.currentTimeMillis(); m_currentTimeSec = m_currentTimeMs / 1000; m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs; - //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); m_lastAngleYaw = m_currentAngleYaw; m_currentAngleYaw = getGyroYaw(); @@ -299,152 +297,8 @@ public class Drive extends SubsystemBase { getDistanceInches(m_leftFrontMotor), -getDistanceInches(m_rightFrontMotor)); - if (m_currentTimeSec % 30 == 0) { - coolFalcon(true); - SmartDashboard.putBoolean("Solenoid", true); - } else if ((m_currentTimeSec - 1) % 30 == 0) { - coolFalcon(false); - SmartDashboard.putBoolean("Solenoid", false); - } - - try { - //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); - //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); - //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); - SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); - SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); - SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); - SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); - - //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); - //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); - //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); - //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); - - //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); - SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); - - /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity()); - SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity()); - */ - - SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); - SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); - - SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent()); - SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent()); - - SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); - SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent()); - SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent()); - SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); - - //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); - //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); - - //SmartDashboard.putString("Odometry Values Meters", getPose().toString()); - //SmartDashboard.putNumber("Odometry Heading", getHeading()); - - //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); - //SmartDashboard.putNumber("Delta Time", m_deltaTime); - - if (m_currentSong != m_songChooser.getSelected()){ - m_currentSong = m_songChooser.getSelected(); - selectSong(m_currentSong); - //System.err.println(m_currentSong); - } - } catch (Exception e) { - System.err.println("Error while using Drive SmartDashboard"); - // e.printStackTrace(System.err); - } - } - - public void setRightMotorGains(boolean isHighGear) { - if (!isHighGear) { - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); - } else { - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); - } - } - - /** - * Sets Motors to a NeutralMode. - * - * @param mode NeutralMode to set motors to - */ - public void setDriveTrainNeutralMode(NeutralMode mode) { - m_leftFrontMotor.setNeutralMode(mode); - m_rightFrontMotor.setNeutralMode(mode); - m_leftBackMotor.setNeutralMode(mode); - m_rightBackMotor.setNeutralMode(mode); + runFalconCooling(); + updateSmartDashboard(); } /** @@ -578,6 +432,104 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } + /** + * Set to high or low gear based on boolean state, true = high, false = low + * @param state Chooses between high or low gear + */ + public void setShiftState(boolean state) { + if (state == true) { + m_speedShift.set(DoubleSolenoid.Value.kReverse); + } + if (state == false) { + m_speedShift.set(DoubleSolenoid.Value.kForward); + } + setRightMotorGains(state); + m_isSpeedShiftHigh = state; + } + + /** + * Set to open or close solenoid that cools the falcon, true = open, false = close + * @param state Chooses between open and close + */ + public void coolFalcon(boolean state) { + if (state == true) { + m_coolFalcon.set(DoubleSolenoid.Value.kForward); + } + if (state == false) { + m_coolFalcon.set(DoubleSolenoid.Value.kReverse); + } + } + + /** + * + */ + public void runFalconCooling() { + if (m_currentTimeSec % 30 == 0) { + coolFalcon(true); + SmartDashboard.putBoolean("Solenoid", true); + } else if ((m_currentTimeSec - 1) % 30 == 0) { + coolFalcon(false); + SmartDashboard.putBoolean("Solenoid", false); + } + } + + /** + * Selects a song to play! + * @param song The name of the song to be played + */ + public void selectSong(String song) { + SmartDashboard.putString("Selected Song", song); + m_orchestra.loadMusic(song); + } + + /* + * Plays Music! + */ + public void playSong() { + m_orchestra.play(); + } + + /** + * Resets the encoders for both motors. + */ + public void resetEncoders() { + m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); + + m_totalLeftDistanceInches = 0; + m_totalRightDistanceInches = 0; + } + + /** + * Resets the odometry to the specified pose. + * + * @param pose The pose to which to set the odometry. + */ + public void setOdometry(Pose2d pose) { + resetEncoders(); + m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); + } + + /** + * Resets the yaw of the pigeon + */ + public void resetGyroYaw() { + m_pigeon.setYaw(0); + m_pigeon.setAccumZAngle(0); + resetGyroAngles(); + } + + /** + * Add docs here + */ + public void resetGyroAngles() { + m_lastAngleYaw = 0; + m_currentAngleYaw = 0; + m_kinematicsTargetAngle = 0; + } + /** * Returns the current yaw of the pigeon */ @@ -609,27 +561,11 @@ public class Drive extends SubsystemBase { return ypr[2]; } - /** - * Resets the yaw of the pigeon - */ - public void resetGyroYaw() { - m_pigeon.setYaw(0); - m_pigeon.setAccumZAngle(0); - resetGyroAngles(); - } - - /** - * Add docs here - */ - public void resetGyroAngles() { - m_lastAngleYaw = 0; - m_currentAngleYaw = 0; - m_kinematicsTargetAngle = 0; - } //lol //sko //ridge //brayden=bad coder + /** * Returns the heading of the robot * @return The robot's heading in degrees, from -180 to 180 @@ -666,29 +602,6 @@ public class Drive extends SubsystemBase { -getVelocityInchesPerSecond(m_rightBackMotor)); } - /** - * Resets the encoders for both motors. - */ - public void resetEncoders() { - m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS); - - m_totalLeftDistanceInches = 0; - m_totalRightDistanceInches = 0; - } - - /** - * Resets the odometry to the specified pose. - * - * @param pose The pose to which to set the odometry. - */ - public void setOdometry(Pose2d pose) { - resetEncoders(); - m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading())); - } - /** * Gets the encoder value (position) of a motor * @param falcon The motor to get the position of @@ -750,48 +663,145 @@ public class Drive extends SubsystemBase { public double metersToInches(double meters) { return meters * DriveConstants.INCHES_PER_METER; } - - /* - * Plays Music! - */ - public void playSong() { - m_orchestra.play(); + + public void setRightMotorGains(boolean isHighGear) { + if (!isHighGear) { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); + } else { + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + + m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY); + m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS); + } } /** - * Selects a song to play! - * @param song The name of the song to be played + * Sets Motors to a NeutralMode. + * + * @param mode NeutralMode to set motors to */ - public void selectSong(String song) { - SmartDashboard.putString("Selected Song", song); - m_orchestra.loadMusic(song); - } - /** - * Set to high or low gear based on boolean state, true = high, false = low - * @param state Chooses between high or low gear - */ - public void setShiftState(boolean state) { - if (state == true) { - m_speedShift.set(DoubleSolenoid.Value.kReverse); - } - if (state == false) { - m_speedShift.set(DoubleSolenoid.Value.kForward); - } - setRightMotorGains(state); - m_isSpeedShiftHigh = state; + public void setDriveTrainNeutralMode(NeutralMode mode) { + m_leftFrontMotor.setNeutralMode(mode); + m_rightFrontMotor.setNeutralMode(mode); + m_leftBackMotor.setNeutralMode(mode); + m_rightBackMotor.setNeutralMode(mode); } - /** - * Set to open or close solenoid that cools the falcon, true = open, false = close - * @param state Chooses between open and close - */ - public void coolFalcon(boolean state) { - if (state == true) { - m_coolFalcon.set(DoubleSolenoid.Value.kForward); - } - if (state == false) { - m_coolFalcon.set(DoubleSolenoid.Value.kReverse); + public void updateSmartDashboard() { + try { + //SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw()); + //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch()); + //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); + SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); + SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); + SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); + SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); + + //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition()); + //SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0)); + + //SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + //SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor)); + SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor)); + + /*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity()); + SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity()); + */ + + SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature()); + SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature()); + + SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent()); + SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent()); + + SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent()); + SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent()); + SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent()); + SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); + + //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); + //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); + //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); + //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); + + //SmartDashboard.putString("Odometry Values Meters", getPose().toString()); + //SmartDashboard.putNumber("Odometry Heading", getHeading()); + + //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); + //SmartDashboard.putNumber("Delta Time", m_deltaTime); + + if (m_currentSong != m_songChooser.getSelected()){ + m_currentSong = m_songChooser.getSelected(); + selectSong(m_currentSong); + //System.err.println(m_currentSong); + } + } catch (Exception e) { + System.err.println("Error while using Drive SmartDashboard"); + // e.printStackTrace(System.err); } } - } From 237f35171f9a0a6aca99a4642a6496a97ba1fa62 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 23:54:10 -0700 Subject: [PATCH 22/23] Cleanup Constants --- src/main/java/frc4388/robot/Constants.java | 24 ++++++++++--------- src/main/java/frc4388/robot/Robot.java | 1 - .../java/frc4388/robot/RobotContainer.java | 1 - 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7e01b54..3678f33 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -29,6 +29,19 @@ public final class Constants { public static final int DRIVE_RIGHT_BACK_CAN_ID = 5; public static final int PIGEON_ID = 6; + /* Drive Inversions */ + public static final boolean isRightMotorInverted = false; + public static final boolean isLeftMotorInverted = false; + public static final boolean isRightArcadeInverted = false; + public static final boolean isAuxPIDInverted = false; + + /* Drive Configuration */ + public static final double OPEN_LOOP_RAMP_RATE = 0.1; + public static final double NEUTRAL_DEADBAND = 0.04; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = + new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01); + public static final int CLOSED_LOOP_TIME_MS = 1; + /* PID Constants Drive*/ public static final int DRIVE_TIMEOUT_MS = 30; public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); @@ -88,17 +101,6 @@ public final class Constants { public static final double TICKS_PER_WHEEL_REV_LOW = TICKS_PER_MOTOR_REV * MOTOR_ROT_PER_WHEEL_ROT_LOW; public static final double TICKS_PER_INCH_LOW = TICKS_PER_WHEEL_REV_LOW/INCHES_PER_WHEEL_REV; public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW; - - public static final double OPEN_LOOP_RAMP_RATE = 0.1; - public static final double NEUTRAL_DEADBAND = 0.04; - public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = - new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01); - public static final boolean isRightMotorInverted = false; - public static final boolean isLeftMotorInverted = false; - public static final boolean isRightArcadeInverted = false; - public static final boolean isAuxPIDInverted = false; - - public static final int CLOSED_LOOP_TIME_MS = 1; } public static final class IntakeConstants { diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index ec52eb2..1029754 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,7 +7,6 @@ package frc4388.robot; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import edu.wpi.first.wpilibj.TimedRobot; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3f40a5d..2156bd0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -28,7 +28,6 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.DrivePositionMPAux; import frc4388.robot.commands.DriveWithJoystick; -import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; From 50854ddd14594c012f168238337f50df2523cbba Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 23:56:57 -0700 Subject: [PATCH 23/23] Remove unused variable --- src/main/java/frc4388/robot/subsystems/Drive.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index a0e49ae..fca1e7b 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -85,7 +85,6 @@ public class Drive extends SubsystemBase { public double m_lastAngleYaw = 0; public double m_currentAngleYaw = 0; - public double m_kinematicsTargetAngle = 0; /* Smart Dashboard Objects */ SendableChooser m_songChooser = new SendableChooser(); @@ -527,7 +526,6 @@ public class Drive extends SubsystemBase { public void resetGyroAngles() { m_lastAngleYaw = 0; m_currentAngleYaw = 0; - m_kinematicsTargetAngle = 0; } /**