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 1/9] 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 2/9] 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 3/9] 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 4/9] 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 5/9] 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 6/9] 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 7/9] 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 0541a2463c4197fca8f707c3397e5fd6a9a3b9af Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 17:47:47 -0700 Subject: [PATCH 8/9] 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 322e4c7a44e05d2f515f554ae8ac4947ef6a6b0f Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Tue, 25 Feb 2020 21:33:12 -0700 Subject: [PATCH 9/9] 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); }