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

shooter method real value conversions on pivots, drivetrain slew gone

Browse Source
This commit is contained in:
Tyler-J42 2024-02-17 01:51:16 -05:00
parent 56fb39d38c
commit 9e8377b574
5 changed files with 40 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,8 +36,8 @@ public class RobotContainer {
private Drivetrain drivetrain;
private CommandXboxController primary;
//private CommandXboxController secondary;
private CommandXboxController driver;
private CommandXboxController operator;
private Trigger isTeleopTrigger;
@ -75,7 +75,7 @@ public class RobotContainer {
// TODO Specify a default auto string once we have one
autoChooser = AutoBuilder.buildAutoChooser();
primary = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
driver = new CommandXboxController(OIConstants.kPrimaryXboxUSB);
//secondary = new CommandXboxController(OIConstants.kSecondaryXboxUSB);
isTeleopTrigger = new Trigger(DriverStation::isTeleopEnabled);
@ -107,64 +107,64 @@ public class RobotContainer {
drivetrain.setDefaultCommand(
drivetrain.teleopCommand(
primary::getLeftY,
primary::getLeftX,
primary::getRightX,
driver::getLeftY,
driver::getLeftX,
driver::getRightX,
OIConstants.kTeleopDriveDeadband
)
);
primary.povCenter().onFalse(
driver.povCenter().onFalse(
drivetrain.driveCardinal(
primary::getLeftY,
primary::getLeftX,
primary.getHID()::getPOV,
driver::getLeftY,
driver::getLeftX,
driver.getHID()::getPOV,
OIConstants.kTeleopDriveDeadband).until(
() -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
)
);
// Lock on to the appropriate Amp AprilTag while still being able to strafe and drive forward and back
primary.a().onTrue(
driver.a().onTrue(
drivetrain.driveAprilTagLock(
primary::getLeftY,
primary::getLeftX,
driver::getLeftY,
driver::getLeftX,
OIConstants.kTeleopDriveDeadband,
ampTagID
).until(
() -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
)
);
// Lock on to the appropriate Source AprilTag while still being able to strafe and drive forward and back
primary.b().onTrue(
driver.b().onTrue(
drivetrain.driveAprilTagLock(
primary::getLeftY,
primary::getLeftX,
driver::getLeftY,
driver::getLeftX,
OIConstants.kTeleopDriveDeadband,
sourceTagID
).until(
() -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
)
);
// Lock on to the appropriate Speaker AprilTag while still being able to strafe and drive forward and back
primary.x().onTrue(
driver.x().onTrue(
drivetrain.driveAprilTagLock(
primary::getLeftY,
primary::getLeftX,
driver::getLeftY,
driver::getLeftX,
OIConstants.kTeleopDriveDeadband,
speakerTag
).until(
() -> MathUtil.applyDeadband(primary.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
() -> MathUtil.applyDeadband(driver.getRightX(), OIConstants.kTeleopDriveDeadband) != 0
)
);
// This was originally a run while held, not sure that's really necessary, change it if need be
primary.y().onTrue(drivetrain.zeroHeadingCommand());
driver.y().onTrue(drivetrain.zeroHeadingCommand());
// This was originally a run while held, not sure that's really necessary, change it if need be
primary.rightBumper().onTrue(drivetrain.setXCommand());
driver.rightBumper().onTrue(drivetrain.setXCommand());
/*
* This has been added because interest has been expressed in trying field relative vs
@ -175,7 +175,7 @@ public class RobotContainer {
* If it becomes something that we need to switch <i>prior</i> to the start of the match, a different
* mechanism will need to be devised, this will work for now.
*/
primary.start().onTrue(drivetrain.toggleFieldRelativeControl());
driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
}
private void shuffleboardSetup() {

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

@ -4,7 +4,7 @@ public class IntakeConstants {
public static final int kIntakePivotID = 9;
public static final int kIntakeRollerID = 10;
public static final double kIntakePivotConversionFactor = 0;
public static final double kIntakePivotConversionFactor = 1/(20.0*(28.0/15.0));
public static final int kPivotCurrentLimit = 40;

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

@ -17,6 +17,8 @@ public class ShooterConstants {
public static final double kShooterPivotI = 0.0;
public static final double kShooterPivotD = 0.0;
public static final double kPivotConversion = 1/(40.0*(28.0/15.0));
public static final double kSShooterPivotFF = 0.0;
public static final double kGShooterPivotFF = 0.0;
public static final double kVShooterPivotFF = 0.0;

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

@ -301,7 +301,7 @@ public class Drivetrain extends SubsystemBase {
-MathUtil.applyDeadband(ySpeed.getAsDouble(), deadband),
-MathUtil.applyDeadband(rotation.getAsDouble(), deadband),
() -> fieldRelativeControl,
true
false
);
});
}

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

@ -6,9 +6,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@ -31,7 +29,7 @@ public class Shooter extends SubsystemBase{
private PIDController bottomShooterPID;
private SimpleMotorFeedforward bottomShooterFF;
private ProfiledPIDController shooterPivotPID;
private PIDController shooterPivotPID;
private ArmFeedforward shooterPivotFF;
public Shooter(){
@ -44,6 +42,7 @@ public class Shooter extends SubsystemBase{
shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
pivotEncoder = new Encoder(0, 1);
pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion);
//TODO These can probably devolve into BangBang controllers and let the feed forwards maintain speed
topShooterPID = new PIDController(
@ -60,14 +59,10 @@ public class Shooter extends SubsystemBase{
topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
shooterPivotPID = new ProfiledPIDController(
shooterPivotPID = new PIDController(
ShooterConstants.kShooterPivotP,
ShooterConstants.kShooterPivotI,
ShooterConstants.kShooterPivotD,
new TrapezoidProfile.Constraints(
ShooterConstants.kMaxPivotSpeed,
ShooterConstants.kMaxPivotAcceleration
)
ShooterConstants.kShooterPivotD
);
shooterPivotFF = new ArmFeedforward(
@ -77,11 +72,15 @@ public class Shooter extends SubsystemBase{
);
}
public Command setShooterAngle(double setpointAngle){
public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
return run(()-> {
shooterPivot.setVoltage(
shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) +
shooterPivotFF.calculate(setpointAngle, 0));
shooterPivotFF.calculate(setpointAngle, 0.0));
topShooter.setVoltage(topShooterPID.calculate(topEncoder.getVelocity(), topRPM)+topShooterFF.calculate(topRPM));
bottomShooter.setVoltage(bottomShooterPID.calculate(bottomEncoder.getVelocity(), bottomRPM)+bottomShooterFF.calculate(bottomRPM));
});
}
}