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": [],
"autoFolders": [],
"defaultMaxVel": 3.0,
"defaultMaxAccel": 3.0,
"defaultMaxAccel": 4.0,
"defaultMaxAngVel": 540.0,
"defaultMaxAngAccel": 720.0,
"maxModuleSpeed": 4.03

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -33,7 +33,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 3.0,
"maxAcceleration": 3.0,
"maxAcceleration": 4.0,
"maxAngularVelocity": 540.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.PrintCommand;
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.Trigger;
import frc.robot.Commands.autoIntaking;
@ -111,6 +112,7 @@ public class RobotContainer {
).withTimeout(2.0)
);
/*
NamedCommands.registerCommand(
"Speaker Note Shot",
Commands.parallel(
@ -120,10 +122,29 @@ public class RobotContainer {
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(
"Auto Intake",
Commands.parallel(
@ -132,7 +153,22 @@ public class RobotContainer {
() -> 1.0
),
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"));

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.PIDController;
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.SubsystemBase;
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(){
return Units.rotationsToRadians(intakeEncoder.getPosition()/IntakeConstants.kIntakePivotConversionFactor)-armOffset;
}