Skip to content

Commit de6f84e

Browse files
MyaTaheriMyaTaheri
committed
3/4/24
1 parent 678ebcc commit de6f84e

File tree

10 files changed

+269
-19
lines changed

10 files changed

+269
-19
lines changed

‎.pathplanner/settings.json‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
{
2+
"robotWidth": 0.9,
3+
"robotLength": 0.9,
4+
"holonomicMode": true,
5+
"pathFolders": [],
6+
"autoFolders": [],
7+
"defaultMaxVel": 3.0,
8+
"defaultMaxAccel": 3.0,
9+
"defaultMaxAngVel": 540.0,
10+
"defaultMaxAngAccel": 720.0,
11+
"maxModuleSpeed": 4.5
12+
}

‎src/main/deploy/pathplanner/paths/Left2NoteBlue.path‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
{
2+
"version": 1.0,
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 0.9323613212443815,
7+
"y": 6.656317475275852
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 1.9323613212443809,
12+
"y": 6.656317475275852
13+
},
14+
"isLocked": false,
15+
"linkedName": null
16+
},
17+
{
18+
"anchor": {
19+
"x": 2.6751120813030673,
20+
"y": 6.984842182244751
21+
},
22+
"prevControl": {
23+
"x": 1.6751120813030673,
24+
"y": 6.984842182244751
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": null
29+
}
30+
],
31+
"rotationTargets": [],
32+
"constraintZones": [],
33+
"eventMarkers": [
34+
{
35+
"name": "print",
36+
"waypointRelativePos": 0.75,
37+
"command": {
38+
"type": "parallel",
39+
"data": {
40+
"commands": [
41+
{
42+
"type": "named",
43+
"data": {
44+
"name": "printLeft2Note"
45+
}
46+
}
47+
]
48+
}
49+
}
50+
}
51+
],
52+
"globalConstraints": {
53+
"maxVelocity": 0.5,
54+
"maxAcceleration": 0.5,
55+
"maxAngularVelocity": 90.0,
56+
"maxAngularAcceleration": 90.0
57+
},
58+
"goalEndState": {
59+
"velocity": 0,
60+
"rotation": 180.0,
61+
"rotateFast": false
62+
},
63+
"reversed": false,
64+
"folder": null,
65+
"previewStartingState": {
66+
"rotation": -120.0,
67+
"velocity": 0
68+
},
69+
"useDefaultConstraints": false
70+
}

‎src/main/deploy/pathplanner/paths/Middle2Note.path‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
{
2+
"version": 1.0,
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 1.3233687216166703,
7+
"y": 5.562302316231041
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 2.323368721616671,
12+
"y": 5.562302316231041
13+
},
14+
"isLocked": false,
15+
"linkedName": null
16+
},
17+
{
18+
"anchor": {
19+
"x": 2.8958670517985157,
20+
"y": 5.562302316231041
21+
},
22+
"prevControl": {
23+
"x": 1.8958670517985157,
24+
"y": 5.562302316231041
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": null
29+
}
30+
],
31+
"rotationTargets": [],
32+
"constraintZones": [],
33+
"eventMarkers": [
34+
{
35+
"name": "print",
36+
"waypointRelativePos": 0.9,
37+
"command": {
38+
"type": "parallel",
39+
"data": {
40+
"commands": [
41+
{
42+
"type": "named",
43+
"data": {
44+
"name": "print"
45+
}
46+
}
47+
]
48+
}
49+
}
50+
}
51+
],
52+
"globalConstraints": {
53+
"maxVelocity": 3.0,
54+
"maxAcceleration": 3.0,
55+
"maxAngularVelocity": 540.0,
56+
"maxAngularAcceleration": 720.0
57+
},
58+
"goalEndState": {
59+
"velocity": 0,
60+
"rotation": 0,
61+
"rotateFast": false
62+
},
63+
"reversed": false,
64+
"folder": null,
65+
"previewStartingState": {
66+
"rotation": 0,
67+
"velocity": 0
68+
},
69+
"useDefaultConstraints": true
70+
}

