Merge pull request #10 from Tino-FRC-2473/Integration

Full robot integration

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

@@ -30,6 +30,9 @@ public final class Constants {
 	public static final class IntakeStorageConstants {
 		public static final int SPARK_INTAKE = 3;
 		public static final int SPARK_STORAGE = 8;
+
+		public static final int INTAKE_PISTON_FORWARD_PORT = 6;
+		public static final int INTAKE_PISTON_REVERSE_PORT = 1;
 	}
 
 	public static final class ShooterConstants {
@@ -81,8 +84,9 @@ public static final class DriveConstants {
 	public static final class JoystickConstants {
 		public static final int WHEEL_PORT = 0;
 		public static final int THROTTLE_PORT = 2;
+		public static final int JOYSTICK_1_PORT = 1;
 
-		public static final int BUTTON_PANEL_PORT = 1;
+		public static final int BUTTON_PANEL_PORT = 3;
 		public static final int JOYSTICK_2_PORT = 4;
 
 		public static final int BUTTON_1_PORT = 1;

src/main/java/frc/robot/Robot.java

@@ -11,9 +11,6 @@
 import edu.wpi.first.wpilibj.SerialPort.Port;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc.robot.commands.ServoControlCommand;
-import frc.robot.commands.TeleopArcadeDriveCommand;
 import frc.robot.cv.Jetson;
 
 /**

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

@@ -11,24 +11,17 @@
 import edu.wpi.first.wpilibj.Joystick;
 import frc.robot.Constants.JoystickConstants;
 
-import frc.robot.commands.LiftCommand;
-import frc.robot.commands.LiftRunDownCommand;
 import frc.robot.commands.LiftRunToEncoder;
-import frc.robot.commands.LiftRunToHeight;
-import frc.robot.commands.TestMotorCommand;
 import frc.robot.commands.WinchDriveCommand;
-import frc.robot.commands.auto.HorizontalShiftCommand;
-import frc.robot.subsystems.TestMotorSubsystem;
-import frc.robot.trajectory.*;
 import frc.robot.commands.TeleopArcadeDriveCommand;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.IntakeStorageSubsystem;
 import frc.robot.subsystems.LiftMechanism;
-import frc.robot.subsystems.ServoSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.LiftMechanism.LiftHeights;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -42,13 +35,11 @@ public class RobotContainer {
 	// public final TestMotorSubsystem testMotorSubsystem = new TestMotorSubsystem();
 	// public final TestMotorCommand testMotorCommand = new TestMotorCommand(testMotorSubsystem);
 
-	public final static IntakeStorageSubsystem intakeStorageSubsystem = new IntakeStorageSubsystem();
-	public final static ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
-	public final static LiftMechanism liftMech = new LiftMechanism();
+	private final IntakeStorageSubsystem intakeStorageSubsystem = new IntakeStorageSubsystem();
+	private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
+	private final LiftMechanism liftSubsystem = new LiftMechanism();
 	private final DriveSubsystem driveSubsystem = new DriveSubsystem();
 
-	// public final ServoSubsystem servoSubsystem = new ServoSubsystem();
-
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 * 
@@ -58,19 +49,19 @@ public class RobotContainer {
 	private JoystickButton cvButton;
 
 	private Joystick throttle;
+	private JoystickButton runShooterButton;
 
 	private Joystick buttonPanel;
 
-	private JoystickButton joystick1Trigger;
-	private JoystickButton joystick1PrimaryButton;
-	private JoystickButton joystick1_11;
-	private JoystickButton joystick1_10;
+	private JoystickButton intakeButton;
+	private JoystickButton shooterPistonButton;
+	private JoystickButton scissorPositionButton;
+	private JoystickButton runWinchButton;
 
-	private JoystickButton buttonPanel2;
-	private JoystickButton buttonPanel4;
-	private JoystickButton buttonPanel5;
-	private JoystickButton buttonPanel3; 
-	private JoystickButton buttonPanel1; 
+	private JoystickButton scissorDownDial;
+	private JoystickButton scissorLowDial;
+	private JoystickButton scissorMediumDial;
+	private JoystickButton scissorHighDial;
 
 
 	public RobotContainer() {
@@ -83,6 +74,20 @@ public DriveSubsystem getDriveSubsystem() {
 		return driveSubsystem;
 	}
 
+	public IntakeStorageSubsystem getIntakeStorageSubsystem() {
+		return intakeStorageSubsystem;
+	}
+
+	public ShooterSubsystem getShooterSubsystem() {
+		return shooterSubsystem;
+	}
+
+	public LiftMechanism getLiftSubsystem() {
+		return liftSubsystem;
+	}
+
+
+
 	public Joystick getWheel() {
 		return wheel;
 	}
@@ -99,63 +104,52 @@ public Joystick getButtonPanel() {
 		return buttonPanel;
 	}
 
-	public JoystickButton getButtonPanel2() {
-		return buttonPanel2;
-	}
-
-	public JoystickButton getButtonPanel4() {
-		return buttonPanel4;
-	}
-
-	public JoystickButton getButtonPanel6() {
-		return buttonPanel6;
-	}
-
 	/**
 	 * Use this method to define your button->command mappings. Buttons can be
 	 * created by instantiating a {@link GenericHID} or one of its subclasses
 	 * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
 	 * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
 	 */
 	private void configureButtonBindings() {
-
-		joystick1 = new Joystick(JoystickConstants.JOYSTICK_1_PORT);
-		joystick1Trigger = new JoystickButton(joystick1, 1);
-		joystick1PrimaryButton = new JoystickButton(joystick1, 3);
-		joystick1_10 = new JoystickButton(joystick1, 10);
-		joystick1_11 = new JoystickButton(joystick1, 11);
-
-		joystick1Trigger.whenPressed(new InstantCommand(() -> shooterSubsystem.runShooter(0.6)));
-		joystick1Trigger.whenReleased(new InstantCommand(() -> shooterSubsystem.runShooter(0)));
-
-		joystick1PrimaryButton.whenPressed(new InstantCommand(() -> intakeStorageSubsystem.runIntakeMotor(0.7)));
-		joystick1PrimaryButton.whenReleased(new InstantCommand(() -> intakeStorageSubsystem.runIntakeMotor(0)));
-
-		joystick1_10.whenPressed(new InstantCommand(() -> intakeStorageSubsystem.runStorageMotor(0)));
-		joystick1_11.whenPressed(new InstantCommand(() -> intakeStorageSubsystem.runStorageMotor(0.5)));
 
 		wheel = new Joystick(JoystickConstants.WHEEL_PORT);
 		cvButton = new JoystickButton(wheel, 6);
 
 		throttle = new Joystick(JoystickConstants.THROTTLE_PORT);
+		runShooterButton = new JoystickButton(throttle, 7);
 
 		buttonPanel = new Joystick(JoystickConstants.BUTTON_PANEL_PORT);
-		buttonPanel2 = new JoystickButton(buttonPanel, 2);
-		buttonPanel4 = new JoystickButton(buttonPanel, 4);
-		buttonPanel5 = new JoystickButton(buttonPanel, 5);
-		buttonPanel3 = new JoystickButton(buttonPanel, 3); 
-		buttonPanel1 = new JoystickButton(buttonPanel, 1); 
-		buttonPanel6 = new JoystickButton(buttonPanel, 6);
-
-		//-108.76 ticks -> 4ft 3 inches (with -15)
-		//-229.581146 ticks -> 5ft 3 inches (with -15)
-		//-533.91 ticks -> 6ft 7 inches (with -15)
-
-		buttonPanel2.whenPressed(new LiftCommand(liftMech,-229.581146));
-		buttonPanel4.whenPressed(new LiftCommand(liftMech, -108.76));//-229.581146
-		buttonPanel5.whenPressed(new LiftCommand(liftMech, -533.91));
-		buttonPanel3.whileHeld(new WinchDriveCommand(liftMech,0.5)); 
-		buttonPanel1.whenPressed(new LiftRunDownCommand(liftMech, 0.1)); //runDown power must be positive
+
+		intakeButton = new JoystickButton(buttonPanel, 2);
+		shooterPistonButton = new JoystickButton(buttonPanel, 4);
+		scissorPositionButton = new JoystickButton(buttonPanel, 6);
+		runWinchButton = new JoystickButton(buttonPanel, 8);
+
+		runShooterButton.whenPressed(new InstantCommand(() -> shooterSubsystem.runShooter(0.6)));
+		runShooterButton.whenReleased(new InstantCommand(() -> shooterSubsystem.runShooter(0)));
+
+		intakeButton.whenPressed(new InstantCommand(() -> intakeStorageSubsystem.deployIntake(0.7)));
+		intakeButton.whenReleased(new InstantCommand(() -> intakeStorageSubsystem.retractIntake()));
+
+		shooterPistonButton.whenPressed(new InstantCommand(() -> shooterSubsystem.launchBallWithPiston()));
+
+		scissorPositionButton.whenPressed(new LiftRunToEncoder(liftSubsystem, getDialHeight().getValue(), 0.5));
+
+		runWinchButton.whileHeld(new WinchDriveCommand(liftSubsystem, 0.5));
+
+
+		scissorDownDial = new JoystickButton(buttonPanel, 1);
+		scissorLowDial = new JoystickButton(buttonPanel, 3);
+		scissorMediumDial = new JoystickButton(buttonPanel, 5);
+		scissorHighDial = new JoystickButton(buttonPanel, 7);
+	}
+
+	public LiftHeights getDialHeight() {
+		if (scissorDownDial.get()) return LiftHeights.DOWN;
+		if (scissorLowDial.get()) return LiftHeights.LOW;
+		if (scissorMediumDial.get()) return LiftHeights.MEDIUM;
+		if (scissorHighDial.get()) return LiftHeights.HIGH;
+		return null;
 	}
 
 	/**
@@ -167,8 +161,4 @@ public Command getAutonomousCommand() {
 		driveSubsystem.resetPose();
 		return null;
 	}
-
-	public Joystick getButtonPanel() {
-		return buttonPanel;
-	}
 }

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

@@ -1,9 +1,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.LiftMechanism;
-import frc.robot.subsystems.TestMotorSubsystem;
 
 public class LiftCommand extends SequentialCommandGroup {
 

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

@@ -7,10 +7,7 @@
 
 package frc.robot.commands;
 
-import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
-
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.Robot;
 import frc.robot.subsystems.LiftMechanism;
 
 public class LiftRunDownCommand extends CommandBase {
@@ -29,9 +26,9 @@ public LiftRunDownCommand(LiftMechanism liftMech, double power) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-       
-    liftMech.setPower(power); 
-      
+    if (power > 0) {
+      liftMech.setPower(power); 
+    }
   }
 
   // Called every time the scheduler runs while the command is scheduled.

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

@@ -8,57 +8,60 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.Robot;
 import frc.robot.subsystems.LiftMechanism;
 
 public class LiftRunToEncoder extends CommandBase {
 
-    LiftMechanism liftMech;
-    /**
-     * Creates a new Teleop.
-     */
-
-    public double toEncoder;
-    public double power;
+	LiftMechanism liftMech;
 
-    public LiftRunToEncoder(LiftMechanism liftMech, double toEncoder, double power) {
-      addRequirements();
-        this.liftMech = liftMech; 
-        this.toEncoder = toEncoder; 
-        this.power = power; 
-    }
+	public double toEncoder;
+	public double power;
 
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-
-  }
+	public LiftRunToEncoder(LiftMechanism liftMech, double toEncoder, double power) {
+		addRequirements();
+		this.liftMech = liftMech;
+		this.toEncoder = toEncoder;
+		power = Math.abs(power);
+		if (liftMech.getLiftMotor().getEncoder().getPosition() >= toEncoder) {
+			this.power = -power;
+		} else {
+			this.power = power;
+		}
 
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    if (liftMech.getLiftMotor().getEncoder().getPosition() > toEncoder){ //-108.5
-      liftMech.setPower(power);
-     } else {
-     liftMech.setPower(0);
-    }
-
-      System.out.println(liftMech.getLiftMotor().getEncoder().getPosition());
-
+	}
 
+	// Called when the command is initially scheduled.
+	@Override
+	public void initialize() {
+		liftMech.setPower(power);
 
+	}
 
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-      liftMech.setPower(0);
-  }
+	// Called every time the scheduler runs while the command is scheduled.
+	@Override
+	public void execute() {
+		// if (liftMech.getLiftMotor().getEncoder().getPosition() > toEncoder){ //-108.5
+		// liftMech.setPower(power);
+		// } else {
+		// liftMech.setPower(0);
+		// }
+		System.out.println(liftMech.getLiftMotor().getEncoder().getPosition());
 
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return liftMech.getLiftMotor().getEncoder().getPosition() < toEncoder;
-  }
+	}
+
+	// Called once the command ends or is interrupted.
+	@Override
+	public void end(boolean interrupted) {
+		liftMech.setPower(0);
+	}
+
+	// Returns true when the command should end.
+	@Override
+	public boolean isFinished() {
+		if (power > 0) {
+			return liftMech.getLiftMotor().getEncoder().getPosition() > toEncoder;
+
+		}
+		return liftMech.getLiftMotor().getEncoder().getPosition() < toEncoder;
+	}
 }

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

@@ -7,10 +7,7 @@
 
 package frc.robot.commands;
 
-import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
-
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.Robot;
 import frc.robot.subsystems.LiftMechanism;
 
 public class LiftRunToHeight extends CommandBase {