diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6b77ab7..018127e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,11 +68,11 @@ public RobotContainer(RobotRunType runtimeType) { ; private void configureButtonBindings() { - driver.rightTrigger().whileTrue(intake.intakeCommand()); - driver.leftTrigger().whileTrue(intake.outakCommand()); - driver.a().whileTrue(randMot.runPositiveCommand()); - driver.b().whileTrue(randMot.runNegativeCommand()); - driveTrain.setDefaultCommand(driveTrain.driveCommand(driver.getLeftY(), driver.getRightY())); + driver.rightTrigger().whileTrue(intake.intakeCommand(1)); + driver.leftTrigger().whileTrue(intake.intakeCommand(-1)); + driver.a().whileTrue(randMot.runMotorCommand(1)); + driver.b().whileTrue(randMot.runMotorCommand(-1)); + driveTrain.setDefaultCommand(driveTrain.driveCommand(driver)); } diff --git a/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotors.java b/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotors.java index 6eca39e..52ae61d 100644 --- a/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotors.java +++ b/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotors.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.RandomMotors; import org.littletonrobotics.junction.Logger; - import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,14 +24,10 @@ public void runMotor(double power) { } public Command runMotorCommand(double power) { - return run(() -> runMotor(power)); - } - - public Command runPositiveCommand() { - return runMotorCommand(1); - } - - public Command runNegativeCommand() { - return runMotorCommand(-1); + return startEnd(() -> { + runMotor(power); + }, () -> { + runMotor(0); + }); } } diff --git a/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotorsIO.java b/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotorsIO.java index 64c1873..3ce5876 100644 --- a/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotorsIO.java +++ b/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotorsIO.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.RandomMotors; import org.littletonrobotics.junction.AutoLog; + public interface RandomMotorsIO { @@ -16,4 +17,8 @@ public static class RandomMotorsIOInputs { public default void updateInputsIO(RandomMotorsIOInputs inputs) {} public default void runMotorIO(double voltage) {} -} \ No newline at end of file + + public default double encoder() { + return 0; + } +} diff --git a/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotorsReal.java b/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotorsReal.java index 0bfb937..6cece45 100644 --- a/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotorsReal.java +++ b/src/main/java/frc/robot/subsystems/RandomMotors/RandomMotorsReal.java @@ -5,7 +5,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; - import frc.robot.Constants; public class RandomMotorsReal implements RandomMotorsIO { @@ -13,12 +12,16 @@ public class RandomMotorsReal implements RandomMotorsIO { new CANSparkMax(Constants.Motors.RandomMotors.NEO, MotorType.kBrushless); private final TalonFX falcon = new TalonFX(Constants.Motors.RandomMotors.FALCON); private RelativeEncoder rEncoder = neo.getEncoder(); - private StatusSignal falconVelocity; + private StatusSignal falconVelocity; - public RandomMotorsReal(){ + public RandomMotorsReal() { falconVelocity = falcon.getVelocity(); } + public double encoder() { + return neo.get(); + } + @Override public void updateInputsIO(RandomMotorsIOInputs inputs) { inputs.falconVelocityIO = getFalconVelocity(); @@ -31,9 +34,8 @@ public void runMotor(double power) { falcon.set(power); } - public double getNeoVelocity(){ - return rEncoder.getVelocity - (); + public double getNeoVelocity() { + return rEncoder.getVelocity(); } private double getFalconVelocity() { diff --git a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java index 8dc0577..5905a61 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/Drivetrain.java @@ -1,8 +1,9 @@ package frc.robot.subsystems.drive; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; /** * Drivetrain subsystem. @@ -29,8 +30,8 @@ public void setPower(double lPower, double rPower) { io.setDrivePowerIO(lPower, rPower); } - public Command driveCommand(double leftY, double rightY) { - return run(() -> setPower(leftY, rightY)); + public Command driveCommand(CommandXboxController controller) { + return run(() -> setPower(controller.getLeftY(), controller.getRightY())); } } diff --git a/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java b/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java index c9fa2c9..94d130f 100644 --- a/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java +++ b/src/main/java/frc/robot/subsystems/drive/DrivetrainReal.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.drive; -import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj.motorcontrol.VictorSP; import frc.robot.Constants; @@ -9,20 +8,20 @@ */ public class DrivetrainReal implements DrivetrainIO { - VictorSP FLMotor = new VictorSP(Constants.Motors.DriveTrainMotors.FLMOTOR); - VictorSP FRMotor = new VictorSP(Constants.Motors.DriveTrainMotors.FRMOTOR); - VictorSP BLMotor = new VictorSP(Constants.Motors.DriveTrainMotors.BLMOTOR); - VictorSP BRMotor = new VictorSP(Constants.Motors.DriveTrainMotors.BRMOTOR); + private final VictorSP FLMotor = new VictorSP(Constants.Motors.DriveTrainMotors.FLMOTOR); + private final VictorSP FRMotor = new VictorSP(Constants.Motors.DriveTrainMotors.FRMOTOR); + private final VictorSP BLMotor = new VictorSP(Constants.Motors.DriveTrainMotors.BLMOTOR); + private final VictorSP BRMotor = new VictorSP(Constants.Motors.DriveTrainMotors.BRMOTOR); /** * Drivetrain Real */ public DrivetrainReal() { - FLMotor.addFollower(BLMotor); - FRMotor.addFollower(BRMotor); + BLMotor.addFollower(FLMotor); + BRMotor.addFollower(FRMotor); - FRMotor.setInverted(true); + BLMotor.setInverted(true); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/intake.java b/src/main/java/frc/robot/subsystems/intake/intake.java index 41b9484..c215db0 100644 --- a/src/main/java/frc/robot/subsystems/intake/intake.java +++ b/src/main/java/frc/robot/subsystems/intake/intake.java @@ -1,8 +1,8 @@ package frc.robot.subsystems.Intake; import org.littletonrobotics.junction.Logger; - import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Intake extends SubsystemBase { @@ -26,17 +26,12 @@ public void setIntakePower(double power) { } - public Command runMotorCommand(int power) { - return run(() -> setIntakePower(power)); - } - - - - public Command intakeCommand() { - return runMotorCommand(1); // Borrows runMotorCommand from above at 1 - } - public Command outakCommand() { - return runMotorCommand(-1); // Borrows runMotorCommand from above at -1 + public Command intakeCommand(double power) { + return Commands.startEnd(() -> { + setIntakePower(power); + }, () -> { + setIntakePower(0); + }, this); } diff --git a/src/main/java/frc/robot/subsystems/intake/intakeReal.java b/src/main/java/frc/robot/subsystems/intake/intakeReal.java index b9d0dec..9ac4364 100644 --- a/src/main/java/frc/robot/subsystems/intake/intakeReal.java +++ b/src/main/java/frc/robot/subsystems/intake/intakeReal.java @@ -5,18 +5,20 @@ public class intakeReal implements IntakeIO { VictorSP leftIntake = new VictorSP(frc.robot.Constants.Motors.IntakeMotors.LMOTOR); VictorSP rightIntake = new VictorSP(frc.robot.Constants.Motors.IntakeMotors.RMOTOR); - - public void setIntakePower(double power) { + + public intakeReal() { rightIntake.setInverted(true); + leftIntake.addFollower(rightIntake); + } + + public void setIntakePower(double power) { leftIntake.set(power); - rightIntake.set(power); } @Override - public void updateInputsIO(IntakeIOInputs inputs) { - }; + public void updateInputsIO(IntakeIOInputs inputs) {}; }