‎src/main/deploy/pathplanner/paths/Right2NoteRed.path‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
{
2+
"version": 1.0,
3+
"waypoints": [
4+
{
5+
"anchor": {
6+
"x": 15.781810114583042,
7+
"y": 6.7769344718348545
8+
},
9+
"prevControl": null,
10+
"nextControl": {
11+
"x": 14.197917918710854,
12+
"y": 6.876687903298562
13+
},
14+
"isLocked": false,
15+
"linkedName": null
16+
},
17+
{
18+
"anchor": {
19+
"x": 13.91634883530184,
20+
"y": 6.975960595336369
21+
},
22+
"prevControl": {
23+
"x": 14.959553395669293,
24+
"y": 6.829575439284806
25+
},
26+
"nextControl": null,
27+
"isLocked": false,
28+
"linkedName": null
29+
}
30+
],
31+
"rotationTargets": [],
32+
"constraintZones": [],
33+
"eventMarkers": [
34+
{
35+
"name": "print",
36+
"waypointRelativePos": 0.55,
37+
"command": {
38+
"type": "parallel",
39+
"data": {
40+
"commands": [
41+
{
42+
"type": "named",
43+
"data": {
44+
"name": "printRight2NoteRed"
45+
}
46+
}
47+
]
48+
}
49+
}
50+
}
51+
],
52+
"globalConstraints": {
53+
"maxVelocity": 3.0,
54+
"maxAcceleration": 3.0,
55+
"maxAngularVelocity": 540.0,
56+
"maxAngularAcceleration": 720.0
57+
},
58+
"goalEndState": {
59+
"velocity": 0,
60+
"rotation": 0,
61+
"rotateFast": false
62+
},
63+
"reversed": false,
64+
"folder": null,
65+
"previewStartingState": {
66+
"rotation": -60.0,
67+
"velocity": 0
68+
},
69+
"useDefaultConstraints": false
70+
}

‎src/main/deploy/pathplanner/paths/SimpleBack.path‎

