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] 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); } /**