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

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/RobotContainer.java
+1 -1
View File
@@ -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 //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),
new GotoCoordinates(m_robotDrive, 36, 36, 0));//, new GotoCoordinates(m_robotDrive, 36, 36, -90));//,
//new StorageIntakeGroup(m_robotIntake, m_robotStorage), //new StorageIntakeGroup(m_robotIntake, m_robotStorage),
//new TurnDegrees(m_robotDrive, 75), //new TurnDegrees(m_robotDrive, 75),
//new DriveStraightToPositionMM(m_robotDrive, 18.0), //new DriveStraightToPositionMM(m_robotDrive, 18.0),
src/main/java/frc4388/robot/commands/GotoCoordinates.java
+5 -1
View File
@@ -7,6 +7,7 @@
package frc4388.robot.commands; package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive; 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_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
m_currentAngle = calcAngle(); m_currentAngle = calcAngle();
SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle);
m_endAngle = endAngle; m_endAngle = endAngle;
addCommands( new TurnDegrees(m_drive, m_currentAngle), addCommands( new TurnDegrees(m_drive, m_currentAngle),
new DriveStraightToPositionMM(m_drive, m_hypotDist), 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() { public boolean isQuadrantThree() {