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

GotoCoordinate Works, Trying to Implement TurnAngle

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