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

4 note testing with shooting and intaking

Browse Source
This commit is contained in:
Bradley Bickford 2024-03-14 17:29:33 -04:00
parent 97938bfea2
commit d15e9ff14d
26 changed files with 85 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.pathplanner/settings.json
View File

@ -5,7 +5,7 @@
"pathFolders": [], "pathFolders": [],
"autoFolders": [], "autoFolders": [],
"defaultMaxVel": 3.0, "defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0, "defaultMaxAccel": 4.0,
"defaultMaxAngVel": 540.0, "defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0, "defaultMaxAngAccel": 720.0,
"maxModuleSpeed": 4.03 "maxModuleSpeed": 4.03

2
src/main/deploy/pathplanner/paths/Center Subwoofer to Center.path
View File

@ -55,7 +55,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/From Subwoofer Far Edge to Amp.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/From Subwoofer Front to Amp.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/He He He Haw.path
View File

@ -55,7 +55,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Just Backup.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

6
src/main/deploy/pathplanner/paths/L Sub to Note 1.path
View File

@ -16,11 +16,11 @@
}, },
{ {
"anchor": { "anchor": {
"x": 2.4114851648506717, "x": 2.7506156436814293,
"y": 7.016492665463964 "y": 7.016492665463964
}, },
"prevControl": { "prevControl": {
"x": 1.943718987153074, "x": 2.2828494659838316,
"y": 7.004798511021525 "y": 7.004798511021525
}, },
"nextControl": null, "nextControl": null,
@ -50,7 +50,7 @@
], ],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

14
src/main/deploy/pathplanner/paths/L Sub to Note 2.path
View File

@ -16,12 +16,12 @@
}, },
{ {
"anchor": { "anchor": {
"x": 2.5284267092750707, "x": 2.809086415893629,
"y": 5.671664904583371 "y": 5.566417514601411
}, },
"prevControl": { "prevControl": {
"x": 2.1659079215594326, "x": 2.446567628177991,
"y": 6.057572001183889 "y": 5.952324611201929
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -31,7 +31,7 @@
"rotationTargets": [ "rotationTargets": [
{ {
"waypointRelativePos": 0.7, "waypointRelativePos": 0.7,
"rotationDegrees": 157.35591349901955, "rotationDegrees": 136.8500052941468,
"rotateFast": true "rotateFast": true
} }
], ],
@ -39,13 +39,13 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 159.0754982550787, "rotation": 139.08561677997477,
"rotateFast": false "rotateFast": false
}, },
"reversed": false, "reversed": false,

2
src/main/deploy/pathplanner/paths/Left Note 3 to Sub.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Left Preloaded+Pickup.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

12
src/main/deploy/pathplanner/paths/Left Sub to Note 3.path
View File

@ -16,12 +16,12 @@
}, },
{ {
"anchor": { "anchor": {
"x": 2.388096855965791, "x": 2.4348734737355513,
"y": 4.303448834817898 "y": 4.139730672623739
}, },
"prevControl": { "prevControl": {
"x": 0.867856778448599, "x": 0.9146333962183593,
"y": 4.502249460339377 "y": 4.338531298145218
}, },
"nextControl": null, "nextControl": null,
"isLocked": false, "isLocked": false,
@ -33,13 +33,13 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": 157.9772276319205, "rotation": 171.86989764584402,
"rotateFast": false "rotateFast": false
}, },
"reversed": false, "reversed": false,

4
src/main/deploy/pathplanner/paths/Left Subwoofer.path
View File

@ -33,13 +33,13 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },
"goalEndState": { "goalEndState": {
"velocity": 0, "velocity": 0,
"rotation": -141.11550356628527, "rotation": -151.50436138175493,
"rotateFast": false "rotateFast": false
}, },
"reversed": false, "reversed": false,

2
src/main/deploy/pathplanner/paths/Leftmost Center Note.path
View File

@ -83,7 +83,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Note 1 to Speaker.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Note 1 to amp.path
View File

@ -44,7 +44,7 @@
], ],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Note 2 to L Sub (4S).path
View File

@ -39,7 +39,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Note 2 to L Sub.path
View File

@ -39,7 +39,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Note 3 to Sub.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Right Sub Note 3.path
View File

@ -39,7 +39,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Right Sub from Center.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Right Sub to Right Center Note.path
View File

@ -57,7 +57,7 @@
], ],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Right Sub.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Right Subwoofer to Center.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

2
src/main/deploy/pathplanner/paths/Right Wall to Sub.path
View File

@ -33,7 +33,7 @@
"eventMarkers": [], "eventMarkers": [],
"globalConstraints": { "globalConstraints": {
"maxVelocity": 3.0, "maxVelocity": 3.0,
"maxAcceleration": 3.0, "maxAcceleration": 4.0,
"maxAngularVelocity": 540.0, "maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0 "maxAngularAcceleration": 720.0
}, },

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

@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Commands.autoIntaking; import frc.robot.Commands.autoIntaking;
@ -111,6 +112,7 @@ public class RobotContainer {
).withTimeout(2.0) ).withTimeout(2.0)
); );
/*
NamedCommands.registerCommand( NamedCommands.registerCommand(
"Speaker Note Shot", "Speaker Note Shot",
Commands.parallel( Commands.parallel(
@ -120,10 +122,29 @@ public class RobotContainer {
1.0 1.0
), ),
indexer.shootNote(() -> 1.0) indexer.shootNote(() -> 1.0)
).withTimeout(2.0) ).withTimeout(1.0)
);
*/
NamedCommands.registerCommand(
"Speaker Note Shot",
shooter.angleSpeedsSetpoints(
() -> ShooterConstants.kShooterLoadAngle,
1.0,
1.0
).raceWith(
new WaitCommand(0.5)
).andThen(
shooter.angleSpeedsSetpoints(
() -> ShooterConstants.kShooterLoadAngle,
1.0,
1.0
).alongWith(indexer.shootNote(() -> 1.0))
.withTimeout(1.0)
)
); );
/*
NamedCommands.registerCommand( NamedCommands.registerCommand(
"Auto Intake", "Auto Intake",
Commands.parallel( Commands.parallel(
@ -132,7 +153,22 @@ public class RobotContainer {
() -> 1.0 () -> 1.0
), ),
indexer.advanceNote() indexer.advanceNote()
).withTimeout(3.0)//.until(indexer::getBeamBreak) ).until(() -> !indexer.getBeamBreak())
.withTimeout(1.0)
);*/
NamedCommands.registerCommand(
"Auto Intake",
intake.intakeControl(
() -> IntakeConstants.kDownAngle,
() -> 1.0
).alongWith(
indexer.advanceNote()
).until(() -> !indexer.getBeamBreak())
.andThen(
intake.stopAll()
)
//.withTimeout(1.0)
); );
//NamedCommands.registerCommand("Auto Intake", new PrintCommand("Intake Note")); //NamedCommands.registerCommand("Auto Intake", new PrintCommand("Intake Note"));

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

@ -10,6 +10,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.util.Units; import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.function.BooleanConsumer;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.constants.IntakeConstants; import frc.robot.constants.IntakeConstants;
@ -154,6 +155,13 @@ public class Intake extends SubsystemBase{
}); });
} }
public Command stopAll() {
return runOnce(() -> {
intakeRoller.set(0);
intakePivot.setVoltage(0);
});
}
public double getIntakeAngle(){ public double getIntakeAngle(){
return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeConstants.kIntakePivotConversionFactor)-armOffset; return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeConstants.kIntakePivotConversionFactor)-armOffset;
} }