From 237eaeb7efcaced6e48cd3478f3c0a29fdca9a17 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 28 Feb 2020 21:25:09 -0700 Subject: [PATCH] GotoCoordinate Works, Trying to Implement TurnAngle --- src/main/java/frc4388/robot/RobotContainer.java | 10 +++++----- .../java/frc4388/robot/commands/GotoCoordinates.java | 10 ++++++---- src/main/java/frc4388/robot/subsystems/Drive.java | 4 ++-- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 65f8912..9b3b15d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -125,7 +125,7 @@ public class RobotContainer { // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new GotoCoordinates(m_robotDrive, 12, 40)); + .whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0)); // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) @@ -192,10 +192,10 @@ public class RobotContainer { //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those //This assumes that we are positioned against the right wall with our shooter facing the target. - return new SequentialCommandGroup(new Wait(2, m_robotDrive), + return new SequentialCommandGroup(new Wait(0, m_robotDrive), //add aim command //add shooter command - new DriveStraightToPositionMM(m_robotDrive, 48.0), + //new DriveStraightToPositionMM(m_robotDrive, 48.0), /*new ParallelCommandGroup( new StorageIntakeGroup(m_robotIntake, m_robotStorage), new DriveStraightToPositionMM(m_robotDrive, 36.0)), @@ -207,8 +207,8 @@ public class RobotContainer { //add shooter command //Below this would be the picking up additional balls outside of those in the trench directly behind us - new TurnDegrees(m_robotDrive, -90), - new DriveStraightToPositionMM(m_robotDrive, 96.0));//, + //new GotoCoordinates(m_robotDrive, 36, 36), + new GotoCoordinates(m_robotDrive, 36, 36, 0));//, //new StorageIntakeGroup(m_robotIntake, m_robotStorage), //new TurnDegrees(m_robotDrive, 75), //new DriveStraightToPositionMM(m_robotDrive, 18.0), diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java index e836820..5ca4c75 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java @@ -20,13 +20,12 @@ public class GotoCoordinates extends SequentialCommandGroup { double m_yTarget; double m_currentAngle; double m_hypotDist; - - double m_lastAngle; + double m_endAngle; /** * Creates a new GotoPosition. */ - public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) { + public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); m_drive = subsystem; @@ -37,9 +36,12 @@ public class GotoCoordinates extends SequentialCommandGroup { m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); m_currentAngle = calcAngle(); + m_endAngle = endAngle; - addCommands(new TurnDegrees(m_drive, m_currentAngle), new DriveStraightToPositionMM(m_drive, m_hypotDist)); + addCommands( new TurnDegrees(m_drive, m_currentAngle), + new DriveStraightToPositionMM(m_drive, m_hypotDist), + new TurnDegrees(m_drive, m_endAngle - m_currentAngle - 90)); } public boolean isQuadrantThree() { diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3380fd0..8c2e2bd 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -727,8 +727,8 @@ 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(); + double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); SmartDashboard.putNumber("Left Motor RPM", leftRPM); SmartDashboard.putNumber("Right Motor RPM", rightRPM);