Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

autos work

Browse Source
This commit is contained in:
aarav18
2022-02-04 19:02:41 -07:00
parent e9ab948adc
commit dd30f1dc6f
3 changed files with 45 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/First Test Path.path
+1 -1
View File
@@ -1 +1 @@
{"waypoints":[{"anchorPoint":{"x":2.9323920716806944,"y":3.0374957301638745},"prevControl":null,"nextControl":{"x":3.342296339765092,"y":3.0164749984672383},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.351291461203611,"y":4.340781095355294},"prevControl":{"x":4.382822558748565,"y":4.172615241782208},"nextControl":{"x":4.382822558748565,"y":4.172615241782208},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.0539707286311115,"y":3.1110682911020993},"prevControl":{"x":5.541896945862856,"y":3.0644810944794307},"nextControl":{"x":6.566044511399367,"y":3.157655487724768},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.002934143799321,"y":1.8077829259106803},"prevControl":{"x":5.189186585269721,"y":2.027791159030595},"nextControl":null,"holonomicAngle":89.09777883846981,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
{"waypoints":[{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":null,"nextControl":{"x":3.410442907302426,"y":3.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":4.5},"prevControl":{"x":4.288229266113705,"y":4.529967680625016},"nextControl":{"x":4.67537248192198,"y":4.475183036710464},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.0,"y":3.0},"prevControl":{"x":5.973638727554094,"y":3.050086417647222},"nextControl":{"x":6.10510365848318,"y":2.8003030488819602},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":1.5029823163094613},"prevControl":{"x":4.6862524414704,"y":1.722990549429376},"nextControl":{"x":4.3137475585296,"y":1.2829740831895466},"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":{"x":3.5480060960121915,"y":2.4748364324314855},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null}
src/main/deploy/pathplanner/generatedJSON/First Test Path.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/java/frc4388/robot/RobotContainer.java
+43 -29
View File
@@ -8,6 +8,10 @@ package frc4388.robot;
import java.util.List;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
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;
@@ -107,6 +111,14 @@ public class RobotContainer {
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
}
public void configAuto(boolean pathplanner) {
}
public void configAuto() {
configAuto(true);
}
/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
@@ -115,21 +127,6 @@ public class RobotContainer {
public Command getAutonomousCommand() {
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
// PATH PLANNER TRAJECTORY IMPLEMENTATION
// PathPlannerTrajectory testFirstPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0);
// PPSwerveControllerCommand command = new PPSwerveControllerCommand(
// testFirstPath,
// pose,
// SwerveDrive.m_kinematics,
// new PIDController(0.5, 0, 0),
// new PIDController(0.5, 0, 0),
// new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(1.0, 1.0)),
// outputModuleStates,
// m_robotSwerveDrive
// );
// WPILIB TRAJECTORY IMPLEMENTATION
TrajectoryConfig config = new TrajectoryConfig(1.0, 1.0)
.setKinematics(m_robotSwerveDrive.m_kinematics);
Trajectory exTraj = TrajectoryGenerator.generateTrajectory(
@@ -138,21 +135,19 @@ public class RobotContainer {
new Pose2d(1, 0, new Rotation2d(0)),
config);
Trajectory testFirstPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0);
Trajectory firstTestPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0);
Trajectory moveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0);
Trajectory rotate = PathPlanner.loadPath("Rotate", 1.0, 1.0);
Trajectory currentPath = rotate; //Change this to change auto
Trajectory currentPath = moveForward; // change this to change auto
PIDController xController = new PIDController(1.5, 0.0, 0.0);
PIDController yController = new PIDController(1.5, 0.0, 0.0);
PIDController xController = new PIDController(10.0, 0.0, 0.0);
PIDController yController = new PIDController(1.3, 0.0, 0.0);
ProfiledPIDController thetaController = new ProfiledPIDController(
3.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI / 5, Math.PI / 4));
10.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI));
thetaController.enableContinuousInput(-Math.PI, Math.PI);
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
// testFirstPath,
// moveForward,
currentPath,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
@@ -162,17 +157,36 @@ public class RobotContainer {
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive
);
// m_robotSwerveDrive.m_gyro.reset();
// m_robotSwerveDrive.resetOdometry(testFirstPath.getInitialPose());
PathPlannerTrajectory ppfirstTestPath = PathPlanner.loadPath("First Test Path", 4.0, 4.0);
PathPlannerTrajectory ppMoveForward = PathPlanner.loadPath("Move Forward", 1.0, 1.0);
PathPlannerTrajectory ppRotate = PathPlanner.loadPath("Rotate", 1.0, 1.0);
PathPlannerTrajectory ppCurrentPath = ppfirstTestPath; // change this to change auto
PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand(
ppCurrentPath,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
xController,
yController,
thetaController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive
);
// return new SequentialCommandGroup(
// new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(currentPath.getInitialPose())),
// swerveControllerCommand,
// new InstantCommand(() -> m_robotSwerveDrive.stopModules())
// );
return new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(currentPath.getInitialPose())),
swerveControllerCommand,
new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()),
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrentPath.getInitialPose())),
ppSwerveControllerCommand,
new InstantCommand(() -> m_robotSwerveDrive.stopModules())
);
//return swerveControllerCommand.andThen(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, true));
//return new InstantCommand();
}
/**