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

Created 6 Ball Auto with PathWeaver

Browse Source 
Co-Authored-By: Keenan D. Buckley <hfocus@users.noreply.github.com>
Co-Authored-By: kyrarivera <kyrarivera@users.noreply.github.com>
This commit is contained in:
Aarav Shah
2020-03-07 12:41:37 -07:00
parent 3bbd00e0f6
commit 05c0ef4047
12 changed files with 122 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -73,7 +73,7 @@ public final class Constants {
public static final Gains DRIVE_VELOCITY_GAINS_BACK = new Gains(0.0, 0.0, 0.0, 0.0518, 0, 1.0);
/* Trajectory Constants */
public static final double MAX_SPEED_METERS_PER_SECOND = 1;
public static final double MAX_SPEED_METERS_PER_SECOND = 1.0;
public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1.0;
public static final double TRACK_WIDTH_METERS = 0.648;
public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(TRACK_WIDTH_METERS);
src/main/java/frc4388/robot/Robot.java
+2 -1
View File
@@ -10,6 +10,7 @@ package frc4388.robot;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -78,7 +79,7 @@ public class Robot extends TimedRobot {
m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
m_robotContainer.setDriveGearState(false);
m_robotContainer.resetOdometry();
m_robotContainer.resetOdometry(new Pose2d());
//m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
/*
src/main/java/frc4388/robot/RobotContainer.java
+55 -16
View File
@@ -7,26 +7,33 @@
package frc4388.robot;
import java.nio.file.Path;
import java.util.List;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.DriveConstants;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.commands.auto.SixBallAutoMiddle;
import frc4388.robot.commands.auto.Wait;
import frc4388.robot.commands.climber.DisengageRachet;
import frc4388.robot.commands.climber.RunClimberWithTriggers;
@@ -233,10 +240,19 @@ public class RobotContainer {
public Command getAutonomousCommand() {
// Create config for trajectory
TrajectoryConfig config = getTrajectoryConfig();
Trajectory trajectory = getTrajectory(config);
RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
//Trajectory trajectory = getTrajectory(config);
String trajectoryJSON0 = "paths/FirstPath0.wpilib.json";
String trajectoryJSON1 = "paths/FirstPath1.wpilib.json";
String[] sixBallAutoMiddlePaths = new String[]{
"FirstPath0",
"FirstPath1"
};
// Run path following command, then stop at the end.
return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//SequentialCommandGroup ramseteCommands = new SequentialCommandGroup(ramseteCommand0, ramseteCommand1);
return new SixBallAutoMiddle(buildPaths(sixBallAutoMiddlePaths)).andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return new AutoPath1FromCenter(m_robotDrive, m_robotPneumatics);
//return new AutoPath2FromRight(m_robotDrive, m_robotPneumatics);
@@ -262,25 +278,19 @@ public class RobotContainer {
Trajectory getTrajectory(TrajectoryConfig config) {
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
new Pose2d(0, 0, new Rotation2d(0)),
new Pose2d(2.9, -2.4, new Rotation2d(0)),
// Pass through these two interior waypoints, making an 's' curve path
List.of(
new Translation2d(1, -1),
new Translation2d(2, 0.0),
new Translation2d(1, 1),
new Translation2d(0, 0),
new Translation2d(1, -1),
new Translation2d(2, 0.0),
new Translation2d(1, 1)
new Translation2d(4.1, -1.7)
),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(0, 0, new Rotation2d(0)),
new Pose2d(5.1, -0.7, new Rotation2d(0)),
// Pass config
config);
return exampleTrajectory;
}
RamseteCommand getRamseteCommand(Trajectory trajectory) {
public RamseteCommand getRamseteCommand(Trajectory trajectory) {
RamseteCommand ramseteCommand = new RamseteCommand(
trajectory,
m_robotDrive::getPose,
@@ -292,6 +302,36 @@ public class RobotContainer {
return ramseteCommand;
}
public RamseteCommand[] buildPaths(String[] paths) {
RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length];
Trajectory initialTrajectory;
try {
if (true) {
String path = paths[0];
String trajectoryJSON = "paths/" + path + ".wpilib.json";
Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
initialTrajectory = trajectory;
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[0] = ramseteCommand;
}
for(int i = 1; i < paths.length; i++) {
String path = paths[i];
String trajectoryJSON = "paths/" + path + ".wpilib.json";
Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[i] = ramseteCommand;
}
} catch (Exception e) {
DriverStation.reportError("Unable to open trajectory", e.getStackTrace());
}
return ramseteCommands;
}
/**
* Sets Motors to a NeutralMode.
* @param mode NeutralMode to set motors to
@@ -311,9 +351,8 @@ public class RobotContainer {
/**
*
*/
public void resetOdometry() {
m_robotDrive.resetGyroAngles();
m_robotDrive.setOdometry(new Pose2d());
public void resetOdometry(Pose2d pose) {
m_robotDrive.setOdometry(pose);
}
/**
src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
+35
View File
@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package frc4388.robot.commands.auto;
import java.nio.file.Path;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
// NOTE: Consider using this command inline, rather than writing a subclass. For more
// information, see:
// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
public class SixBallAutoMiddle extends SequentialCommandGroup {
/**
* Creates a new SixBallAutoMiddle.
*/
public SixBallAutoMiddle(RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
addCommands(
paths[0],
paths[1]
);
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+9 -8
View File
@@ -105,7 +105,7 @@ public class Drive extends SubsystemBase {
m_leftBackMotor.configFactoryDefault();
m_rightBackMotor.configFactoryDefault();
m_pigeon.configFactoryDefault();
resetGyroYaw();
resetGyroYaw(0);
/* Config Open Loop Ramp so we don't make sudden output changes */
@@ -497,24 +497,25 @@ public class Drive extends SubsystemBase {
*/
public void setOdometry(Pose2d pose) {
resetEncoders();
resetGyroYaw(pose.getRotation().getDegrees());
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
}
/**
* Resets the yaw of the pigeon
*/
public void resetGyroYaw() {
m_pigeon.setYaw(0);
m_pigeon.setAccumZAngle(0);
resetGyroAngles();
public void resetGyroYaw(double angle) {
m_pigeon.setYaw(angle);
m_pigeon.setAccumZAngle(angle);
resetGyroAngles(angle);
}
/**
* Add docs here
*/
public void resetGyroAngles() {
m_lastAngleYaw = 0;
m_currentAngleYaw = 0;
public void resetGyroAngles(double angle) {
m_lastAngleYaw = angle;
m_currentAngleYaw = angle;
}
/**