Modification

This commit is contained in:
ryan123rudder
2021-03-16 23:17:50 -06:00
parent b0f4857b0d
commit d5a4d6cab3
7 changed files with 16 additions and 23 deletions
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -329,7 +329,10 @@ public class RobotContainer {
m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart));
String[] bounce = new String[]{
"Bounce"
"Bounce1",
"Bounce2",
"Bounce3",
"Bounce4"
};
m_bounce = new Bounce(m_robotDrive, buildPaths(bounce));
@@ -401,13 +404,8 @@ public class RobotContainer {
//return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_slalom.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return new SequentialCommandGroup(m_driveOffLineForward,
// new InstantCommand(() -> resetOdometry(new Pose2d())),
// new TankDriveVelocity(m_robotDrive, 1000, 1000, 1));
//return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return new SequentialCommandGroup(new TankDriveVelocity(m_robotDrive, 1000, 1000, 3), new TankDriveVelocity(m_robotDrive, 3000, 3000, 1));
//return m_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0));
} catch (Exception e) {
@@ -416,6 +414,7 @@ public class RobotContainer {
return new InstantCommand();
}
TrajectoryConfig getTrajectoryConfig() {
return new TrajectoryConfig(
DriveConstants.MAX_SPEED_METERS_PER_SECOND,
@@ -424,21 +423,6 @@ public class RobotContainer {
.setKinematics(DriveConstants.kDriveKinematics);
}
Trajectory getTrajectory(TrajectoryConfig config) {
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
new Pose2d(2.9, -2.4, new Rotation2d(0)),
// Pass through these two interior waypoints, making an 's' curve path
List.of(
new Translation2d(4.1, -1.7)
),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(5.1, -0.7, new Rotation2d(0)),
// Pass config
config);
return exampleTrajectory;
}
public RamseteCommand getRamseteCommand(Trajectory trajectory) {
RamseteCommand ramseteCommand = new RamseteCommand(
trajectory,
@@ -466,8 +450,8 @@ public class RobotContainer {
SmartDashboard.putString("trajectoryPath Initial", trajectoryPath.toString());
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
initialTrajectory = trajectory;
initialTrajectory = trajectory;
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[0] = ramseteCommand;
times[0] = initialTrajectory.getTotalTimeSeconds();
@@ -21,7 +21,11 @@ public class Bounce extends SequentialCommandGroup {
public Bounce(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(paths[0]
addCommands(
paths[0],
paths[1],
paths[2],
paths[3]
);
}
}
@@ -514,6 +514,7 @@ public class Drive extends SubsystemBase {
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
}
/**
* Resets the yaw of the pigeon
*/