Merge branch 'main' of https://github.com/redshiftrobotics/crescendo-…

…2024

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

@@ -90,8 +90,7 @@ public static class ArmConstants {
 		public static final double ARM_INTAKE_DEGREES = -40;
 
 		public static final int LEFT_MOTOR_ID = 5;
-		// public static final int LEFT_ENCODER_ID = 0;
-		public static final int RIGHT_MOTOR_ID = 19;
+ 		public static final int RIGHT_MOTOR_ID = 19;
 		public static final int RIGHT_ENCODER_ID = 6;
 
 		public static final boolean ARE_MOTORS_REVERSED = false;
@@ -101,7 +100,11 @@ public static class ArmConstants {
 		public static final double ELEVATION_PID_P = 15;
 		public static final double ELEVATION_PID_I = 0;
 		public static final double ELEVATION_PID_D = 0;
+	}
 
+	public static class IntakeShooterConstants {
+		public static final boolean INTAKE_REVERSE = false;	
+		public static final boolean FLYWHEEL_REVERSE = false;	
 	}
 
 	public static class RobotMovementConstants {

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

@@ -87,6 +87,7 @@ public void teleopInit() {
 		}
 
 		robotContainer.setUpDriveController();
+		robotContainer.setUpOperatorController();
 	}
 
 	/** This function is called periodically during operator control. */

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

@@ -15,7 +15,7 @@
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
 import frc.robot.subsystems.Vision;
-import frc.robot.subsystems.arm.ArmInterface;
+import frc.robot.subsystems.arm.Arm;
 import frc.robot.subsystems.arm.DummyArm;
 import frc.robot.subsystems.arm.RealArm;
 import frc.robot.inputs.ChassisDriveInputs;
@@ -81,7 +81,7 @@ public class RobotContainer {
 	 * if ArmConstants.HAS_ARM is false, a dummy class implementing the arm's API is
 	 * created instead to prevent errors.
 	 */
-	private final ArmInterface arm = Constants.ArmConstants.HAS_ARM ? new RealArm(
+	private final Arm arm = Constants.ArmConstants.HAS_ARM ? new RealArm(
 			ArmConstants.LEFT_MOTOR_ID,
 			ArmConstants.RIGHT_MOTOR_ID,
 			ArmConstants.RIGHT_ENCODER_ID,
@@ -94,11 +94,15 @@ public class RobotContainer {
 
 	private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, VisionConstants.CAMERA_POSE);
 
+<<<<<<< HEAD
 	private final ArmRotateTo armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES);
 	private final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
 	private final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
 
 	private final LightStrip lightStrip = new LightStrip(LightConstants.LED_CONTROLLER_PWM_SLOT);
+=======
+	private final LightStrip lightStrip = new LightStrip(new AddressableLED(LightConstants.LED_CONTROLLER_PWM_SLOT));
+>>>>>>> e1694f74bed809833cdfab066afd87d0316debc3
 
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -111,6 +115,8 @@ public RobotContainer() {
 		autoChooser.addOption("Make LEDs red", new SetLightstripColor(lightStrip, 0.61));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 
+		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);
+
 		configureBindings();
 
 		setUpDriveController();
@@ -123,10 +129,7 @@ public void setUpDriveController() {
 		final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
 		final HIDType genericHIDType = genericHID.getType();
 
-		final CommandJoystick operatorJoystick = new CommandJoystick(DriverConstants.OPERATOR_JOYSTICK_PORT);
-
 		SmartDashboard.putString("Drive Controller", genericHIDType.toString());
-		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);
 
 		drivetrain.removeDefaultCommand();
 
@@ -149,12 +152,6 @@ public void setUpDriveController() {
 
 			joystick.button(3).onTrue(Commands.runOnce(inputs::toggleFieldRelative));
 
-			// This bypasses arm remote control, arm remote control is incompatible with
-			// autonomous commands
-			operatorJoystick.button(4).onTrue(armToIntake);
-			operatorJoystick.button(5).onTrue(armToAmp);
-			operatorJoystick.button(6).onTrue(armToSpeaker);
-
 			// joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
 			// joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates,
 			// drivetrain));
@@ -181,6 +178,33 @@ public void setUpDriveController() {
 		drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs));
 	}
 
+	public void setUpOperatorController() {
+		// Create joysticks
+		final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
+		final HIDType genericHIDType = genericHID.getType();
+
+		final ArmRotateTo armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES);
+		final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
+		final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
+
+		SmartDashboard.putString("Operator Controller", genericHIDType.toString());
+
+		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
+			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
+
+			joystick.button(4).onTrue(armToIntake);
+			joystick.button(5).onTrue(armToAmp);
+			joystick.button(6).onTrue(armToSpeaker);
+		} else {
+			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
+
+			xbox.rightTrigger().onTrue(armToIntake);
+			xbox.leftTrigger(5).onTrue(armToSpeaker);
+			xbox.povDown().onTrue(armToAmp);
+
+		}
+	}
+
 	/** Use this method to define your trigger->command mappings. */
 	private void configureBindings() {
 	}

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

