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

Started Path 2, Made endAngle an optional param

Browse Source
This commit is contained in:
Aarav Shah
2020-02-29 15:38:59 -07:00
parent a711fe43d4
commit 48b303fdfe
4 changed files with 30 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
-1
View File
@@ -191,7 +191,6 @@ public class RobotContainer {
RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
// Run path following command, then stop at the end.
//return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return new AutoPath1FromCenter(m_robotDrive);
return new AutoPath2FromRight(m_robotDrive);
src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
+5 -5
View File
@@ -28,13 +28,13 @@ public class AutoPath1FromCenter extends SequentialCommandGroup {
//shoot pre-loaded 3 balls
new GotoCoordinates(m_drive, 75, 44, -90),
//Start Intake Ball 1
new GotoCoordinates(m_drive, 0, 12, 0),
new GotoCoordinates(m_drive, 0, 28, 0),
new GotoCoordinates(m_drive, 0, 12),
new GotoCoordinates(m_drive, 0, 28),
//Start Intake Ball 2
new GotoCoordinates(m_drive, 0, 8, 0),
new GotoCoordinates(m_drive, 0, 28, 0),
new GotoCoordinates(m_drive, 0, 8),
new GotoCoordinates(m_drive, 0, 28),
//Start Intake Ball 3
new GotoCoordinates(m_drive, 0, 8, 0),
new GotoCoordinates(m_drive, 0, 8),
new Wait(m_drive, 0, 2)
//Shoot 3 Balls
);
src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
+8 -6
View File
@@ -25,16 +25,18 @@ public class AutoPath2FromRight extends SequentialCommandGroup {
m_drive = subsystem;
addCommands( new Wait(m_drive, 0, 1),
new GotoCoordinates(m_drive, 0, 77, 0),
new GotoCoordinates(m_drive, 0, 77),
//Start Intake Ball 1
new GotoCoordinates(m_drive, 0, 8, 0),
new GotoCoordinates(m_drive, 0, 28, 0),
new GotoCoordinates(m_drive, 0, 8),
new GotoCoordinates(m_drive, 0, 28),
//Start Intake Ball 2
new GotoCoordinates(m_drive, 0, 8, 0),
new GotoCoordinates(m_drive, 0, 8),
//Shoot 5 Balls
new GotoCoordinates(m_drive, 0, 28, 0),
new GotoCoordinates(m_drive, 0, 28),
//Start Intake Ball 1 (second round)
new GotoCoordinates(m_drive, 0, 8, 0),
new GotoCoordinates(m_drive, 0, 8),
//Start Moving to 4th Ball
new GotoCoordinates(m_drive, 60, -50),
new Wait(m_drive, 0, 2)
);
}
src/main/java/frc4388/robot/commands/GotoCoordinates.java
+17 -2
View File
@@ -38,10 +38,25 @@ public class GotoCoordinates extends SequentialCommandGroup {
m_currentAngle = calcAngle();
SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle);
m_endAngle = endAngle;
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new Wait(m_drive, 0, 0),
new DriveStraightToPositionPID(m_drive, m_hypotDist),
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
}
public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) {
m_drive = subsystem;
m_xTarget = xTarget;
m_yTarget = yTarget;
m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
m_currentAngle = calcAngle();
m_endAngle = m_currentAngle;
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new Wait(m_drive, 0, 0),