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

did a lot

Browse Source
This commit is contained in:
Aarav Shah
2020-03-10 20:13:52 -06:00
parent 28d9e0a5f4
commit 59fa78f6e3
18 changed files with 177 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+30 -5
View File
@@ -19,6 +19,7 @@ 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.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
@@ -33,6 +34,8 @@ 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.DriveOffLineBackward;
import frc4388.robot.commands.auto.DriveOffLineForward;
import frc4388.robot.commands.auto.EightBallAutoMiddle;
import frc4388.robot.commands.auto.SixBallAutoMiddle;
import frc4388.robot.commands.auto.Wait;
@@ -92,11 +95,16 @@ public class RobotContainer {
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
/* Autos */
SixBallAutoMiddle m_sixBallAutoMiddle;
double m_totalTimeAuto;
SixBallAutoMiddle m_sixBallAutoMiddle;
EightBallAutoMiddle m_eightBallAutoMiddle;
DriveOffLineForward m_driveOffLineForward;
DriveOffLineBackward m_driveOffLineBackward;
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -247,12 +255,26 @@ public class RobotContainer {
String[] sixBallAutoMiddlePaths = new String[]{
"SixBallMidComplete"
};
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths, false));
String[] eightBallAutoMiddlePaths = new String[]{
"EightBallMidComplete"
};
m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths));
m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths, false));
String[] driveOffLineForwardPaths = new String[]{
"DriveOffLineForward"
};
m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, buildPaths(driveOffLineForwardPaths, false));
String[] driveOffLineBackwardPaths = new String[]{
"DriveOffLineBackward"
};
m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths, true));
}
/**
@@ -271,7 +293,9 @@ public class RobotContainer {
SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
//return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
return m_driveOffLineBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
} catch (Exception e) {
System.err.println("ERROR");
@@ -314,7 +338,7 @@ public class RobotContainer {
return ramseteCommand;
}
public RamseteCommand[] buildPaths(String[] paths) {
public RamseteCommand[] buildPaths(String[] paths, boolean isReversed) {
RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length];
double[] times = new double[paths.length];
Trajectory initialTrajectory;
@@ -327,6 +351,7 @@ public class RobotContainer {
Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
initialTrajectory = trajectory;
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[0] = ramseteCommand;
times[0] = initialTrajectory.getTotalTimeSeconds();