NoahLacks63/SecondRobotEval
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Retry for SE2

Browse Source
This commit is contained in:
NoahLacks63 2024-12-18 02:06:22 +00:00
parent 496891cce4
commit 31ef6bb688
5 changed files with 139 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

178
src/main/java/frc/robot/RobotContainer.java
View File

@ -19,109 +19,111 @@ import frc.robot.subsystems.Intake;
import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Shooter;
public class RobotContainer { public class RobotContainer {
private Drivetrain drivetrain; private Drivetrain drivetrain;
private Indexer indexer; private Indexer indexer;
private Intake intake; private Intake intake;
private Shooter shooter; private Shooter shooter;
private CommandXboxController driver; private CommandXboxController driver;
//constructor// //constructor//
public RobotContainer() { public RobotContainer() {
drivetrain = new Drivetrain(); drivetrain = new Drivetrain();
indexer = new Indexer(); indexer = new Indexer();
intake = new Intake(); intake = new Intake();
shooter = new Shooter(); shooter = new Shooter();
driver = new CommandXboxController(OIConstants.kDriverUSB); driver = new CommandXboxController(OIConstants.kDriverUSB);
configureBindings(); configureBindings();
configureShuffleboard(); configureShuffleboard();
} }
private void configureBindings() { private void configureBindings() {
//default commands// //default commands//
drivetrain.setDefaultCommand( drivetrain.setDefaultCommand(
drivetrain.driveArcade( drivetrain.driveArcade(
null, driver::getLeftX,
null driver::getLeftY
) )
); );
indexer.setDefaultCommand(indexer.stopIndexer()); indexer.setDefaultCommand(indexer.stopIndexer());
intake.setDefaultCommand(intake.stopIntake()); intake.setDefaultCommand(intake.stopIntake());
shooter.setDefaultCommand(shooter.stopShooter()); shooter.setDefaultCommand(shooter.runShooter(driver::getRightTriggerAxis));
//driver inputs// //driver inputs//
driver.rightTrigger().whileTrue( driver.a().whileTrue(
shooter.runShooter(driver.getRightTriggerAxis()) intake.runIntake(1)
); .alongWith(indexer.runIndexer(() -> 1))
);
driver.a().whileTrue(
intake.runIntake(1)
.alongWith(indexer.runIndexer(1))
);
driver.b().whileTrue( driver.b().whileTrue(
intake.runIntake(-1) intake.runIntake(-1)
.alongWith(indexer.runIndexer(-1)) .alongWith(indexer.runIndexer(() -> -1))
); );
driver.x().onTrue(smartShooter()); driver.x().onTrue(smartShooter());
} }
//configures shuffleboard// //configures shuffleboard//
private void configureShuffleboard() { private void configureShuffleboard() {
ShuffleboardTab robotIndicatorTab = Shuffleboard.getTab(OIConstants.kRobotIndicatorsTabName); ShuffleboardTab robotIndicatorTab = Shuffleboard.getTab(OIConstants.kRobotIndicatorsTabName);
robotIndicatorTab.addDouble("Left Drivetrain Encoder Distance", drivetrain::getLeftEncoder) robotIndicatorTab.addDouble("Left Drivetrain Encoder Distance", drivetrain::getLeftEncoderPos)
.withPosition(0, 0) .withPosition(0, 0)
.withSize(2, 1) .withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView); .withWidget(BuiltInWidgets.kTextView);
robotIndicatorTab.addDouble("Right Drivetrain Encoder Distance", drivetrain::getRightEncoder) robotIndicatorTab.addDouble("Right Drivetrain Encoder Distance", drivetrain::getRightEncoderPos)
.withPosition(2, 0) .withPosition(2, 0)
.withSize(2, 1) .withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView); .withWidget(BuiltInWidgets.kTextView);
robotIndicatorTab.addDouble("Drivetrain Gyro", drivetrain::getGyroAngle) robotIndicatorTab.addDouble("Drivetrain Gyro", drivetrain::getGyroAngle)
.withPosition(0, 1) .withPosition(0, 1)
.withSize(2, 1) .withSize(2, 1)
.withWidget(BuiltInWidgets.kGyro); .withWidget(BuiltInWidgets.kGyro);
robotIndicatorTab.addDouble("Top Shooter Encoder Rate", shooter::getFrontEncoder) robotIndicatorTab.addDouble("Top Shooter Encoder Rate", shooter::getFrontEncoderRate)
.withPosition(4, 0) .withPosition(4, 0)
.withSize(2, 2) .withSize(2, 2)
.withWidget(BuiltInWidgets.kDial) .withWidget(BuiltInWidgets.kDial)
.withProperties(Map.of( .withProperties(Map.of(
"Max", ShooterConstants.kShooterFireSpeed) "Max", ShooterConstants.kShooterFireSpeed)
); );
robotIndicatorTab.addDouble("Bottom Shooter Encoder Rate", shooter::getFrontEncoder) robotIndicatorTab.addDouble("Bottom Shooter Encoder Rate", shooter::getFrontEncoderRate)
.withPosition(6, 0) .withPosition(6, 0)
.withSize(2, 2) .withSize(2, 2)
.withWidget(BuiltInWidgets.kDial) .withWidget(BuiltInWidgets.kDial)
.withProperties(Map.of( .withProperties(Map.of(
"Max", ShooterConstants.kShooterFireSpeed) "Max", ShooterConstants.kShooterFireSpeed)
); );
} }
public Command getAutonomousCommand() { //moves, rotates, runs auto shooter//
return drivetrain.moveDistance(1.5, 5) public Command getAutonomousCommand() {
.andThen(drivetrain.goToAngle(45, 2)) drivetrain.resetGyro();
.andThen(shooter.runAutoShooter()) drivetrain.resetDriveEncoders();
.until(shooter::shooterAtSpeed)
.andThen(shooter.runAutoShooter())
.alongWith(indexer.runIndexer(1))
.withTimeout(2);
}
//smart commands// return drivetrain.moveDistance(1.5, 5)
private Command smartShooter() { .andThen(
return shooter.runAutoShooter() drivetrain.goToAngle(
.until(shooter::shooterAtSpeed) 45,
.andThen(shooter.runAutoShooter()) 2
.alongWith(indexer.runIndexer(1)) ),
.withTimeout(2); smartShooter()
} );
}
//smart commands//
private Command smartShooter() {
return shooter.runAutoShooter()
.until(shooter::shooterAtSpeed)
.andThen(
shooter.runAutoShooter(),
indexer.runIndexer(() -> 1)
).withTimeout(2);
}
} }

