Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 08:48:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

Added endAngle to GotoCoordinates

Browse Source
This commit is contained in:
Aarav Shah
2020-02-28 21:54:46 -07:00
parent 237eaeb7ef
commit 8704cf515f
2 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/GotoCoordinates.java
+5 -1
View File
@@ -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() {