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;
public class RobotContainer {
private Drivetrain drivetrain;
private Indexer indexer;
private Intake intake;
private Shooter shooter;
private Drivetrain drivetrain;
private Indexer indexer;
private Intake intake;
private Shooter shooter;
private CommandXboxController driver;
private CommandXboxController driver;
//constructor//
public RobotContainer() {
drivetrain = new Drivetrain();
indexer = new Indexer();
intake = new Intake();
shooter = new Shooter();
//constructor//
public RobotContainer() {
drivetrain = new Drivetrain();
indexer = new Indexer();
intake = new Intake();
shooter = new Shooter();
driver = new CommandXboxController(OIConstants.kDriverUSB);
driver = new CommandXboxController(OIConstants.kDriverUSB);
configureBindings();
configureShuffleboard();
}
configureBindings();
configureShuffleboard();
}
private void configureBindings() {
//default commands//
drivetrain.setDefaultCommand(
drivetrain.driveArcade(
null,
null
)
);
private void configureBindings() {
//default commands//
drivetrain.setDefaultCommand(
drivetrain.driveArcade(
driver::getLeftX,
driver::getLeftY
)
);
indexer.setDefaultCommand(indexer.stopIndexer());
intake.setDefaultCommand(intake.stopIntake());
shooter.setDefaultCommand(shooter.stopShooter());
indexer.setDefaultCommand(indexer.stopIndexer());
intake.setDefaultCommand(intake.stopIntake());
shooter.setDefaultCommand(shooter.runShooter(driver::getRightTriggerAxis));
//driver inputs//
driver.rightTrigger().whileTrue(
shooter.runShooter(driver.getRightTriggerAxis())
);
driver.a().whileTrue(
intake.runIntake(1)
.alongWith(indexer.runIndexer(1))
);
//driver inputs//
driver.a().whileTrue(
intake.runIntake(1)
.alongWith(indexer.runIndexer(() -> 1))
);
driver.b().whileTrue(
intake.runIntake(-1)
.alongWith(indexer.runIndexer(-1))
);
driver.b().whileTrue(
intake.runIntake(-1)
.alongWith(indexer.runIndexer(() -> -1))
);
driver.x().onTrue(smartShooter());
}
driver.x().onTrue(smartShooter());
}
//configures shuffleboard//
private void configureShuffleboard() {
ShuffleboardTab robotIndicatorTab = Shuffleboard.getTab(OIConstants.kRobotIndicatorsTabName);
//configures shuffleboard//
private void configureShuffleboard() {
ShuffleboardTab robotIndicatorTab = Shuffleboard.getTab(OIConstants.kRobotIndicatorsTabName);
robotIndicatorTab.addDouble("Left Drivetrain Encoder Distance", drivetrain::getLeftEncoder)
.withPosition(0, 0)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView);
robotIndicatorTab.addDouble("Left Drivetrain Encoder Distance", drivetrain::getLeftEncoderPos)
.withPosition(0, 0)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView);
robotIndicatorTab.addDouble("Right Drivetrain Encoder Distance", drivetrain::getRightEncoder)
.withPosition(2, 0)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView);
robotIndicatorTab.addDouble("Right Drivetrain Encoder Distance", drivetrain::getRightEncoderPos)
.withPosition(2, 0)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kTextView);
robotIndicatorTab.addDouble("Drivetrain Gyro", drivetrain::getGyroAngle)
.withPosition(0, 1)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kGyro);
robotIndicatorTab.addDouble("Drivetrain Gyro", drivetrain::getGyroAngle)
.withPosition(0, 1)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kGyro);
robotIndicatorTab.addDouble("Top Shooter Encoder Rate", shooter::getFrontEncoder)
.withPosition(4, 0)
.withSize(2, 2)
.withWidget(BuiltInWidgets.kDial)
.withProperties(Map.of(
"Max", ShooterConstants.kShooterFireSpeed)
);
robotIndicatorTab.addDouble("Top Shooter Encoder Rate", shooter::getFrontEncoderRate)
.withPosition(4, 0)
.withSize(2, 2)
.withWidget(BuiltInWidgets.kDial)
.withProperties(Map.of(
"Max", ShooterConstants.kShooterFireSpeed)
);
robotIndicatorTab.addDouble("Bottom Shooter Encoder Rate", shooter::getFrontEncoder)
.withPosition(6, 0)
.withSize(2, 2)
.withWidget(BuiltInWidgets.kDial)
.withProperties(Map.of(
"Max", ShooterConstants.kShooterFireSpeed)
);
}
robotIndicatorTab.addDouble("Bottom Shooter Encoder Rate", shooter::getFrontEncoderRate)
.withPosition(6, 0)
.withSize(2, 2)
.withWidget(BuiltInWidgets.kDial)
.withProperties(Map.of(
"Max", ShooterConstants.kShooterFireSpeed)
);
}
public Command getAutonomousCommand() {
return drivetrain.moveDistance(1.5, 5)
.andThen(drivetrain.goToAngle(45, 2))
.andThen(shooter.runAutoShooter())
.until(shooter::shooterAtSpeed)
.andThen(shooter.runAutoShooter())
.alongWith(indexer.runIndexer(1))
.withTimeout(2);
}
//moves, rotates, runs auto shooter//
public Command getAutonomousCommand() {
drivetrain.resetGyro();
drivetrain.resetDriveEncoders();
//smart commands//
private Command smartShooter() {
return shooter.runAutoShooter()
.until(shooter::shooterAtSpeed)
.andThen(shooter.runAutoShooter())
.alongWith(indexer.runIndexer(1))
.withTimeout(2);
}
return drivetrain.moveDistance(1.5, 5)
.andThen(
drivetrain.goToAngle(
45,
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 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;
public class Drivetrain extends SubsystemBase {
//creates instance variables//
private CANSparkMax leftFront;
private CANSparkMax leftRear;
@ -27,12 +26,12 @@ public class Drivetrain extends SubsystemBase {
private ADXRS450_Gyro gyro;
private PIDController turnController;
private PIDController distanceController;
private PIDController rightDistanceController;
private PIDController leftDistanceController;
private DifferentialDrive drive;
public Drivetrain() {
//creates instances of each motor//
leftFront = new CANSparkMax(DrivetrainConstants.kLeftFrontCAN, MotorType.kBrushless);
leftRear = new CANSparkMax(DrivetrainConstants.kLeftRearCAN, MotorType.kBrushless);
@ -57,28 +56,47 @@ public class Drivetrain extends SubsystemBase {
);
turnController.setTolerance(DrivetrainConstants.kTurnControllerTolerance);
distanceController = new PIDController(
rightDistanceController = new PIDController(
DrivetrainConstants.kDistanceControllerP,
DrivetrainConstants.kDistanceControllerI,
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);
}
//getters/
//gyro methods//
public double getGyroAngle() {
return gyro.getAngle() * (DrivetrainConstants.kInvertGyro ? -1 : 1);
}
public double getLeftEncoder() {
public void resetGyro() {
gyro.reset();
return;
}
//encoder methods//
public double getLeftEncoderPos() {
return leftFrontEncoder.getPosition();
}
public double getRightEncoder() {
public double getRightEncoderPos() {
return rightFrontEncoder.getPosition();
}
public void resetDriveEncoders() {
leftFrontEncoder.setPosition(0);
rightFrontEncoder.setPosition(0);
return;
}
//method to turn drivetrain a certain angle//
public Command goToAngle(double angle, double timeout) {
@ -94,12 +112,16 @@ public class Drivetrain extends SubsystemBase {
//method to move drivetrain a certain distance//
public Command moveDistance(double distance, double timeout) {
return run(() -> {
double output = distanceController.calculate(
double outputL = rightDistanceController.calculate(
(leftFrontEncoder.getPosition() + rightFrontEncoder.getPosition()) / 2,
distance
);
drive.arcadeDrive(output, 0.0);
}).until(distanceController::atSetpoint).withTimeout(timeout);
double outputR = rightDistanceController.calculate(
(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//

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

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