mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 00:38:00 -06:00
Merge branch 'master' of https://github.com/Team4388/RiseOfRidgebotics2020
This commit is contained in:
Aarav Shah
@@ -9,7 +9,6 @@ package frc4388.robot;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
@@ -17,7 +16,6 @@ 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.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.trajectory.Trajectory;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
|
||||
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
|
||||
@@ -28,17 +26,11 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
|
||||
import frc4388.robot.Constants.*;
|
||||
import frc4388.robot.commands.DriveStraightAtVelocityPID;
|
||||
import frc4388.robot.commands.DrivePositionMPAux;
|
||||
import frc4388.robot.commands.DriveWithJoystick;
|
||||
import frc4388.robot.commands.DriveStraightToPositionMM;
|
||||
import frc4388.robot.commands.DriveStraightToPositionPID;
|
||||
import frc4388.robot.commands.DriveWithJoystick;
|
||||
import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
|
||||
import frc4388.robot.commands.DriveWithJoystickDriveStraight;
|
||||
import frc4388.robot.commands.RunClimberWithTriggers;
|
||||
import frc4388.robot.commands.RunExtenderOutIn;
|
||||
import frc4388.robot.commands.RunIntakeWithTriggers;
|
||||
import frc4388.robot.commands.ShooterVelocityControlPID;
|
||||
import frc4388.robot.commands.StorageIntakeGroup;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.Intake;
|
||||
@@ -46,15 +38,11 @@ import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.Shooter;
|
||||
import frc4388.robot.subsystems.Climber;
|
||||
import frc4388.robot.commands.RunLevelerWithJoystick;
|
||||
import frc4388.robot.subsystems.Drive;
|
||||
import frc4388.robot.subsystems.Intake;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.commands.TrackTarget;
|
||||
import frc4388.robot.commands.storageOutake;
|
||||
import frc4388.robot.subsystems.Camera;
|
||||
import frc4388.robot.subsystems.Leveler;
|
||||
import frc4388.robot.subsystems.Storage;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
|
||||
@@ -94,15 +82,14 @@ public class RobotContainer {
|
||||
m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController()));
|
||||
// drives intake with input from triggers on the opperator controller
|
||||
m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController()));
|
||||
// drives climber with input from triggers on the opperator controller
|
||||
m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
|
||||
// runs the drum shooter in idle mode
|
||||
m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runShooterWithInput(m_operatorXbox.getLeftXAxis()), m_robotShooter));
|
||||
// drives climber with input from triggers on the opperator controller
|
||||
m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController()));
|
||||
// drives the leveler with an axis input from the driver controller
|
||||
m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController()));
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
|
||||
}
|
||||
|
||||
|
||||
@@ -113,20 +100,34 @@ public class RobotContainer {
|
||||
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
/* Test Buttons */
|
||||
// A driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(new InstantCommand());
|
||||
|
||||
// B driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
|
||||
.whenPressed(new InstantCommand());
|
||||
|
||||
// Y driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
|
||||
.whenPressed(new InstantCommand());
|
||||
|
||||
// X driver test button
|
||||
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
|
||||
.whenPressed(new InstantCommand());
|
||||
|
||||
/* Driver Buttons */
|
||||
// sets solenoids into high gear
|
||||
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
|
||||
|
||||
// sets solenoids into low gear
|
||||
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive));
|
||||
|
||||
.whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive));
|
||||
|
||||
/* Operator Buttons */
|
||||
|
||||
//TODO: Shooter Buttons
|
||||
//TODO: Shooter Buttons
|
||||
// shoots until released
|
||||
//new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
// .whileHeld(new ShootShooter(m_robotShooter, m_robotStorage, 5));
|
||||
@@ -161,7 +162,62 @@ public class RobotContainer {
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)
|
||||
.whileHeld(new storageOutake(m_robotStorage));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// Create config for trajectory
|
||||
TrajectoryConfig config = getTrajectoryConfig();
|
||||
Trajectory trajectory = getTrajectory(config);
|
||||
RamseteCommand ramseteCommand = getRamseteCommand(trajectory);
|
||||
// Run path following command, then stop at the end.
|
||||
//return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
|
||||
|
||||
// return new InstantCommand();
|
||||
return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0);
|
||||
}
|
||||
|
||||
TrajectoryConfig getTrajectoryConfig() {
|
||||
return new TrajectoryConfig(
|
||||
DriveConstants.MAX_SPEED_METERS_PER_SECOND,
|
||||
DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
.setKinematics(DriveConstants.kDriveKinematics);
|
||||
}
|
||||
|
||||
Trajectory getTrajectory(TrajectoryConfig config) {
|
||||
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
|
||||
// Start at the origin facing the +X direction
|
||||
new Pose2d(0, 0, new Rotation2d(0)),
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
List.of(
|
||||
new Translation2d(10, 0)
|
||||
),
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
new Pose2d(20, 20, new Rotation2d(0)),
|
||||
// Pass config
|
||||
config);
|
||||
// 10 = 20, 20 = 35, 30 = 53.5
|
||||
// (0,10) = (8,22)
|
||||
|
||||
return exampleTrajectory;
|
||||
}
|
||||
|
||||
RamseteCommand getRamseteCommand(Trajectory trajectory) {
|
||||
RamseteCommand ramseteCommand = new RamseteCommand(
|
||||
trajectory,
|
||||
m_robotDrive::getPose,
|
||||
new RamseteController(),
|
||||
DriveConstants.kDriveKinematics,
|
||||
m_robotDrive::tankDriveVelocity,
|
||||
m_robotDrive);
|
||||
|
||||
return ramseteCommand;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets Motors to a NeutralMode.
|
||||
* @param mode NeutralMode to set motors to
|
||||
@@ -178,54 +234,14 @@ public class RobotContainer {
|
||||
m_robotDrive.setShiftState(state);
|
||||
}
|
||||
|
||||
public void configDriveTrainSensors(FeedbackDevice type) {
|
||||
m_robotDrive.configMotorSensor(type);
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public void resetOdometry() {
|
||||
m_robotDrive.resetGyroAngles();
|
||||
m_robotDrive.setOdometry(new Pose2d());
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
|
||||
// Create config for trajectory
|
||||
/*TrajectoryConfig config = new TrajectoryConfig( DriveConstants.MAX_SPEED_METERS_PER_SECOND,
|
||||
DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED)
|
||||
// Add kinematics to ensure max speed is actually obeyed
|
||||
.setKinematics(DriveConstants.kDriveKinematics);
|
||||
|
||||
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
|
||||
// Start at the origin facing the +X direction
|
||||
new Pose2d(0, 0, new Rotation2d(0)),
|
||||
// Pass through these two interior waypoints, making an 's' curve path
|
||||
List.of(
|
||||
new Translation2d(10, 0)
|
||||
),
|
||||
// End 3 meters straight ahead of where we started, facing forward
|
||||
new Pose2d(20, 20, new Rotation2d(0)),
|
||||
// Pass config
|
||||
config);
|
||||
// 10 = 20, 20 = 35, 30 = 53.5
|
||||
// (0,10) = (8,22)
|
||||
RamseteCommand ramseteCommand = new RamseteCommand(
|
||||
exampleTrajectory,
|
||||
m_robotDrive::getPose,
|
||||
new RamseteController(),
|
||||
DriveConstants.kDriveKinematics,
|
||||
m_robotDrive::tankDriveVelocity,
|
||||
m_robotDrive);
|
||||
|
||||
// Run path following command, then stop at the end.
|
||||
return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));*/
|
||||
return new InstantCommand();
|
||||
}
|
||||
|
||||
/**
|
||||
* Used for analog inputs like triggers and axises.
|
||||
* @return IHandController interface for the Driver Controller.
|
||||
|
||||
Reference in New Issue
Block a user