From 11f7f2272e7cbfbe1d7db043bd7b6ca795d0160f Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 19 Feb 2020 19:15:45 -0700 Subject: [PATCH] Fix Dead assist so it uses the right constants and TurnRate code --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 1 + .../robot/commands/DriveWithJoystickDriveStraight.java | 4 ++-- .../robot/commands/DriveWithJoystickUsingDeadAssistPID.java | 6 +----- src/main/java/frc4388/robot/subsystems/Drive.java | 6 +++++- 5 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4dcbaaf..13ce0ee 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -31,7 +31,7 @@ public final class Constants { 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.0, 0.05, 0, 0.5); + 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_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 80484b4..c0879e3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -33,6 +33,7 @@ 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; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java index 2a30287..a298f9c 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickDriveStraight.java @@ -47,7 +47,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN); + m_currentGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(1); double moveInput = -m_controller.getLeftYAxis(); double steerInput = m_controller.getRightXAxis(); double moveOutput = 0; @@ -102,7 +102,7 @@ public class DriveWithJoystickDriveStraight extends CommandBase { private void resetGyroTarget() { //m_targetGyro = m_currentGyro; m_targetGyro = m_currentGyro - + (3 * m_drive.m_rightFrontMotor.getSelectedSensorVelocity(1)); + + m_drive.getTurnRate(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 3ce828f..7756772 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -131,11 +131,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { private void resetGyroTarget() { m_targetGyro = m_currentGyro; m_targetGyro = m_currentGyro - + (3 * m_drive.m_rightFrontMotor.getSelectedSensorVelocity(1)); - - m_targetGyro = MathUtil.clamp( m_targetGyro, - m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8), - m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8)); + + m_drive.getTurnRate(); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 58c14fb..64bb463 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -318,6 +318,9 @@ public class Drive extends SubsystemBase { SmartDashboard.putString("Odometry Values Meters", getPose().toString()); SmartDashboard.putNumber("Odometry Heading", getHeading()); + SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); + //SmartDashboard.putNumber("Delta Time", m_deltaTime); + } catch (Exception e) { System.err.println("Error in the Drive Subsystem"); // e.printStackTrace(System.err); @@ -539,7 +542,8 @@ public class Drive extends SubsystemBase { */ public double getTurnRate() { double deltaYaw = m_currentAngleYaw - m_lastAngleYaw; - return deltaYaw / (m_deltaTime/1000); + double turnRate = 1000 * deltaYaw / m_deltaTime; + return turnRate; } /**