diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index dd5bd18..bc06117 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -120,7 +120,7 @@ public static final class Mod0 { public static final int driveMotorID = 2; public static final int angleMotorID = 3; public static final int canCoderID = 0; - public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(223); //43 + public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(0); //43 public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, desiredAngle); } @@ -130,7 +130,7 @@ public static final class Mod1 { public static final int driveMotorID = 8; public static final int angleMotorID = 1; public static final int canCoderID = 1; - public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(344); //344 + public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(0); //344 public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, desiredAngle); } @@ -140,7 +140,7 @@ public static final class Mod2 { public static final int driveMotorID = 4; public static final int angleMotorID = 5; public static final int canCoderID = 2; - public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(87); + public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(0); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, desiredAngle); } @@ -150,7 +150,7 @@ public static final class Mod3 { public static final int driveMotorID = 6; public static final int angleMotorID = 7; public static final int canCoderID = 3; - public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(73.5); + public static final Rotation2d desiredAngle = Rotation2d.fromDegrees(0); public static final SwerveModuleConstants constants = new SwerveModuleConstants(driveMotorID, angleMotorID, canCoderID, desiredAngle); } @@ -199,6 +199,7 @@ public static class IntakeConstants{ public static final double intakeUpSpeed = 0.1; public static final double intakeDownSpeed = -0.1; public static final double rollerSpeed = 0.1; + public static final int IRport = 1; } // Shooter constants diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b76cb06..bcce282 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -115,6 +115,8 @@ private void configureButtonBindings() { ControlMap.m_gunnerController.x().whileTrue(m_moveRollers); ControlMap.m_gunnerController.y().whileTrue(m_shootSpeaker); ControlMap.m_gunnerController.leftTrigger().whileTrue(m_shootAmp); + + // FOR TESTING, REMOVE FOR COMP ControlMap.m_gunnerController.leftBumper().whileTrue(m_shootLeftMotor); ControlMap.m_gunnerController.rightBumper().whileTrue(m_shootRightMotor); } diff --git a/src/main/java/frc/robot/commands/IntakeCommands/MoveRollers.java b/src/main/java/frc/robot/commands/IntakeCommands/MoveRollers.java index 8f45061..67101ee 100644 --- a/src/main/java/frc/robot/commands/IntakeCommands/MoveRollers.java +++ b/src/main/java/frc/robot/commands/IntakeCommands/MoveRollers.java @@ -36,6 +36,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_Intake.getIRSensor(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 1b4329a..f14bd43 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -4,6 +4,7 @@ package frc.robot.subsystems; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.ctre.phoenix.motorcontrol.TalonSRXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; @@ -18,6 +19,7 @@ public class Intake extends SubsystemBase { private static final double DEADZONE = 0.05; private CANSparkMax m_IntakePivotMotor; private TalonSRX m_IntakeRollerMotor; + private DigitalInput IRSensor = new DigitalInput(Constants.IntakeConstants.IRport); static Intake instance = null; /** Creates a new Intake. */ public Intake() { @@ -51,6 +53,10 @@ public void moveRollers(double rotate){ m_IntakeRollerMotor.set(TalonSRXControlMode.PercentOutput, rotate); } + public boolean getIRSensor(){ + return IRSensor.get(); + } + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index f2b6b12..b902063 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -31,8 +31,8 @@ public static Shooter getInstance(){ public void shoot(double rotateLeft, double rotateRight){ if(rotateLeft < DEADZONE) rotateLeft = 0; if(rotateRight < DEADZONE) rotateRight = 0; - m_ShooterMotorLeft.set(TalonSRXControlMode.PercentOutput, rotateLeft); - m_ShooterMotorRight.set(TalonSRXControlMode.PercentOutput, rotateRight); + moveLeftMotor(rotateLeft); + moveRightMotor(rotateRight); } // move left motor