From 8704cf515f2271bb11fe87a90253d8a67dcff17d Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 28 Feb 2020 21:54:46 -0700 Subject: [PATCH] Added endAngle to GotoCoordinates --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/commands/GotoCoordinates.java | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9b3b15d..81f4033 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -208,7 +208,7 @@ public class RobotContainer { //Below this would be the picking up additional balls outside of those in the trench directly behind us //new GotoCoordinates(m_robotDrive, 36, 36), - new GotoCoordinates(m_robotDrive, 36, 36, 0));//, + new GotoCoordinates(m_robotDrive, 36, 36, -90));//, //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 5ca4c75..5580cfc 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Drive; @@ -36,12 +37,15 @@ public class GotoCoordinates extends SequentialCommandGroup { m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); m_currentAngle = calcAngle(); + + SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle); + m_endAngle = endAngle; addCommands( new TurnDegrees(m_drive, m_currentAngle), new DriveStraightToPositionMM(m_drive, m_hypotDist), - new TurnDegrees(m_drive, m_endAngle - m_currentAngle - 90)); + new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); } public boolean isQuadrantThree() {