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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
.pathplanner/settings.json Normal file
View File

@ -0,0 +1,12 @@
{
"robotWidth": 0.88,
"robotLength": 0.88,
"holonomicMode": true,
"pathFolders": [],
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"maxModuleSpeed": 4.03
}

74
src/main/deploy/pathplanner/autos/Right 1S 2A.auto Normal file
View File

@ -0,0 +1,74 @@
{
"version": 1.0,
"startingPose": {
"position": {
"x": 1.1134340217398382,
"y": 6.525338178881486
},
"rotation": 42.79740183823424
},
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Speaker Note Shot"
}
},
{
"type": "path",
"data": {
"pathName": "Preloaded+Pickup"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "wait",
"data": {
"waitTime": 2.0
}
},
{
"type": "named",
"data": {
"name": "Amp Handoff"
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "Note 1 to amp"
}
},
{
"type": "named",
"data": {
"name": "Amp Note Shot"
}
},
{
"type": "path",
"data": {
"pathName": "Leftmost Center Note"
}
},
{
"type": "path",
"data": {
"pathName": "Note 1 to amp"
}
}
]
}
},
"folder": null,
"choreoAuto": false
}

50
src/main/deploy/pathplanner/autos/Right 2 Amp.auto Normal file
View File

@ -0,0 +1,50 @@
{
"version": 1.0,
"startingPose": null,
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Speaker Note Shot"
}
},
{
"type": "path",
"data": {
"pathName": "Preloaded+Pickup"
}
},
{
"type": "deadline",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "Amp Handoff"
}
},
{
"type": "path",
"data": {
"pathName": "Note 1 to amp"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "Amp Note Shot"
}
}
]
}
},
"folder": null,
"choreoAuto": false
}

43
src/main/deploy/pathplanner/autos/Right 3 Amp.auto Normal file
View File

@ -0,0 +1,43 @@
{
"version": 1.0,
"startingPose": {
"position": {
"x": 1.1134340217398382,
"y": 6.525338178881486
},
"rotation": 33.13560954871888
},
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Preloaded+Pickup"
}
},
{
"type": "path",
"data": {
"pathName": "Note 1 to amp"
}
},
{
"type": "path",
"data": {
"pathName": "Leftmost Center Note"
}
},
{
"type": "path",
"data": {
"pathName": "Note 1 to amp"
}
}
]
}
},
"folder": null,
"choreoAuto": false
}

1
src/main/deploy/pathplanner/navgrid.json Normal file
View File

File diff suppressed because one or more lines are too long

102
src/main/deploy/pathplanner/paths/Leftmost Center Note.path Normal file
View File

@ -0,0 +1,102 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 3.347017520241028,
"y": 6.993104356575341
},
"prevControl": null,
"nextControl": {
"x": 4.446268037830382,
"y": 7.110045900999739
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.071755505338439,
"y": 7.297152372084174
},
"prevControl": {
"x": 5.077752377722138,
"y": 7.262069908751458
},
"nextControl": {
"x": 6.725659674531382,
"y": 7.320231342764915
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.63877220062539,
"y": 7.425788070951012
},
"prevControl": {
"x": 6.621380764133118,
"y": 7.460870534278332
},
"nextControl": {
"x": 8.656163637117663,
"y": 7.390705607623692
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.545818145767345,
"y": 6.642279723305886
},
"prevControl": {
"x": 6.323179825848877,
"y": 7.12758713266797
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 1,
"rotationDegrees": 172.50414236027015,
"rotateFast": false
}
],
"constraintZones": [
{
"name": "New Constraints Zone",
"minWaypointRelativePos": 0.85,
"maxWaypointRelativePos": 1.35,
"constraints": {
"maxVelocity": 1.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
}
}
],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 1.5,
"rotation": -179.23610153906992,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": 0,
"velocity": 0
},
"useDefaultConstraints": true
}

