Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 08:48:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

rebuilt all paths, flipped odometry in auto (janky)

Browse Source 
Mostly untested
This commit is contained in:
ryan123rudder
2020-07-13 14:54:42 -06:00
parent a62a91f5f3
commit 560dda1f58
22 changed files with 90 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
+19 -2
View File
@@ -10,8 +10,10 @@ package frc4388.robot.commands.auto;
import java.nio.file.Path;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
@@ -27,9 +29,24 @@ public class SixBallAutoMiddle extends SequentialCommandGroup {
public SixBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
/* TODO
* Flip line 41 to true, and test positive and negative directions with smart dash
* If those match to the picture on Ryan's phone, figure out what else could be wrong
* If they don't match, make them match
* Sincerly, Past Ryan and Keenan
*/
addCommands(
paths[0]
new InstantCommand(() -> drive.getPose(), drive),
new InstantCommand(() -> drive.SetHeading(true), drive),
new InstantCommand(() -> drive.updatePosition(), drive),
new InstantCommand(() -> drive.setOdometry(drive.savedOdometry), drive),
paths[0],
paths[1],
new InstantCommand(() -> drive.getPose(), drive),
new InstantCommand(() -> drive.SetHeading(false), drive),
new InstantCommand(() -> drive.updatePosition(), drive),
new InstantCommand(() -> drive.setOdometry(drive.savedOdometry), drive)
);
}
}