1
src/main/java/frc/robot/constants/ShooterConstants.java
View File

@ -25,4 +25,5 @@ public class ShooterConstants {
public static final int kFFG = 0; public static final int kFFG = 0;
public static final int kFFV = 0; public static final int kFFV = 0;
public static final int kBangBangScale = 8;
} }

44
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.DrivetrainConstants; import frc.robot.constants.DrivetrainConstants;
public class Drivetrain extends SubsystemBase { public class Drivetrain extends SubsystemBase {
//creates instance variables// //creates instance variables//
private CANSparkMax leftFront; private CANSparkMax leftFront;
private CANSparkMax leftRear; private CANSparkMax leftRear;
@ -27,12 +26,12 @@ public class Drivetrain extends SubsystemBase {
private ADXRS450_Gyro gyro; private ADXRS450_Gyro gyro;
private PIDController turnController; private PIDController turnController;
private PIDController distanceController; private PIDController rightDistanceController;
private PIDController leftDistanceController;
private DifferentialDrive drive; private DifferentialDrive drive;
public Drivetrain() { public Drivetrain() {
//creates instances of each motor// //creates instances of each motor//
leftFront = new CANSparkMax(DrivetrainConstants.kLeftFrontCAN, MotorType.kBrushless); leftFront = new CANSparkMax(DrivetrainConstants.kLeftFrontCAN, MotorType.kBrushless);
leftRear = new CANSparkMax(DrivetrainConstants.kLeftRearCAN, MotorType.kBrushless); leftRear = new CANSparkMax(DrivetrainConstants.kLeftRearCAN, MotorType.kBrushless);
@ -57,28 +56,47 @@ public class Drivetrain extends SubsystemBase {
); );
turnController.setTolerance(DrivetrainConstants.kTurnControllerTolerance); turnController.setTolerance(DrivetrainConstants.kTurnControllerTolerance);
distanceController = new PIDController( rightDistanceController = new PIDController(
DrivetrainConstants.kDistanceControllerP, DrivetrainConstants.kDistanceControllerP,
DrivetrainConstants.kDistanceControllerI, DrivetrainConstants.kDistanceControllerI,
DrivetrainConstants.kDistanceControllerD DrivetrainConstants.kDistanceControllerD
); );
distanceController.setTolerance(DrivetrainConstants.kDistanceControllerTolerance); rightDistanceController.setTolerance(DrivetrainConstants.kDistanceControllerTolerance);
leftDistanceController = new PIDController(
DrivetrainConstants.kDistanceControllerP,
DrivetrainConstants.kDistanceControllerI,
DrivetrainConstants.kDistanceControllerD
);
leftDistanceController.setTolerance(DrivetrainConstants.kDistanceControllerTolerance);
drive = new DifferentialDrive(leftFront, rightFront); drive = new DifferentialDrive(leftFront, rightFront);
} }
//getters/ //gyro methods//
public double getGyroAngle() { public double getGyroAngle() {
return gyro.getAngle() * (DrivetrainConstants.kInvertGyro ? -1 : 1); return gyro.getAngle() * (DrivetrainConstants.kInvertGyro ? -1 : 1);
} }
public double getLeftEncoder() { public void resetGyro() {
gyro.reset();
return;
}
//encoder methods//
public double getLeftEncoderPos() {
return leftFrontEncoder.getPosition(); return leftFrontEncoder.getPosition();
} }
public double getRightEncoder() { public double getRightEncoderPos() {
return rightFrontEncoder.getPosition(); return rightFrontEncoder.getPosition();
} }
public void resetDriveEncoders() {
leftFrontEncoder.setPosition(0);
rightFrontEncoder.setPosition(0);
return;
}
//method to turn drivetrain a certain angle// //method to turn drivetrain a certain angle//
public Command goToAngle(double angle, double timeout) { public Command goToAngle(double angle, double timeout) {
@ -94,12 +112,16 @@ public class Drivetrain extends SubsystemBase {
//method to move drivetrain a certain distance// //method to move drivetrain a certain distance//
public Command moveDistance(double distance, double timeout) { public Command moveDistance(double distance, double timeout) {
return run(() -> { return run(() -> {
double output = distanceController.calculate( double outputL = rightDistanceController.calculate(
(leftFrontEncoder.getPosition() + rightFrontEncoder.getPosition()) / 2, (leftFrontEncoder.getPosition() + rightFrontEncoder.getPosition()) / 2,
distance distance
); );
drive.arcadeDrive(output, 0.0); double outputR = rightDistanceController.calculate(
}).until(distanceController::atSetpoint).withTimeout(timeout); (leftFrontEncoder.getPosition() + rightFrontEncoder.getPosition()) / 2,
distance
);
drive.tankDrive(outputL, outputR);
}).until(rightDistanceController::atSetpoint).withTimeout(timeout);
} }
//methods that take 2 doubles and convert them to motor commands// //methods that take 2 doubles and convert them to motor commands//

6
src/main/java/frc/robot/subsystems/Indexer.java
View File

@ -1,5 +1,7 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
@ -15,9 +17,9 @@ public class Indexer extends SubsystemBase {
} }
//indexer commands// //indexer commands//
public Command runIndexer(double speed) { public Command runIndexer(DoubleSupplier speed) {
return run(() -> { return run(() -> {
motor1.set(speed); motor1.set(speed.getAsDouble());
}); });
} }

19
src/main/java/frc/robot/subsystems/Shooter.java
View File

@ -1,5 +1,7 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import java.util.function.DoubleSupplier;
import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.BangBangController;
@ -14,6 +16,7 @@ public class Shooter extends SubsystemBase {
private WPI_VictorSPX rearMotor; private WPI_VictorSPX rearMotor;
private BangBangController bangBangController; private BangBangController bangBangController;
private SimpleMotorFeedforward motorFeedforward; private SimpleMotorFeedforward motorFeedforward;
private Encoder frontMotorEncoder; private Encoder frontMotorEncoder;
@ -50,33 +53,33 @@ public class Shooter extends SubsystemBase {
} }
//getters// //getters//
public double getFrontEncoder() { public double getFrontEncoderRate() {
return frontMotorEncoder.getRate(); return frontMotorEncoder.getRate();
} }
public double getRearEncoder() { public double getRearEncoderRate() {
return rearMotorEncoder.getRate(); return rearMotorEncoder.getRate();
} }
public boolean shooterAtSpeed() { public boolean shooterAtSpeed() {
return (frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2 >= ShooterConstants.kShooterFireSpeed; return bangBangController.atSetpoint();
} }
//shooter commands// //shooter run/stop commands//
public Command runAutoShooter() { public Command runAutoShooter() {
return run(() -> { return run(() -> {
double output = bangBangController.calculate( double output = (bangBangController.calculate(
(frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2, (frontMotorEncoder.getRate() + rearMotorEncoder.getRate()) / 2,
ShooterConstants.kShooterFireSpeed ShooterConstants.kShooterFireSpeed) * ShooterConstants.kBangBangScale
) + motorFeedforward.calculate(ShooterConstants.kShooterFireSpeed); ) + motorFeedforward.calculate(ShooterConstants.kShooterFireSpeed);
frontMotor.setVoltage(output); frontMotor.setVoltage(output);
}); });
} }
public Command runShooter(double speed) { public Command runShooter(DoubleSupplier speed) {
return run(() -> { return run(() -> {
frontMotor.set(speed); frontMotor.set(speed.getAsDouble());
}); });
} }