Working 2 note auto

src/main/java/frc/robot/Constants.java

@@ -183,13 +183,13 @@ public static class RobotMovementConstants {
 		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(3);
 		public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(1);
 
-		public static final double ROTATION_PID_P = 5.0;
-		public static final double ROTATION_PID_I = 0.0;
-		public static final double ROTATION_PID_D = 0.0;
+		public static final double ROTATION_PID_P = 0.21;
+		public static final double ROTATION_PID_I = 0.001;
+		public static final double ROTATION_PID_D = 0;
 
-		public static final double TRANSLATION_PID_P = 0.5;
+		public static final double TRANSLATION_PID_P = 0.25;
 		public static final double TRANSLATION_PID_I = 0;
-		public static final double TRANSLATION_PID_D = 0.01;
+		public static final double TRANSLATION_PID_D = 0;
 		public static final double MAX_TRANSLATION_SPEED = 1.0;
 	}
 

src/main/java/frc/robot/RobotContainer.java

@@ -12,7 +12,9 @@
 import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.commands.HangControl;
 import frc.robot.commands.ArmRotateTo;
+import frc.robot.commands.AutoRotateTo;
 import frc.robot.commands.SetLightstripColorFor;
+import frc.robot.commands.SpinFlywheelShooter;
 import frc.robot.subsystems.ChassisDriveInputs;
 import frc.robot.subsystems.LightStrip;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -34,6 +36,7 @@
 import edu.wpi.first.wpilibj.GenericHID.HIDType;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -132,10 +135,14 @@ public class RobotContainer {
 	 */
 	public RobotContainer() {
 		autoChooser.setDefaultOption("Forward", Autos.startingAuto(drivetrain, arm, leftHang, rightHang));
-		autoChooser.addOption("1+Forward", Autos.shootStartingAuto(drivetrain, arm, intakeShooter, leftHang, rightHang));
-		autoChooser.addOption("2+Forward", Autos.shoot2StartingAuto(drivetrain, arm, intakeShooter, leftHang, rightHang));
+		autoChooser.addOption("1+Forward",
+				Autos.shootStartingAuto(drivetrain, arm, intakeShooter, leftHang, rightHang));
+		autoChooser.addOption("2+Forward",
+				Autos.shoot2StartingAuto(drivetrain, arm, intakeShooter, leftHang, rightHang));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 
+		SmartDashboard.putData("ArmUp", new ArmRotateTo(arm, ArmConstants.ARM_START_DEGREES));
+
 		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);
 
 		configureBindings();
@@ -217,8 +224,8 @@ public void setUpDriveController() {
 
 			xbox.y().onTrue(Commands.runOnce(inputs::toggleFieldRelative));
 
-			xbox.leftBumper().onTrue(coopLightSignal);
-			xbox.rightBumper().onTrue(amplifyLightSignal);
+			xbox.rightBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(0), true));
+			xbox.leftBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), true));
 
 			if (vision != null)
 				xbox.x().onTrue(Commands.runOnce(vision::toggleUsing, vision));
@@ -278,9 +285,13 @@ public void setUpOperatorController() {
 			xbox.leftTrigger().onTrue(Autos.intakeFromFloorStart(arm, intakeShooter));
 			xbox.leftTrigger().onFalse(Autos.intakeFromFloorEnd(arm, intakeShooter));
 
+			xbox.leftBumper().onTrue(Autos.dropInAmp(drivetrain, arm, intakeShooter, vision, inputs));
+
 			xbox.rightTrigger().onTrue(Autos.shootInSpeaker(drivetrain, arm, intakeShooter, vision, inputs));
 
-			xbox.rightBumper().onTrue(Autos.dropInAmp(drivetrain, arm, intakeShooter, vision, inputs));
+			xbox.rightBumper().onTrue(
+					new SpinFlywheelShooter(intakeShooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER));
+			xbox.rightBumper().onFalse(new SpinFlywheelShooter(intakeShooter, 0));
 
 			xbox.y().onTrue(stowArm);
 			xbox.a().onTrue(stowArm2);

src/main/java/frc/robot/commands/AutoDriveTo.java

@@ -64,12 +64,12 @@ public void execute() {
 		double xSpeed = xMovePID.calculate(targetX, goalX);
 		double ySpeed = yMovePID.calculate(targetY, goalY);
 
-		SmartDashboard.putNumber("Current X", targetX);
-		SmartDashboard.putNumber("Current Y", targetY);
-		SmartDashboard.putNumber("X Goal", goalX);
-		SmartDashboard.putNumber("Y Goal", goalY);
+		// SmartDashboard.putNumber("Current X", targetX);
+		// SmartDashboard.putNumber("Current Y", targetY);
+		// SmartDashboard.putNumber("X Goal", goalX);
+		// SmartDashboard.putNumber("Y Goal", goalY);
 
-		double speedLimit = RobotMovementConstants.MAX_TRANSLATION_SPEED;
+		double speedLimit = 0.5;
 		double maxSpeed = Math.max(Math.abs(xSpeed), Math.abs(ySpeed));
 
 		if (maxSpeed > speedLimit) {

src/main/java/frc/robot/commands/AutoRotateTo.java

@@ -57,7 +57,7 @@ public void execute() {
 		double turnsSeed = rotatePID.calculate(currentAngle, this.currentAngleGoal);
 		// SmartDashboard.putNumber("Turn Speed Auto", turnsSeed);
 
-		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
+		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, (turnsSeed)));
 
 		drivetrain.updateSmartDashboard();
 	}

src/main/java/frc/robot/commands/Autos.java

@@ -84,6 +84,7 @@ public static Command shoot2StartingAuto(SwerveDrivetrain drivetrain, Arm arm, I
 								new AutoDriveTo(drivetrain, new Translation2d(driveDistanceForNote1, 0))),
 						new WaitCommand(0.25),
 						intakeFromFloorEnd(arm, shooter),
+						new AutoRotateTo(drivetrain, new Rotation2d(0)),
 						Commands.race(
 								Commands.waitSeconds(3),
 								new AutoDriveTo(drivetrain, new Translation2d(-driveDistanceForNote1, 0))),
@@ -116,7 +117,7 @@ public static Command dropInAmp(SwerveDrivetrain drivetrain, Arm arm, IntakeShoo
 		return Commands.sequence(
 				new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES).alongWith(
 						new SpinFlywheelShooter(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_AMP),
-						new WaitCommand(1)),
+						new WaitCommand(0.5)),
 				new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_AMP),
 				new WaitCommand(0.2),
 				new SpinFlywheelShooter(shooter, 0),