Lines changed: 22 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -3,25 +3,25 @@
33
"waypoints": [
44
{
55
"anchor": {
6-
"x": 1.7609567117282383,
7-
"y": 6.0
6+
"x": 1.0,
7+
"y": 1.0
88
},
99
"prevControl": null,
1010
"nextControl": {
11-
"x": 2.7609567117282374,
12-
"y": 6.0
11+
"x": 1.9999999999999991,
12+
"y": 1.0
1313
},
1414
"isLocked": false,
1515
"linkedName": null
1616
},
1717
{
1818
"anchor": {
19-
"x": 4.0,
20-
"y": 6.0
19+
"x": 2.0,
20+
"y": 1.0
2121
},
2222
"prevControl": {
23-
"x": 3.0,
24-
"y": 6.0
23+
"x": 1.0,
24+
"y": 1.0
2525
},
2626
"nextControl": null,
2727
"isLocked": false,
@@ -30,12 +30,23 @@
3030
],
3131
"rotationTargets": [],
3232
"constraintZones": [],
33-
"eventMarkers": [],
33+
"eventMarkers": [
34+
{
35+
"name": "New Event Marker",
36+
"waypointRelativePos": 0,
37+
"command": {
38+
"type": "parallel",
39+
"data": {
40+
"commands": []
41+
}
42+
}
43+
}
44+
],
3445
"globalConstraints": {
3546
"maxVelocity": 1.0,
3647
"maxAcceleration": 3.0,
37-
"maxAngularVelocity": 540.0,
38-
"maxAngularAcceleration": 720.0
48+
"maxAngularVelocity": 90.0,
49+
"maxAngularAcceleration": 90.0
3950
},
4051
"goalEndState": {
4152
"velocity": 0,

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -77,7 +77,7 @@ public final static class DriverConstants {
7777
}
7878

7979
public final static class WheelConstants {
80-
public final static double ROTATIONS_TO_METERS = 0.33/8.33;
80+
public final static double ROTATIONS_TO_METERS = 0.33/8.33 * 1.17;
8181
}
8282

8383
public enum LimelightDirections {

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -63,9 +63,9 @@ public void disabledPeriodic() {}
6363
@Override
6464
public void autonomousInit() {
6565
System.out.println("STARTING AUTO");
66-
// m_robotContainer.getOdometry().zeroHeading();
67-
// m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
68-
// m_robotContainer.getSwerveDriveSubsystem().resetLockRot();
66+
m_robotContainer.getOdometry().zeroHeading();
67+
m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
68+
m_robotContainer.getSwerveDriveSubsystem().resetLockRot();
6969
System.out.println("GET AUTO");
7070
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
7171
System.out.println("AUTO GOT!");
@@ -89,6 +89,7 @@ public void teleopInit() {
8989
// this line or comment it out.
9090
m_robotContainer.getOdometry().zeroHeading();
9191
m_robotContainer.getSwerveDriveSubsystem().resetLockRot();
92+
m_robotContainer.getOdometry().reset(new Pose2d(0, 0, Rotation2d.fromDegrees(0)));
9293
if (m_autonomousCommand != null) {
9394
m_autonomousCommand.cancel();
9495
}

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
import frc.robot.subsystems.MaxWheelModule;
2020
import frc.robot.subsystems.PivotSubsystem;
2121
import frc.robot.subsystems.ShooterSubsystem;
22+
2223
import com.kauailabs.navx.frc.AHRS;
2324
import com.revrobotics.CANSparkMax;
2425
import com.revrobotics.CANSparkBase.IdleMode;
@@ -34,6 +35,7 @@
3435
import edu.wpi.first.wpilibj.SPI;
3536
import edu.wpi.first.wpilibj2.command.Command;
3637
import edu.wpi.first.wpilibj2.command.InstantCommand;
38+
import edu.wpi.first.wpilibj2.command.PrintCommand;
3739
import edu.wpi.first.wpilibj2.command.RunCommand;
3840
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
3941

@@ -120,7 +122,7 @@ public class RobotContainer {
120122
private final AlignByAprilTag alignLeftOfSpeaker = new AlignByAprilTag(swerveDrive, limelight, odometry, -1.08, -0.96, 1, 0.04, 0.1, -44, -4);
121123

122124
private final ChargeCommand chargeCommand;
123-
125+
124126
SlewRateLimiter limitX = new SlewRateLimiter(6);
125127
SlewRateLimiter limitY = new SlewRateLimiter(6);
126128
//Limits shooter motor speed
@@ -134,7 +136,10 @@ public RobotContainer(RobotBase robot) {
134136

135137
CameraServer.startAutomaticCapture();
136138

137-
NamedCommands.registerCommand("toggleFieldCentric", swerveDrive.toggleFieldCentric());
139+
NamedCommands.registerCommand("print", new PrintCommand("print!"));
140+
NamedCommands.registerCommand("printRight2NoteRed", new PrintCommand("righ2notered path!"));
141+
NamedCommands.registerCommand("printLeft2Note", new PrintCommand("left2note path!"));
142+
138143

139144
// Configure the trigger bindings
140145
configureBindings();

‎src/main/java/frc/robot/commands/autonomous/ChargeCommand.java‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,14 +20,15 @@
2020
/** An example command that uses an example subsystem. */
2121
public class ChargeCommand extends SequentialCommandGroup {
2222

23-
private String TRAJECTORY_NAME = "SimpleBack";
23+
private String TRAJECTORY_NAME = "Middle2Note";
2424
SwerveDriveSubsystem swerveDrive;
2525
FeedSubsystem feedSubsystem;
2626

2727
public ChargeCommand(SwerveDriveSubsystem swerveDrive, FeedSubsystem feedSubsystem) {
2828
this.swerveDrive = swerveDrive;
2929
this.feedSubsystem = feedSubsystem;
3030

31+
3132
addRequirements(swerveDrive, feedSubsystem);
3233

3334
addCommands(

0 commit comments

Comments
 (0)