diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 781a1a9a..ff6bfa8b 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -193,8 +193,7 @@ private void setBindingsDriver() { .onFalse(new InstantCommand(()->drivetrain.setFieldOriented(true))); axisTrigger(driverController, Manipulator.SHOOTER_BUTTON) - .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"), - new IntakeNEO(intakeShooter))); + .whileTrue(new AlignToApriltagMegatag2(drivetrain, limelight)); new JoystickButton(driverController, Driver.rotateFieldRelative0Deg) .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain)); new JoystickButton(driverController, Driver.rotateFieldRelative90Deg) diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java b/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java index 2a9b4722..132643da 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeNEO.java @@ -14,6 +14,8 @@ public class IntakeNEO extends Command { public IntakeNEO(IntakeShooter intake) { addRequirements(this.intake = intake); + SmartDashboard.putNumber("Initial intake speed", .5); + SmartDashboard.putNumber("Slow intake speed", .1); } @Override @@ -22,18 +24,22 @@ public void initialize() { // if (intake.intakeDetectsNote()) { // return; // } - intake.motorSetIntake(.5); // Fast intake speed for initial intake + intake.motorSetIntake(SmartDashboard.getNumber("Initial intake speed", 0)); // Fast intake speed + // for initial + // intake intake.resetCurrentLimit(); - } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { // Intake Led - if((intake.intakeDetectsNote())) { - intake.motorSetIntake(.1); // Slower intake speed triggered after intake ds sees note + if ((intake.intakeDetectsNote())) { + intake.motorSetIntake(SmartDashboard.getNumber("Slow intake speed", 0)); // Slower intake + // speed triggered + // after intake ds + // sees note } } @@ -41,7 +47,7 @@ public void execute() { @Override public void end(boolean interrupted) { intake.stopIntake(); - //intake.resetCurrentLimit(); + // intake.resetCurrentLimit(); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 5dc506a8..2de12074 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -208,6 +208,7 @@ public Drivetrain() { } SmartDashboard.putData("Field", field); + SmartDashboard.putData(this); // for(CANSparkMax driveMotor : driveMotors) // driveMotor.setSmartCurrentLimit(80); @@ -286,7 +287,6 @@ public void periodic() { // moduleBL.periodic(); // moduleBR.periodic(); // double goal = SmartDashboard.getNumber("bigoal", 0); - for (SwerveModule module : modules) { module.periodic(); // module.move(0, goal); @@ -298,12 +298,7 @@ public void periodic() { odometry.update(gyro.getRotation2d(), getModulePositions()); //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); - { - // SmartDashboard.putNumber("front left encoder", moduleFL.getModuleAngle()); - // SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); - // SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); - // SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); - } + //setPose(new Pose2d(SmartDashboard.getNumber("set x", getPose().getTranslation().getX()), SmartDashboard.getNumber("set y", getPose().getTranslation().getY()), Rotation2d.fromDegrees(getHeading()))); // SmartDashboard.putNumber("Odometry X", getPose().getTranslation().getX()); // SmartDashboard.putNumber("Odometry Y", getPose().getTranslation().getY()); @@ -345,6 +340,15 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("Roll", gyro::getRoll, null); builder.addDoubleProperty("Field Offset", () -> fieldOffset, fieldOffset -> this.fieldOffset = fieldOffset); + builder.addDoubleProperty("FL Turn Encoder (Deg)", + () -> moduleFL.getModuleAngle(), null); + builder.addDoubleProperty("FR Turn Encoder (Deg)", + () -> moduleFR.getModuleAngle(), null); + builder.addDoubleProperty("BL Turn Encoder (Deg)", + () -> moduleBL.getModuleAngle(), null); + builder.addDoubleProperty("BR Turn Encoder (Deg)", + () -> moduleBR.getModuleAngle(), null); + } // #region Drive Methods