From a70983a4c2215dc1879218b8b1fd9944f97bf633 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 28 Feb 2020 20:48:22 -0700 Subject: [PATCH] Removed Output Limits, Added GotoCoordinates Command --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 5 +- .../robot/commands/DriveWithJoystick.java | 25 ++++---- .../robot/commands/GotoCoordinates.java | 61 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 7 +++ 5 files changed, 86 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/GotoCoordinates.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5054477..dc283e4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -40,7 +40,7 @@ public final class Constants { public static final double OPEN_LOOP_RAMP_RATE = 0.1; // Seconds from 0 to full power on motor public static final double NEUTRAL_DEADBAND = 0.04; public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = - new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01); + new SupplyCurrentLimitConfiguration(false, 60, 40, 2); public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops /* Drive Train Characteristics */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 20518cc..65f8912 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.DriveStraightAtVelocityPID; import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; +import frc4388.robot.commands.GotoCoordinates; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; @@ -116,7 +117,7 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, -192)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) @@ -124,7 +125,7 @@ public class RobotContainer { // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new DrivePositionMPAux(m_robotDrive, 12.0, 3.0, 1, 36.0)); + .whenPressed(new GotoCoordinates(m_robotDrive, 12, 40)); // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 1802f22..54bc129 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -60,19 +60,22 @@ public class DriveWithJoystick extends CommandBase { } double tempOutputLimit = 0.8; - if (moveOutput > tempOutputLimit) { - moveOutput = tempOutputLimit; - } else if(moveOutput < -tempOutputLimit) { - moveOutput = -tempOutputLimit; - } + boolean isOutputLimited = false; - if (steerOutput > tempOutputLimit) { - steerOutput = tempOutputLimit; - } else if(steerOutput < -tempOutputLimit) { - steerOutput = -tempOutputLimit; - } + if (isOutputLimited) { + if (moveOutput > tempOutputLimit) { + moveOutput = tempOutputLimit; + } else if(moveOutput < -tempOutputLimit) { + moveOutput = -tempOutputLimit; + } - SmartDashboard.putNumber("Steer Output Test", moveOutput); + if (steerOutput > tempOutputLimit) { + steerOutput = tempOutputLimit; + } else if(steerOutput < -tempOutputLimit) { + steerOutput = -tempOutputLimit; + } + } + m_drive.driveWithInput(moveOutput, steerOutput); } diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java new file mode 100644 index 0000000..e836820 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.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.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class GotoCoordinates extends SequentialCommandGroup { + Drive m_drive; + + double m_xTarget; + double m_yTarget; + double m_currentAngle; + double m_hypotDist; + + double m_lastAngle; + + /** + * Creates a new GotoPosition. + */ + public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + m_drive = subsystem; + + m_xTarget = xTarget; + m_yTarget = yTarget; + + m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); + + m_currentAngle = calcAngle(); + + + addCommands(new TurnDegrees(m_drive, m_currentAngle), new DriveStraightToPositionMM(m_drive, m_hypotDist)); + } + + public boolean isQuadrantThree() { + if ((m_xTarget < 0) && (m_yTarget < 0)) { + return true; + } else { + return false; + } + } + + public double calcAngle() { + if (isQuadrantThree()) { + return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90; + } else { + return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90; + } + } + +} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index f4541dc..3380fd0 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -88,6 +88,7 @@ public class Drive extends SubsystemBase { public double m_lastAngleYaw = 0; public double m_currentAngleYaw = 0; + public double m_lastAngleGotoCoordinates; /* Smart Dashboard Objects */ SendableChooser m_songChooser = new SendableChooser(); @@ -726,6 +727,12 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); + double leftRPM = m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity(); + double rightRPM = m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity(); + + SmartDashboard.putNumber("Left Motor RPM", leftRPM); + SmartDashboard.putNumber("Right Motor RPM", rightRPM); + //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());