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

tested RunCommandForTime + DWIForTime in robotsim, both work

Browse Source
This commit is contained in:
aarav18
2022-03-22 16:56:40 -06:00
parent 8af4535191
commit 81ef2a858a
5 changed files with 32 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+17 -15
View File
@@ -72,6 +72,7 @@ import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.ShooterConstants;
import frc4388.robot.Constants.StorageConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.RunCommandForTime;
import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
import frc4388.robot.commands.ClimberCommands.RunClaw;
import frc4388.robot.commands.ClimberCommands.RunClimberPath;
@@ -226,7 +227,7 @@ public class RobotContainer {
}, m_robotClimber));
m_robotBoomBoom.setDefaultCommand(
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45))
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
);
// autoInit();
@@ -431,13 +432,6 @@ public class RobotContainer {
// }).withName("No Autonomous Path");
// }
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
// thetaController.enableContinuousInput(-Math.PI, Math.PI);
loadPath("Move Forward");
// ! this will run each of the specified PathPlanner paths in sequence.
// * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
@@ -453,14 +447,22 @@ public class RobotContainer {
// "Diamond"));
// * assume turret is already pointed towards target.
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
new ParallelRaceGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
));
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
// new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
// new ParallelRaceGroup(
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage)
// ));
// TODO: we should test TrackTarget timing with my RunCommandForTime thing at some point, same with DriveWithInput timing
// return new RunCommandForTime(new RunCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, true), m_robotSwerveDrive), 1.0, true);
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.3, 0.0, 0.0}, 1.0));//,
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new ParallelCommandGroup(
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0)
//));
// * aim with RotateUntilTarget
// return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
+5 -1
View File
@@ -43,6 +43,7 @@ public class DriveWithInputForTime extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
System.out.println("RUNNING");
elapsed = System.currentTimeMillis() - start;
this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true);
}
@@ -54,6 +55,9 @@ public class DriveWithInputForTime extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (elapsed >= duration);
System.out.println("Duration: " + duration);
System.out.println("Elapsed: " + elapsed);
return ((double) elapsed >= duration);
}
}
src/main/java/frc4388/robot/commands/RunCommandForTime.java
+3
View File
@@ -56,6 +56,7 @@ public class RunCommandForTime extends CommandBase {
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
System.out.println("RUNNING");
this.elapsed = System.currentTimeMillis() - this.start;
this.command.execute();
}
@@ -70,6 +71,8 @@ public class RunCommandForTime extends CommandBase {
@Override
public boolean isFinished() {
if (this.override) {
System.out.println("Duration: " + duration);
System.out.println("Elapsed: " + elapsed);
return (this.elapsed >= this.duration);
} else {
return (this.command.isFinished() && (this.elapsed >= this.duration));
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+6 -6
View File
@@ -174,9 +174,9 @@ public class BoomBoom extends SubsystemBase {
public void periodic() {
// This method will be called once per scheduler run
// speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0);
SmartDashboard.putNumber("Shooter Current", getCurrent());
SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage());
SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
// SmartDashboard.putNumber("Shooter Current", getCurrent());
// SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage());
// SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
}
public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
@@ -191,9 +191,9 @@ public class BoomBoom extends SubsystemBase {
public void runDrumShooter(double speed) {
// m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed + speed2);
m_shooterFalconLeft.set(speed);
SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
// SmartDashboard.putNumber("BoomBoom percent speed", speed + speed2);
// SmartDashboard.putNumber("BoomBoom current stator", m_shooterFalconLeft.getStatorCurrent());
// SmartDashboard.putNumber("BoomBoom current supply", m_shooterFalconLeft.getSupplyCurrent());
}