@@ -0,0 +1,33 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.arm.Arm;
+
+public class ArmRotateBy extends Command {
+
+	private final Rotation2d dSetpoint;
+	private final Arm arm;
+
+	public ArmRotateBy(Arm arm, Rotation2d dRotation) {
+		this.dSetpoint = dRotation;
+		this.arm = arm;
+
+		addRequirements(arm);
+	}
+
+	public ArmRotateBy(Arm arm, double dDegrees) {
+		this(arm, Rotation2d.fromDegrees(dDegrees));
+	}
+
+	@Override
+	public void initialize() {
+		arm.setSetpoint(arm.getArmPosition().plus(dSetpoint));
+	}
+
+	@Override
+	public boolean isFinished() {
+		// return arm.isAtDesiredPosition();
+		return true;
+	}
+}

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

@@ -1,19 +1,23 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.arm.ArmInterface;
+import frc.robot.subsystems.arm.Arm;
 
 public class ArmRotateTo extends Command {
 
-	private final double setpoint;
-	private final ArmInterface arm;
+	private final Rotation2d setpoint;
+	private final Arm arm;
 
-	public ArmRotateTo(ArmInterface arm, double degree) {
-		this.setpoint = degree;
+	public ArmRotateTo(Arm arm, Rotation2d rotation) {
+		this.setpoint = rotation;
 		this.arm = arm;
 
 		addRequirements(arm);
+	}
 
+	public ArmRotateTo(Arm arm, double degrees) {
+		this(arm, Rotation2d.fromDegrees(degrees));
 	}
 
 	@Override

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

@@ -1,7 +1,7 @@
 package frc.robot.commands;
 
 import frc.robot.subsystems.SwerveDrivetrain;
-import frc.robot.subsystems.arm.ArmInterface;
+import frc.robot.subsystems.arm.Arm;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -25,7 +25,7 @@ public static Command rotateTestAuto(SwerveDrivetrain drivetrain, double degrees
 	}
 
 	/** Linden did this */
-	public static Command startingAuto(ArmInterface arm, SwerveDrivetrain drivetrain, boolean invertY) {
+	public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) {
 
 		// assumes start position in corner
 		double invert = 1;

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

@@ -0,0 +1,71 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import edu.wpi.first.wpilibj.motorcontrol.Talon;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.IntakeShooterConstants;
+
+/**
+ * The intake & shooter system (mounted to the end of the arm)
+ *
+ * Other proposed names for this class were:
+ * The Relocator
+ * Non-Linear Accelarator
+ * I. C. E.
+ * Throngler
+ * 
+ * @author Aceius E.
+ */
+public class IntakeShooter extends SubsystemBase {
+	/*
+	 * 2 motors for flywheel
+	 * 2 for intake
+	 * neo550s, 2 sim motors,
+	 */
+
+	private final Talon flywheel1; // AndyMark CIM, doublecheck this
+	private final Talon flywheel2;
+	private final CANSparkMax intake1;
+	private final CANSparkMax intake2; // might not exist
+
+	public IntakeShooter(int flywheel1id, int flywheel2id, int intake1id, int intake2id) {
+		this.flywheel1 = new Talon(flywheel1id);
+		this.flywheel2 = new Talon(flywheel2id);
+
+		flywheel1.setInverted(IntakeShooterConstants.FLYWHEEL_REVERSE);
+		flywheel1.setInverted(IntakeShooterConstants.FLYWHEEL_REVERSE);
+
+		this.intake1 = new CANSparkMax(intake1id, CANSparkLowLevel.MotorType.kBrushless);
+		this.intake2 = new CANSparkMax(intake2id, CANSparkLowLevel.MotorType.kBrushless);
+
+		intake1.setInverted(IntakeShooterConstants.INTAKE_REVERSE);
+		intake2.setInverted(IntakeShooterConstants.INTAKE_REVERSE);
+	}
+
+	public void startFlyWheels() {
+		flywheel1.set(1);
+		flywheel2.set(1);
+	}
+
+	public void stopFlywheels() {
+		flywheel1.stopMotor();
+		flywheel2.stopMotor();
+	}
+
+	public void intake() {
+		intake1.set(1);
+		intake1.set(1);
+	}
+
+	public void intakeReverse() {
+		intake1.set(-1);
+		intake1.set(-1);
+	}
+
+	public void stopIntake() {
+		intake1.stopMotor();
+		intake1.stopMotor();
+	}
+
+}

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

@@ -0,0 +1,35 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.ArmConstants;
+
+/**
+ * This interface represnets the arm. This interface is implemented by the
+ * RealArm (formerly known as Arm) class.
+ */
+public abstract class Arm extends SubsystemBase {
+
+	public abstract void setSetpoint(double degrees);
+
+	public void setSetpoint(Rotation2d rotation) {
+		setSetpoint(rotation.getDegrees());
+	}
+
+	public abstract boolean isAtDesiredPosition();
+
+	public abstract Rotation2d getArmPosition();
+
+
+	public void setArmToAmpPosition() {
+		setSetpoint(ArmConstants.ARM_AMP_SHOOTING_DEGREES);
+	}
+
+	public void setArmToSpeakerPosition() {
+		setSetpoint(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
+	}
+
+	public void setArmToIntakePosition() {
+		setSetpoint(ArmConstants.ARM_INTAKE_DEGREES);
+	}
+}

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

@@ -1,27 +1,27 @@
 package frc.robot.subsystems.arm;
 
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * This is a dummy stand in class that represents the arm, in the event that the
  * robot does not have an arm in order to prevent errors.
  */
-public class DummyArm extends SubsystemBase implements ArmInterface {
-	public void changeArmAngleDegreesBy(double desiredDegrees) {
-		System.out.println(desiredDegrees);
-	}
+public class DummyArm extends Arm {
 
-	public void setArmToAmpPosition() {
-	}
+	private Rotation2d armPosition = new Rotation2d();
 
-	public void setArmToSpeakerPosition() {
-	}
+	@Override
+	public void setSetpoint(double degrees) {
+		SmartDashboard.putNumber("Arm Setpoint Degrees", degrees);
+		SmartDashboard.putNumber("Arm Degrees", degrees);
 
-	public void setArmToIntakePosition() {
+		armPosition = Rotation2d.fromDegrees(degrees);
 	}
 
-	public void setSetpoint(double degree) {
-		System.out.println(degree);
+	@Override
+	public Rotation2d getArmPosition() {
+		return armPosition;
 	}
 
 	public boolean isAtDesiredPosition() {