I have watched countless videos and have gotten the robot to only shift. All it will do is shift forward an inch and move back to the starting position. I have researched this problem and have realized that it could only be going through the first waypoint that my path weaver outputs to WPILIB. If anyone can help I would greatly appreciate it. sorry if the code is somewhat crude, this is only my second year coding in Java.
Here is my code: https://github.com/avalanche-svg/Pathweaver-With-Ramsete_Command