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

on controller shooter pid + paths

Browse Source
This commit is contained in:
Tyler-J42
2024-02-22 01:51:52 -05:00
parent 570696fa7a
commit b262fde35f
17 changed files with 499 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -80,11 +80,13 @@ public class RobotContainer {
intake = new Intake();
shooter = new Shooter();
indexer = new Indexer();
shooter = new Shooter(indexer.getBeamBreak());
climber = new Climber(shooter.getShooterAngle());
indexer = new Indexer();
// An example Named Command, doesn't need to remain once we start actually adding real things
// ALL Named Commands need to be defined AFTER subsystem initialization and BEFORE auto/controller configuration

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

@@ -7,7 +7,7 @@ import edu.wpi.first.math.util.Units;
public final class DrivetrainConstants {
// Driving Parameters - Note that these are not the maximum capable speeds of
// the robot, rather the allowed maximum speeds
public static final double kMaxSpeedMetersPerSecond = 4.6;
public static final double kMaxSpeedMetersPerSecond = 4.1;
public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
public static final double kDirectionSlewRate = 2.4; // radians per second

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

@@ -10,8 +10,7 @@ public class ShooterConstants {
public static final double kShooterI = 0.0;
public static final double kShooterD = 0.0;
public static final double kSShooterFF = 0.0;
public static final double kVShooterFF = 0.0;
public static final double kShooterFF = 0.0;
public static final double kShooterPivotP = 0.0;
public static final double kShooterPivotI = 0.0;

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

@@ -49,6 +49,6 @@ public class SwerveModuleConstants {
public static final IdleMode kDrivingMotorIdleMode = IdleMode.kBrake;
public static final IdleMode kTurningMotorIdleMode = IdleMode.kBrake;
public static final int kDrivingMotorCurrentLimit = 60; // amps
public static final int kDrivingMotorCurrentLimit = 65; // amps
public static final int kTurningMotorCurrentLimit = 30; // amps
}

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

@@ -15,7 +15,6 @@ public class Climber extends SubsystemBase {
private DoubleSupplier shooterAngle;
//TODO What tells the climber to stop climbing when it achieves the target height?
public Climber(DoubleSupplier shooterAngle) {
if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){
motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);

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

@@ -5,12 +5,12 @@ import java.util.function.DoubleSupplier;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkPIDController;
import com.revrobotics.CANSparkBase.IdleMode;
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.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.Command;
@@ -28,54 +28,49 @@ public class Shooter extends SubsystemBase{
private Encoder pivotEncoder;
private PIDController topShooterPID;
private SimpleMotorFeedforward topShooterFF;
private PIDController bottomShooterPID;
private SimpleMotorFeedforward bottomShooterFF;
private SparkPIDController topPID;
private SparkPIDController bottomPID;
private PIDController shooterPivotPID;
private ArmFeedforward shooterPivotFF;
private DigitalInput shooterBeamBreak;
public Shooter(){
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);
topShooter.setSmartCurrentLimit(40);
topShooter.burnFlash();
bottomShooter.setSmartCurrentLimit(40);
bottomShooter.burnFlash();
topEncoder = topShooter.getEncoder();
bottomEncoder = bottomShooter.getEncoder();
shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
topPID = topShooter.getPIDController();
topPID.setFeedbackDevice(topEncoder);
bottomPID = bottomShooter.getPIDController();
bottomPID.setFeedbackDevice(bottomEncoder);
shooterPivot.setSmartCurrentLimit(50);
shooterPivot.burnFlash();
topShooter.setSmartCurrentLimit(40);
bottomShooter.setSmartCurrentLimit(40);
pivotEncoder = new Encoder(0, 1);
pivotEncoder.setDistancePerPulse(ShooterConstants.kPivotConversion);
shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel);
//TODO These can probably devolve into BangBang controllers and let the feed forwards maintain speed
topShooterPID = new PIDController(
ShooterConstants.kShooterP,
ShooterConstants.kShooterI,
ShooterConstants.kShooterD
);
bottomShooterPID = new PIDController(
ShooterConstants.kShooterP,
ShooterConstants.kShooterI,
ShooterConstants.kShooterD
);
bottomShooter.burnFlash();
shooterPivot.burnFlash();
topShooter.burnFlash();
topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
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,
@@ -83,28 +78,26 @@ public class Shooter extends SubsystemBase{
ShooterConstants.kShooterPivotD
);
shooterPivotFF = new ArmFeedforward(
ShooterConstants.kSShooterPivotFF,
ShooterConstants.kGShooterPivotFF,
ShooterConstants.kVShooterPivotFF
);
}
public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
return run(()-> {
shooterPivot.setIdleMode(IdleMode.kBrake);
angleAndSpeedControl(setpointAngle, topRPM, bottomRPM);
});
}
private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){
shooterPivot.setIdleMode(IdleMode.kBrake);
shooterPivot.setVoltage(
shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) +
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));
});
topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity);
bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity);
}
public Command recieveNote(){
public Command ampHandoff(){
return run(() -> {
shooterPivot.setIdleMode(IdleMode.kBrake);
@@ -112,12 +105,10 @@ public class Shooter extends SubsystemBase{
shooterPivotPID.calculate(pivotEncoder.getDistance(), ShooterConstants.kShooterLoadAngle) +
shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
if(shooterBeamBreak.get()){
topShooter.set(0.25);
bottomShooter.set(0.25);
if(shooterBeamBreak.get() || shooterBeamBreak.get()){
angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 1000.0, 1000.0);
}else{
topShooter.set(0.);
bottomShooter.set(0.);
angleAndSpeedControl(ShooterConstants.kShooterAmpAngle, 0, 0);
}
});
}