@@ -145,14 +146,14 @@ public static Command shootInSpeaker(SwerveDrivetrain drivetrain, Arm arm, Intak
 		}
 
 		return Commands.sequence(
-				Commands.sequence(
-						new SpinFlywheelShooter(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER),
-						new WaitCommand(0.5)),
-				new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES),
-				new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_SPEAKER),
-				new WaitCommand(0.2),
-				new SpinFlywheelShooter(shooter, 0),
-				new SpinIntakeGrabbers(shooter, 0));
+			Commands.parallel(
+				new SpinFlywheelShooterForTime(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER, 0.4),
+				new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES)
+			),
+			new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_SPEAKER),
+			new WaitCommand(0.2),
+			new SpinFlywheelShooter(shooter, 0),
+			new SpinIntakeGrabbers(shooter, 0));
 	}
 
 	public static Command intakeFromFloorStart(Arm arm, IntakeShooter shooter) {

src/main/java/frc/robot/commands/SpinFlywheelShooterForTime.java

@@ -0,0 +1,29 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.IntakeShooter;
+
+public class SpinFlywheelShooterForTime extends Command {
+	private final IntakeShooter intakeShooter;
+	private final double speed;
+	private final double millisToWait;
+
+	public SpinFlywheelShooterForTime(IntakeShooter intakeShooter, double speed, double seconds) {
+		this.intakeShooter = intakeShooter;
+		this.speed = speed;
+		this.millisToWait = Units.secondsToMilliseconds(seconds);
+
+		addRequirements(intakeShooter);
+	}
+
+	@Override
+	public void initialize() {
+		intakeShooter.setFlyWheelShooterSpeed(speed);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return intakeShooter.howLongAtSpeedMillis(speed) > millisToWait;
+	}
+}

src/main/java/frc/robot/subsystems/arm/Arm.java

@@ -30,7 +30,7 @@ public void setArmToIntakePosition() {
 	}
 
 	public void setArmToStartPosition() {
-		setSetpoint(ArmConstants.ARM_START_DEGREES);
+		setSetpoint(ArmConstants.ARM_STOW_2_DEGREES);
 	}
 
 	public void setArmToDrivePosition() {

src/main/java/frc/robot/subsystems/intake/DummyShooter.java

@@ -13,6 +13,11 @@ public void setFlyWheelShooterSpeed(double speed) {
 	public void setIntakeGrabberSpeed(double speed) {
 	}
 
+	@Override
+	public double howLongAtSpeedMillis(double speed) {
+		return 0;
+	}
+
 	@Override
 	public boolean hasNoteInIntake() {
 		return false;

src/main/java/frc/robot/subsystems/intake/IntakeShooter.java

@@ -6,9 +6,11 @@ public abstract class IntakeShooter extends SubsystemBase {
 	public abstract void setFlyWheelShooterSpeed(double speed);
 
 	public abstract void setIntakeGrabberSpeed(double speed);
-	
+
 	public abstract boolean hasNoteInIntake();
 
+	public abstract double howLongAtSpeedMillis(double speed);
+
 	public abstract void eject();
 
 	public abstract void stop();

src/main/java/frc/robot/subsystems/intake/RealShooter.java

@@ -32,6 +32,9 @@ public class RealShooter extends IntakeShooter {
 
 	// private final DigitalInput intakeSwitch;
 
+	private double timeFlywheelSet;
+	private double flywheelSpeed;
+
 	public RealShooter(int flywheel1Id, int flywheel2Id, int intakeID, int intakeLimitSwitchId) {
 		this.flywheel1 = new WPI_VictorSPX(flywheel1Id);
 		this.flywheel2 = new WPI_VictorSPX(flywheel2Id);
@@ -45,6 +48,8 @@ public RealShooter(int flywheel1Id, int flywheel2Id, int intakeID, int intakeLim
 	@Override
 	public void setFlyWheelShooterSpeed(double speed) {
 		SmartDashboard.putNumber("FlywheelShooter", speed);
+		flywheelSpeed = speed;
+		timeFlywheelSet = System.currentTimeMillis();
 
 		flywheel1.set(-speed);
 		flywheel2.set(-speed);
@@ -78,4 +83,11 @@ public void stop() {
 		flywheel2.stopMotor();
 		intake.stopMotor();
 	}
+
+	@Override
+	public double howLongAtSpeedMillis(double speed) {
+		if (flywheelSpeed != speed)
+			return -1;
+		return System.currentTimeMillis() - timeFlywheelSet;
+	}
 }