Skip to content

Commit d78826c

Browse files
ArnavGuptaOfficialArnavGuptaOfficial
committed
Quick Commit So I don't Lose my progress
Quick Commit So I don't Lose my progress
1 parent e76e9a3 commit d78826c

20 files changed

Lines changed: 641 additions & 605 deletions

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

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@
3838
import frc.robot.commands.AimbotAuto;
3939
import frc.robot.commands.AimbotStatic;
4040
import frc.robot.commands.AimbotTele;
41-
import frc.robot.commands.AlignToNoteAuto;
41+
//import frc.robot.commands.AlignToNoteAuto;
4242
import frc.robot.commands.AngleShooter;
4343
// import frc.robot.commands.AngleShooterShoot;
4444
import frc.robot.commands.DriveCommands;
@@ -65,20 +65,20 @@
6565
import frc.robot.subsystems.drive.GyroIO;
6666
import frc.robot.subsystems.drive.GyroIOPigeon2;
6767
import frc.robot.subsystems.drive.ModuleIO;
68-
import frc.robot.subsystems.drive.ModuleIOSim;
68+
//import frc.robot.subsystems.drive.ModuleIOSim;
6969
import frc.robot.subsystems.drive.ModuleIOTalonFX;
7070
import frc.robot.subsystems.drive.VisionIO;
7171
import frc.robot.subsystems.drive.VisionIOLimelight;
7272
import frc.robot.subsystems.elevator.AmpBarIO;
7373
import frc.robot.subsystems.elevator.AmpBarIOSIm;
74-
import frc.robot.subsystems.elevator.AmpBarIOSparkMAX;
74+
//import frc.robot.subsystems.elevator.AmpBarIOSparkMAX;
7575
import frc.robot.subsystems.elevator.Elevator;
7676
import frc.robot.subsystems.elevator.ElevatorIO;
7777
import frc.robot.subsystems.elevator.ElevatorIOSim;
7878
import frc.robot.subsystems.elevator.ElevatorIOTalonFX;
7979
import frc.robot.subsystems.intake.Intake;
80-
import frc.robot.subsystems.intake.IntakeRollerIOSim;
81-
import frc.robot.subsystems.intake.IntakeRollerIOSparkFlex;
80+
//import frc.robot.subsystems.intake.IntakeRollerIOSim;
81+
//import frc.robot.subsystems.intake.IntakeRollerIOSparkFlex;
8282
import frc.robot.subsystems.led.LED;
8383
import frc.robot.subsystems.led.LED_IO;
8484
import frc.robot.subsystems.led.LED_IOCANdle;

‎src/main/java/frc/robot/commands/AlignToAmp.java‎

Lines changed: 98 additions & 98 deletions
Original file line numberDiff line numberDiff line change
@@ -1,113 +1,113 @@
1-
// Copyright (c) FIRST and other WPILib contributors.
2-
// Open Source Software; you can modify and/or share it under the terms of
3-
// the WPILib BSD license file in the root directory of this project.
1+
// // Copyright (c) FIRST and other WPILib contributors.
2+
// // Open Source Software; you can modify and/or share it under the terms of
3+
// // the WPILib BSD license file in the root directory of this project.
44

5-
package frc.robot.commands;
5+
// package frc.robot.commands;
66

7-
import com.pathplanner.lib.auto.AutoBuilder;
8-
import com.pathplanner.lib.path.GoalEndState;
9-
import com.pathplanner.lib.path.PathConstraints;
10-
import com.pathplanner.lib.path.PathPlannerPath;
11-
import edu.wpi.first.math.geometry.Pose2d;
12-
import edu.wpi.first.math.geometry.Rotation2d;
13-
import edu.wpi.first.math.geometry.Translation2d;
14-
import edu.wpi.first.math.util.Units;
15-
import edu.wpi.first.wpilibj.DriverStation;
16-
import edu.wpi.first.wpilibj2.command.Command;
17-
import frc.robot.subsystems.drive.Drive;
18-
import frc.robot.util.FieldConstants;
19-
import java.util.List;
7+
// import com.pathplanner.lib.auto.AutoBuilder;
8+
// import com.pathplanner.lib.path.GoalEndState;
9+
// import com.pathplanner.lib.path.PathConstraints;
10+
// import com.pathplanner.lib.path.PathPlannerPath;
11+
// import edu.wpi.first.math.geometry.Pose2d;
12+
// import edu.wpi.first.math.geometry.Rotation2d;
13+
// import edu.wpi.first.math.geometry.Translation2d;
14+
// import edu.wpi.first.math.util.Units;
15+
// import edu.wpi.first.wpilibj.DriverStation;
16+
// import edu.wpi.first.wpilibj2.command.Command;
17+
// import frc.robot.subsystems.drive.Drive;
18+
// import frc.robot.util.FieldConstants;
19+
// import java.util.List;
2020

