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() {