Merge branch 'master' into button

src/main/java/frc4388/robot/RobotContainer.java

@@ -7,19 +7,34 @@
 
 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;
+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;
 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.button.JoystickButton;
+
 import frc4388.robot.Constants.*;
 import frc4388.robot.commands.DriveStraightAtVelocityPID;
 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;
@@ -74,7 +89,7 @@ public class RobotContainer {
 
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
-        m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, getDriverController()));
+        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
@@ -98,7 +113,7 @@ public class RobotContainer {
     private void configureButtonBindings() {
         /* Driver Buttons */
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 144));
+            .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(0), m_robotDrive));
 
         /* Operator Buttons */
         // activates "Lit Mode"
@@ -130,11 +145,11 @@ public class RobotContainer {
       
         // resets the yaw of the pigeon
         new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 72));
+            .whileHeld(new RunCommand(() -> m_robotDrive.tankDriveVelocity(9, 3), m_robotDrive));
       
         // turn 45 degrees
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new RunCommand(() -> m_robotDrive.runTurningPID(45), m_robotDrive));
+            .whenPressed(new RunCommand(() -> m_robotDrive.driveWithInputAux(0.2, 0), m_robotDrive));
 
 
         // sets solenoids into high gear
@@ -158,7 +173,7 @@ public class RobotContainer {
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
             .whileHeld(new TrackTarget(m_robotShooter));
     }
-      
+       
     /**
      * Sets Motors to a NeutralMode.
      * @param mode NeutralMode to set motors to
@@ -167,13 +182,59 @@ public class RobotContainer {
         m_robotDrive.setDriveTrainNeutralMode(mode);
     }
 
+    /**
+     * Sets the gear of the drivetrain
+     * @param state the gearing of the gearbox (true is high, false is low)
+     */
+    public void setDriveGearState(boolean state) {
+        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() {
-        // no auto
+
+        // 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();
     }