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

auto incomplete

Browse Source
This commit is contained in:
Abhi Sachi
2023-02-09 18:48:43 -07:00
parent 5a8af1d071
commit d80b4faccb
1 changed files with 14 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+14 -4
View File
@@ -10,6 +10,8 @@ package frc4388.robot;
import java.util.ArrayList;
import java.util.List;
import org.opencv.objdetect.HOGDescriptor;
import edu.wpi.first.math.controller.HolonomicDriveController;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -21,6 +23,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.math.trajectory.Trajectory.State;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -125,10 +128,17 @@ public class RobotContainer {
ArrayList<Pose2d> simplePath = new ArrayList<Pose2d>();
simplePath.add(new Pose2d(0, 0, new Rotation2d(0)));
simplePath.add(new Pose2d(0, 1, new Rotation2d(0)));
simplePath.add(new Pose2d(0, -1, new Rotation2d(0)));
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig);
ArrayList<double[]> states = new ArrayList<double[]>();
for(double i = 0; i <=5.0; i += 0.1) {
Trajectory.State state = trajectory.sample(i);
states.add(new double[] {state.velocityMetersPerSecond, state.curvatureRadPerMeter});
}
//Defining PID Controller for tracking trajectory
PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0);
PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.Y_CONTROLLER.kP, 0, 0);
@@ -136,14 +146,14 @@ public class RobotContainer {
AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS);
thetaController.enableContinuousInput(-Math.PI, Math.PI);
HolonomicDriveController holoController = new HolonomicDriveController(xController, yController, thetaController);
//Command to follow trajectory
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
trajectory,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.getKinematics(),
xController,
yController,
thetaController,
holoController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive);