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

Made both auto commands groups, and added param to wait

Browse Source
This commit is contained in:
Aarav Shah
2020-02-29 14:55:17 -07:00
parent 6bdeaefc7c
commit a711fe43d4
5 changed files with 102 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+6 -18
View File
@@ -28,6 +28,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.AutoPath1FromCenter;
import frc4388.robot.commands.AutoPath2FromRight;
import frc4388.robot.commands.DrivePositionMPAux;
import frc4388.robot.commands.DriveStraightAtVelocityPID;
import frc4388.robot.commands.DriveStraightToPositionMM;
@@ -117,7 +119,7 @@ public class RobotContainer {
/* Test Buttons */
// A driver test button
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 240));
.whenPressed(new DriveStraightToPositionMM(m_robotDrive, 24));
// B driver test button
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
@@ -125,7 +127,7 @@ public class RobotContainer {
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new Wait(m_robotDrive, 0));
.whenPressed(new Wait(m_robotDrive, 0, 0));
// X driver test button
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
@@ -190,23 +192,9 @@ public class RobotContainer {
// Run path following command, then stop at the end.
//return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
//This assumes that we are positioned against parallel to the initiation line with our back bumper lined up with the center target
return new SequentialCommandGroup(new Wait(m_robotDrive, 0),
//shoot pre-loaded 3 balls
new GotoCoordinates(m_robotDrive, 75, 44, -90),
//Start Intake Ball 1
new GotoCoordinates(m_robotDrive, 0, 12, 0),
new GotoCoordinates(m_robotDrive, 0, 28, 0),
//Start Intake Ball 2
new GotoCoordinates(m_robotDrive, 0, 8, 0),
new GotoCoordinates(m_robotDrive, 0, 28, 0),
//Start Intake Ball 3
new GotoCoordinates(m_robotDrive, 0, 8, 0)
/*Shoot 3 Balls*/ );
//return new AutoPath1FromCenter(m_robotDrive);
return new AutoPath2FromRight(m_robotDrive);
/*new GotoCoordinates(m_robotDrive, 0, 68.75, 0),*/
//new StorageIntakeGroup(m_robotIntake, m_robotStorage),
}
TrajectoryConfig getTrajectoryConfig() {
return new TrajectoryConfig(
src/main/java/frc4388/robot/commands/AutoPath1FromCenter.java
+42
View File
@@ -0,0 +1,42 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class AutoPath1FromCenter extends SequentialCommandGroup {
Drive m_drive;
/**
* Creates a new AutoPath1FromCenter.
*/
public AutoPath1FromCenter(Drive subsystem) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
m_drive = subsystem;
addCommands( new Wait(m_drive, 0, 1),
//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),
//Start Intake Ball 2
new GotoCoordinates(m_drive, 0, 8, 0),
new GotoCoordinates(m_drive, 0, 28, 0),
//Start Intake Ball 3
new GotoCoordinates(m_drive, 0, 8, 0),
new Wait(m_drive, 0, 2)
//Shoot 3 Balls
);
}
}
src/main/java/frc4388/robot/commands/AutoPath2FromRight.java
+41
View File
@@ -0,0 +1,41 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.subsystems.Drive;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class AutoPath2FromRight extends SequentialCommandGroup {
Drive m_drive;
/**
* Creates a new AutoPath2FromRight.
*/
public AutoPath2FromRight(Drive subsystem) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
m_drive = subsystem;
addCommands( new Wait(m_drive, 0, 1),
new GotoCoordinates(m_drive, 0, 77, 0),
//Start Intake Ball 1
new GotoCoordinates(m_drive, 0, 8, 0),
new GotoCoordinates(m_drive, 0, 28, 0),
//Start Intake Ball 2
new GotoCoordinates(m_drive, 0, 8, 0),
//Shoot 5 Balls
new GotoCoordinates(m_drive, 0, 28, 0),
//Start Intake Ball 1 (second round)
new GotoCoordinates(m_drive, 0, 8, 0),
new Wait(m_drive, 0, 2)
);
}
}
src/main/java/frc4388/robot/commands/GotoCoordinates.java
+1 -1
View File
@@ -44,7 +44,7 @@ public class GotoCoordinates extends SequentialCommandGroup {
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new Wait(m_drive, 0.1),
new Wait(m_drive, 0, 0),
new DriveStraightToPositionPID(m_drive, m_hypotDist),
new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
}
src/main/java/frc4388/robot/commands/Wait.java
+12 -1
View File
@@ -17,15 +17,19 @@ public class Wait extends CommandBase {
long m_waitTime;
long m_currentTime;
SubsystemBase m_subsystem;
int m_waitNum;
int counter = 0;
/**
* Creates a new WaitCommand.
*/
public Wait(SubsystemBase subsystem, double seconds) {
public Wait(SubsystemBase subsystem, double seconds, int waitNum) {
// Use addRequirements() here to declare subsystem dependencies.
m_waitTime = (long) (seconds * 1000);
m_subsystem = subsystem;
m_waitNum = waitNum;
addRequirements(m_subsystem);
}
@@ -40,8 +44,15 @@ public class Wait extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (counter == 0) {
SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime);
}
m_currentTime = System.currentTimeMillis();
SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
counter ++;
}
// Called once the command ends or is interrupted.