21-
public class AlignToAmp extends Command {
22-
private final Drive drive;
21+
// public class AlignToAmp extends Command {
22+
// private final Drive drive;
2323

24-
private List<Translation2d> pointsToAmp;
25-
private Command pathCommand;
26-
private PathPlannerPath path;
24+
// private List<Translation2d> pointsToAmp;
25+
// private Command pathCommand;
26+
// private PathPlannerPath path;
2727

28-
private Rotation2d targetRotation = new Rotation2d();
29-
/** Creates a new AlignToAmp. */
30-
public AlignToAmp(Drive drive) {
31-
// Use addRequirements() here to declare subsystem dependencies.
32-
this.drive = drive;
28+
// private Rotation2d targetRotation = new Rotation2d();
29+
// /** Creates a new AlignToAmp. */
30+
// public AlignToAmp(Drive drive) {
31+
// // Use addRequirements() here to declare subsystem dependencies.
32+
// this.drive = drive;
3333

34-
addRequirements(drive);
34+
// addRequirements(drive);
3535

36-
targetRotation = drive.getRotation();
37-
}
36+
// targetRotation = drive.getRotation();
37+
// }
3838

39-
// Called when the command is initially scheduled.
40-
@Override
41-
public void initialize() {
42-
if (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) {
43-
targetRotation =
44-
new Rotation2d(
45-
(Units.degreesToRadians(
46-
new Rotation2d(FieldConstants.ampCenter.getX(), FieldConstants.ampCenter.getY())
47-
.getDegrees()
48-
+ 13)));
49-
pointsToAmp =
50-
PathPlannerPath.bezierFromPoses(
51-
new Pose2d(
52-
drive.getPose().getX(), drive.getPose().getY(), drive.getPose().getRotation()),
53-
new Pose2d(
54-
FieldConstants.ampCenter.getX(),
55-
FieldConstants.ampCenter.getY() - 0.5,
56-
targetRotation));
57-
path =
58-
new PathPlannerPath(
59-
pointsToAmp,
60-
new PathConstraints(3, 3, Units.degreesToRadians(540), Units.degreesToRadians(720)),
61-
new GoalEndState(0, targetRotation, true));
62-
} else {
63-
targetRotation =
64-
new Rotation2d(
65-
Units.degreesToRadians(
66-
new Rotation2d(
67-
FieldConstants.fieldLength - FieldConstants.ampCenter.getX(),
68-
FieldConstants.ampCenter.getY())
69-
.getDegrees()
70-
+ 62));
39+
// // Called when the command is initially scheduled.
40+
// @Override
41+
// public void initialize() {
42+
// if (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) {
43+
// targetRotation =
44+
// new Rotation2d(
45+
// (Units.degreesToRadians(
46+
// new Rotation2d(FieldConstants.ampCenter.getX(), FieldConstants.ampCenter.getY())
47+
// .getDegrees()
48+
// + 13)));
49+
// pointsToAmp =
50+
// PathPlannerPath.bezierFromPoses(
51+
// new Pose2d(
52+
// drive.getPose().getX(), drive.getPose().getY(), drive.getPose().getRotation()),
53+
// new Pose2d(
54+
// FieldConstants.ampCenter.getX(),
55+
// FieldConstants.ampCenter.getY() - 0.5,
56+
// targetRotation));
57+
// path =
58+
// new PathPlannerPath(
59+
// pointsToAmp,
60+
// new PathConstraints(3, 3, Units.degreesToRadians(540), Units.degreesToRadians(720)),
61+
// new GoalEndState(0, targetRotation, true));
62+
// } else {
63+
// targetRotation =
64+
// new Rotation2d(
65+
// Units.degreesToRadians(
66+
// new Rotation2d(
67+
// FieldConstants.fieldLength - FieldConstants.ampCenter.getX(),
68+
// FieldConstants.ampCenter.getY())
69+
// .getDegrees()
70+
// + 62));
7171

72-
pointsToAmp =
73-
PathPlannerPath.bezierFromPoses(
74-
new Pose2d(
75-
FieldConstants.fieldLength - drive.getPose().getX(),
76-
drive.getPose().getY(),
77-
drive.getPose().getRotation()),
78-
new Pose2d(
79-
FieldConstants.fieldLength - FieldConstants.ampCenter.getX(),
80-
FieldConstants.ampCenter.getY() - 0.5,
81-
targetRotation));
72+
// pointsToAmp =
73+
// PathPlannerPath.bezierFromPoses(
74+
// new Pose2d(
75+
// FieldConstants.fieldLength - drive.getPose().getX(),
76+
// drive.getPose().getY(),
77+
// drive.getPose().getRotation()),
78+
// new Pose2d(
79+
// FieldConstants.fieldLength - FieldConstants.ampCenter.getX(),
80+
// FieldConstants.ampCenter.getY() - 0.5,
81+
// targetRotation));
8282

83-
path =
84-
new PathPlannerPath(
85-
pointsToAmp,
86-
new PathConstraints(3, 3, Units.degreesToRadians(540), Units.degreesToRadians(720)),
87-
new GoalEndState(0, targetRotation, true));
88-
}
83+
// path =
84+
// new PathPlannerPath(
85+
// pointsToAmp,
86+
// new PathConstraints(3, 3, Units.degreesToRadians(540), Units.degreesToRadians(720)),
87+
// new GoalEndState(0, targetRotation, true));
88+
// }
8989

90-
path.preventFlipping = true;
91-
pathCommand = AutoBuilder.followPath(path);
90+
// path.preventFlipping = true;
91+
// pathCommand = AutoBuilder.followPath(path);
9292

93-
pathCommand.initialize();
94-
}
93+
// pathCommand.initialize();
94+
// }
9595

96-
// Called every time the scheduler runs while the command is scheduled.
97-
@Override
98-
public void execute() {
99-
pathCommand.execute();
100-
}
96+
// // Called every time the scheduler runs while the command is scheduled.
97+
// @Override
98+
// public void execute() {
99+
// pathCommand.execute();
100+
// }
101101

102-
// Called once the command ends or is interrupted.
103-
@Override
104-
public void end(boolean interrupted) {
105-
pathCommand.cancel();
106-
}
102+
// // Called once the command ends or is interrupted.
103+
// @Override
104+
// public void end(boolean interrupted) {
105+
// pathCommand.cancel();
106+
// }
107107

108-
// Returns true when the command should end.
109-
@Override
110-
public boolean isFinished() {
111-
return false;
112-
}
113-
}
108+
// // Returns true when the command should end.
109+
// @Override
110+
// public boolean isFinished() {
111+
// return false;
112+
// }
113+
// }

0 commit comments

Comments
 (0)