Team_2648/2024_Robot_Code
5
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

robot testing

Browse Source
This commit is contained in:
Bradley Bickford 2024-02-27 15:48:59 -05:00
parent 3fec792691
commit 82c8e1dcb0
9 changed files with 101 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.roboRIOTeamNumberSetter/roborioteamnumbersetter.json Normal file
View File

@ -0,0 +1 @@
{}

2
src/main/java/frc/robot/Commands/SpeakerShot.java
View File

@ -8,6 +8,6 @@ import frc.robot.subsystems.Shooter;
public class SpeakerShot extends ParallelCommandGroup{
SpeakerShot(Indexer indexer, Shooter shooter){
addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0));
//addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0));
}
}

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

@ -61,6 +61,8 @@ public class RobotContainer {
// TODO There's more than one speaker tag, how do we want to handle this?
private int speakerTag;
private boolean setAmpAngle;
public RobotContainer() {
/*try {
@ -83,9 +85,9 @@ public class RobotContainer {
indexer = new Indexer();
shooter = new Shooter(indexer.getBeamBreak());
shooter = new Shooter(indexer::getBeamBreak);
climber = new Climber(shooter.getShooterAngle());
climber = new Climber(shooter::getShooterAngle);
@ -113,6 +115,8 @@ public class RobotContainer {
speakerTag = 8;
}
setAmpAngle = false;
configureBindings();
shuffleboardSetup();
}
@ -130,20 +134,47 @@ public class RobotContainer {
drivetrain.teleopCommand(
driver::getLeftY,
driver::getLeftX,
driver::getRightX,
() -> { return -driver.getRightX(); },
OIConstants.kTeleopDriveDeadband
)
);
//intake.setDefaultCommand(intake.intakeUpCommand());
intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, driver::getLeftTriggerAxis));
intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, () -> 0.0));
// shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0));
shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis));
shooter.setDefaultCommand(
shooter.angleSpeedsSetpoints(
() -> {
if (setAmpAngle) {
return ShooterConstants.kShooterAmpAngle;
} else {
return ShooterConstants.kShooterLoadAngle;
}
},
() -> {
if (driver.getLeftTriggerAxis() > .25) {
return -1.0;
}else if(driver.getRightTriggerAxis() > 0.25){
return 1.0;
} else {
return 0;
}
}
)
);
//shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis));
climber.setDefaultCommand(climber.stop());
indexer.setDefaultCommand(indexer.shootNote(operator::getRightTriggerAxis));
indexer.setDefaultCommand(indexer.shootNote(
() -> {
if (driver.getLeftTriggerAxis() > .25) {
return -1.0;
}else {
return 0.0;
}
}
));
driver.povCenter().onFalse(
drivetrain.driveCardinal(
@ -208,9 +239,18 @@ public class RobotContainer {
*/
driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
driver.leftBumper().toggleOnTrue(intake.intakeDownCommand());
operator.y().onTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis));
//driver.leftBumper().toggleOnTrue(intake.intakeDownCommand());
operator.a().onTrue(new InstantCommand(() -> { setAmpAngle = true; }));
operator.a().onFalse(new InstantCommand(() -> { setAmpAngle = false; }));
//operator.a().whileTrue(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterAmpAngle, 0, 0));
operator.y().whileTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis));
driver.a().whileTrue(indexer.shootNote(() -> 1.0));
}
@ -229,10 +269,10 @@ public class RobotContainer {
.withPosition(0, 0)
.withSize(2, 1)
.withWidget(BuiltInWidgets.kBooleanBox);
teleopTab.addBoolean("indexer beam break", indexer.getBeamBreak());
teleopTab.addBoolean("shooter beam break", shooter.getBeamBreak());
teleopTab.addDouble("shooter angle", shooter.getShooterAngle());
teleopTab.addDouble("intake angle", intake.getIntakeAngle());
teleopTab.addBoolean("indexer beam break", indexer::getBeamBreak);
teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak);
teleopTab.addDouble("shooter angle", shooter::getShooterAngle);
teleopTab.addDouble("intake angle", intake::getIntakeAngle);
}
public Command getAutonomousCommand() {

2
src/main/java/frc/robot/constants/DrivetrainConstants.java
View File

@ -47,5 +47,5 @@ public final class DrivetrainConstants {
public static final boolean kGyroReversed = true;
public static final double kRobotStartOffset = 90;
public static final double kRobotStartOffset = 0.0;
}

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

@ -12,7 +12,7 @@ public class ShooterConstants {
public static final double kShooterFF = 0.0;
public static final double kShooterPivotP = 0.0;
public static final double kShooterPivotP = 3.0;
public static final double kShooterPivotI = 0.0;
public static final double kShooterPivotD = 0.0;

12
src/main/java/frc/robot/subsystems/Climber.java
View File

@ -16,12 +16,14 @@ public class Climber extends SubsystemBase {
private DoubleSupplier shooterAngle;
public Climber(DoubleSupplier shooterAngle) {
if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){
motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
motor2.follow(motor1);
}
motor2.follow(motor1);
motor1.setInverted(true);
motor2.setInverted(true);
this.shooterAngle = shooterAngle;
}
public Command setSpeedWithSupplier(DoubleSupplier speed) {

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

@ -55,7 +55,7 @@ public class Indexer extends SubsystemBase{
});
}
public BooleanSupplier getBeamBreak(){
return indexerBeamBreak::get;
public boolean getBeamBreak(){
return indexerBeamBreak.get();
}
}

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

@ -27,7 +27,7 @@ public class Intake extends SubsystemBase{
intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
intakeRoller.setSmartCurrentLimit(60);
intakeRoller.setIdleMode(IdleMode.kCoast);
intakeRoller.setIdleMode(IdleMode.kBrake);
intakeRoller.burnFlash();
intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless);
@ -109,7 +109,7 @@ public class Intake extends SubsystemBase{
});
}
public DoubleSupplier getIntakeAngle(){
return intakeEncoder::getPosition;
public double getIntakeAngle(){
return intakeEncoder.getPosition();
}
}

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

