SHARE
TWEET

Untitled

a guest Nov 8th, 2019 93 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*----------------------------------------------------------------------------*/
  2. /* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
  3. /* Open Source Software - may be modified and shared by FRC teams. The code   */
  4. /* must be accompanied by the FIRST BSD license file in the root directory of */
  5. /* the project.                                                               */
  6. /*----------------------------------------------------------------------------*/
  7.  
  8. #include "RobotContainer.h"
  9. #include "subsystems/DriveSubsystem.h"
  10.  
  11. #include <units/units.h>
  12.  
  13. #include <frc/controller/PIDController.h>
  14. #include <frc/geometry/Translation2d.h>
  15. #include <frc/shuffleboard/Shuffleboard.h>
  16. #include <frc/trajectory/Trajectory.h>
  17. #include <frc/trajectory/TrajectoryGenerator.h>
  18. #include <frc2/command/InstantCommand.h>
  19. #include <frc2/command/SequentialCommandGroup.h>
  20. #include <frc2/command/SwerveFollowerCommand.h>
  21. #include <frc2/command/button/JoystickButton.h>
  22.  
  23. #include "Constants.h"
  24.  
  25. using namespace DriveConstants;
  26.  
  27. RobotContainer::RobotContainer() {
  28.   // Initialize all of your commands and subsystems here
  29.  
  30.   // Configure the button bindings
  31.   ConfigureButtonBindings();
  32.  
  33.   // Set up default drive command
  34.   m_drive.SetDefaultCommand(frc2::RunCommand(
  35.       [this] {
  36.         m_drive.Drive(units::meters_per_second_t(m_driverController.GetY(frc::GenericHID::kLeftHand)),
  37.                       units::meters_per_second_t(m_driverController.GetY(frc::GenericHID::kRightHand)),
  38.                       units::radians_per_second_t(m_driverController.GetX(frc::GenericHID::kLeftHand)),
  39.                       false);
  40.       },
  41.       {&m_drive}));
  42. }
  43.  
  44. void RobotContainer::ConfigureButtonBindings() {}
  45.  
  46. frc2::Command* RobotContainer::GetAutonomousCommand() {
  47.   // Set up config for trajectory
  48.   frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
  49.                                AutoConstants::kMaxAcceleration);
  50.   // Add kinematics to ensure max speed is actually obeyed
  51.   config.SetKinematics(m_drive.kDriveKinematics);
  52.  
  53.   // An example trajectory to follow.  All units in meters.
  54.   auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
  55.       // Start at the origin facing the +X direction
  56.       frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
  57.       // Pass through these two interior waypoints, making an 's' curve path
  58.       {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
  59.       // End 3 meters straight ahead of where we started, facing forward
  60.       frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)),
  61.       // Pass the config
  62.       config);
  63.  
  64.   frc2::SwerveFollowerCommand<4> swerveFollowerCommand(
  65.       exampleTrajectory, [this]() { return m_drive.GetPose(); },
  66.  
  67.       m_drive.kDriveKinematics,
  68.  
  69.       frc2::PIDController(AutoConstants::kPXController, 0, 0),
  70.       frc2::PIDController(AutoConstants::kPYController, 0, 0),
  71.       frc::ProfiledPIDController(AutoConstants::kPThetaController, 0, 0,
  72.                                  AutoConstants::kThetaControllerConstraints),
  73.  
  74.       [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },
  75.  
  76.       {&m_drive});
  77.  
  78.   // no auto
  79.   return new frc2::SequentialCommandGroup(
  80.       std::move(swerveFollowerCommand), std::move(swerveFollowerCommand),
  81.       frc2::InstantCommand(
  82.           [this]() {
  83.             m_drive.Drive(units::meters_per_second_t(0),
  84.                           units::meters_per_second_t(0),
  85.                           units::radians_per_second_t(0), false);
  86.           },
  87.           {}));
  88. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top