Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

cummit

Browse Source
This commit is contained in:
aarav18
2023-02-09 18:25:57 -07:00
parent 4900c397c4
commit 5a8af1d071
3 changed files with 17 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+13 -11
View File
@@ -10,6 +10,7 @@ package frc4388.robot;
import java.util.ArrayList;
import java.util.List;
import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
@@ -113,20 +114,20 @@ public class RobotContainer {
SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics());
// generate trajectory
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
// Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, new Rotation2d(0)),
List.of(
new Translation2d(0, 1)),
new Pose2d(0, 2, Rotation2d.fromDegrees(0)),
// new Pose2d(0, 0, new Rotation2d(0)),
// List.of(
// new Translation2d(0, 1)),
// new Pose2d(0, 2, Rotation2d.fromDegrees(0)),
trajectoryConfig);
// trajectoryConfig);
// ArrayList<Pose2d> simplePath = new ArrayList<Pose2d>();
// simplePath.add(new Pose2d(0, 0, new Rotation2d(0)));
// simplePath.add(new Pose2d(0, 1, new Rotation2d(0)));
ArrayList<Pose2d> simplePath = new ArrayList<Pose2d>();
simplePath.add(new Pose2d(0, 0, new Rotation2d(0)));
simplePath.add(new Pose2d(0, 1, new Rotation2d(0)));
// Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig);
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig);
//Defining PID Controller for tracking trajectory
PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0);
@@ -148,7 +149,8 @@ public class RobotContainer {
//Init and wrap-up, return everything
return new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive),
// new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())),
swerveControllerCommand,
new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
}