52
src/main/deploy/pathplanner/paths/Note 1 to Speaker.path Normal file
View File

@ -0,0 +1,52 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 2.0,
"y": 7.0
},
"prevControl": null,
"nextControl": {
"x": 3.0,
"y": 7.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.2654580294915574,
"y": 6.291455090032687
},
"prevControl": {
"x": 1.7098358983042754,
"y": 6.525338178881486
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": -145.88552705465875,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": 179.55484341368705,
"velocity": 0
},
"useDefaultConstraints": true
}

63
src/main/deploy/pathplanner/paths/Note 1 to amp.path Normal file
View File

@ -0,0 +1,63 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 2.353014392638472,
"y": 6.993104356579084
},
"prevControl": null,
"nextControl": {
"x": 1.768306670516475,
"y": 7.004798511021523
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.8384715971711143,
"y": 7.624588696470841
},
"prevControl": {
"x": 1.7950792079196904,
"y": 7.315863019194643
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [
{
"name": "New Event Marker",
"waypointRelativePos": 0,
"command": {
"type": "parallel",
"data": {
"commands": []
}
}
}
],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0.0,
"rotation": 90.0,
"rotateFast": true
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": -178.36342295838335,
"velocity": 0
},
"useDefaultConstraints": true
}

52
src/main/deploy/pathplanner/paths/Preloaded+Pickup.path Normal file
View File

@ -0,0 +1,52 @@
{
"version": 1.0,
"waypoints": [
{
"anchor": {
"x": 1.1134340217398382,
"y": 6.525338178881486
},
"prevControl": null,
"nextControl": {
"x": 1.148516485067158,
"y": 7.191904982100564
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.3179319293111518,
"y": 6.981410202136645
},
"prevControl": {
"x": 1.8033891338437944,
"y": 6.981410202136645
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0
},
"goalEndState": {
"velocity": 0,
"rotation": 0,
"rotateFast": false
},
"reversed": false,
"folder": null,
"previewStartingState": {
"rotation": 40.97173633351488,
"velocity": 0
},
"useDefaultConstraints": true
}

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);
}
});
}

6
vendordeps/PathplannerLib.json
View File

@ -1,7 +1,7 @@
{
"fileName": "PathplannerLib.json",
"name": "PathplannerLib",
"version": "2024.1.2",
"version": "2024.2.3",
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
"frcYear": "2024",
"mavenUrls": [
@ -12,7 +12,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-java",
"version": "2024.1.2"
"version": "2024.2.3"
}
],
"jniDependencies": [],
@ -20,7 +20,7 @@
{
"groupId": "com.pathplanner.lib",
"artifactId": "PathplannerLib-cpp",
"version": "2024.1.2",
"version": "2024.2.3",
"libName": "PathplannerLib",
"headerClassifier": "headers",
"sharedLibrary": false,

10
vendordeps/REVLib.json
View File

@ -1,7 +1,7 @@
{
"fileName": "REVLib.json",
"name": "REVLib",
"version": "2024.2.0",
"version": "2024.2.1",
"frcYear": "2024",
"uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
"mavenUrls": [
@ -12,14 +12,14 @@
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-java",
"version": "2024.2.0"
"version": "2024.2.1"
}
],
"jniDependencies": [
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-driver",
"version": "2024.2.0",
"version": "2024.2.1",
"skipInvalidPlatforms": true,
"isJar": false,
"validPlatforms": [
@ -37,7 +37,7 @@
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-cpp",
"version": "2024.2.0",
"version": "2024.2.1",
"libName": "REVLib",
"headerClassifier": "headers",
"sharedLibrary": false,
@ -55,7 +55,7 @@
{
"groupId": "com.revrobotics.frc",
"artifactId": "REVLib-driver",
"version": "2024.2.0",
"version": "2024.2.1",
"libName": "REVLibDriver",
"headerClassifier": "headers",
"sharedLibrary": false,