@ -28,9 +28,6 @@ public class Shooter extends SubsystemBase{
private Encoder pivotEncoder;
private SparkPIDController topPID;
private SparkPIDController bottomPID;
private PIDController shooterPivotPID;
private ArmFeedforward shooterPivotFF;
@ -41,15 +38,19 @@ public class Shooter extends SubsystemBase{
public Shooter(BooleanSupplier indexerBeamBreak){
topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
shooterPivot.setInverted(true);
topEncoder = topShooter.getEncoder();
bottomEncoder = bottomShooter.getEncoder();
/*
topPID = topShooter.getPIDController();
topPID.setFeedbackDevice(topEncoder);
bottomPID = bottomShooter.getPIDController();
bottomPID.setFeedbackDevice(bottomEncoder);
*/
shooterPivot.setSmartCurrentLimit(50);
topShooter.setSmartCurrentLimit(40);
@ -60,46 +61,52 @@ public class Shooter extends SubsystemBase{
shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel);
topShooter.setIdleMode(IdleMode.kBrake);
topShooter.setIdleMode(IdleMode.kCoast);
bottomShooter.setIdleMode(IdleMode.kCoast);
bottomShooter.burnFlash();
shooterPivot.burnFlash();
topShooter.burnFlash();
topPID.setP(ShooterConstants.kShooterP);
topPID.setI(ShooterConstants.kShooterI);
topPID.setD(ShooterConstants.kShooterD);
topPID.setFF(ShooterConstants.kShooterFF);
bottomPID.setP(ShooterConstants.kShooterP);
bottomPID.setI(ShooterConstants.kShooterI);
bottomPID.setI(ShooterConstants.kShooterD);
bottomPID.setFF(ShooterConstants.kShooterFF);
shooterPivotPID = new PIDController(
ShooterConstants.kShooterPivotP,
ShooterConstants.kShooterPivotI,
ShooterConstants.kShooterPivotD
);
shooterPivotFF = new ArmFeedforward(ShooterConstants.kSShooterPivotFF, ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF);
}
/*
public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
return run(()-> {
angleAndSpeedControl(setpointAngle, topRPM, bottomRPM);
});
}*/
public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, DoubleSupplier speed){
return run(()-> {
angleAndSpeedControl(setpointAngle.getAsDouble(), speed.getAsDouble(), speed.getAsDouble());
});
}
public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, double topRPM, double bottomRPM){
return run(()-> {
angleAndSpeedControl(setpointAngle.getAsDouble(), topRPM, bottomRPM);
});
}
private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){
shooterPivot.setIdleMode(IdleMode.kBrake);
shooterPivot.setVoltage(
shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) +
shooterPivotPID.calculate(getShooterAngle(), setpointAngle) +
shooterPivotFF.calculate(setpointAngle, 0.0));
topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity);
bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity);
//topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity);
//bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity);
topShooter.set(topRPM);
bottomShooter.set(bottomRPM);
}
public Command ampHandoff(){
@ -107,11 +114,11 @@ public class Shooter extends SubsystemBase{
shooterPivot.setIdleMode(IdleMode.kBrake);
shooterPivot.setVoltage(
shooterPivotPID.calculate(pivotEncoder.getDistance(), ShooterConstants.kShooterLoadAngle) +
shooterPivotPID.calculate(getShooterAngle(), ShooterConstants.kShooterLoadAngle) +
shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
if(shooterBeamBreak.get() || indexerBeamBreak){
angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 1000.0, 1000.0);
angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 0.5, 0.5);
}else{
angleAndSpeedControl(ShooterConstants.kShooterAmpAngle, 0, 0);
}
@ -125,12 +132,12 @@ public class Shooter extends SubsystemBase{
});
}
public DoubleSupplier getShooterAngle(){
return () -> {return pivotEncoder.getDistance();};
public double getShooterAngle(){
return pivotEncoder.getDistance() + ShooterConstants.kShooterLoadAngle;
}
public BooleanSupplier getBeamBreak(){
return shooterBeamBreak::get;
public Boolean getBeamBreak(){
return shooterBeamBreak.get();
}
public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier spinny){
@ -141,4 +148,5 @@ public class Shooter extends SubsystemBase{
});
}
}