Debugging ramsete controller

src/main/java/frc4388/robot/Robot.java

@@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -35,6 +36,7 @@ public class Robot extends TimedRobot {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    SmartDashboard.putString("Auto?", "NAH");
   }
 
   /**
@@ -77,7 +79,7 @@ public class Robot extends TimedRobot {
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
     m_robotContainer.resetOdometry();
-    m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
+    //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
 
     /*
      * String autoSelected = SmartDashboard.getString("Auto Selector",
@@ -103,7 +105,7 @@ public class Robot extends TimedRobot {
   @Override
   public void teleopInit() {
     m_robotContainer.setDriveNeutralMode(NeutralMode.Coast);
-    m_robotContainer.configDriveTrainSensors(FeedbackDevice.SensorDifference);
+    //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor);
     
     // This makes sure that the autonomous stops running when
     // teleop starts running. If you want the autonomous to
@@ -112,6 +114,8 @@ public class Robot extends TimedRobot {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
+
+    SmartDashboard.putString("Auto?", "NAH");
   }
 
   /**

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

@@ -31,6 +31,7 @@ import frc4388.robot.Constants.*;
 import frc4388.robot.commands.DriveStraightAtVelocityPID;
 import frc4388.robot.commands.DriveStraightToPositionMM;
 import frc4388.robot.commands.DriveStraightToPositionPID;
+import frc4388.robot.commands.DriveWithJoystick;
 import frc4388.robot.commands.DriveWithJoystickUsingDeadAssistPID;
 import frc4388.robot.commands.RunClimberWithTriggers;
 import frc4388.robot.commands.RunExtenderOutIn;
@@ -77,7 +78,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
@@ -126,7 +127,7 @@ 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(1, 1), m_robotDrive));
       
         // turn 45 degrees
         new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
@@ -184,11 +185,11 @@ public class RobotContainer {
             new Pose2d(0, 0, new Rotation2d(0)),
             // Pass through these two interior waypoints, making an 's' curve path
             List.of(
-                new Translation2d(1, 1),
-                new Translation2d(2, -1)
+                new Translation2d(2, 0)
+                //new Translation2d(4, -2)
             ),
             // End 3 meters straight ahead of where we started, facing forward
-            new Pose2d(3, 0, new Rotation2d(0)),
+            new Pose2d(4, 0, new Rotation2d(0)),
             // Pass config
             config);
 

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -373,7 +373,7 @@ public class Drive extends SubsystemBase {
    * @param rightSpeed the commanded right output
    */
   public void tankDriveVelocity(double leftSpeed, double rightSpeed) {
-    /*DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
+    DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
     ChassisSpeeds chassisSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds);
     double moveVelMPS = chassisSpeeds.vxMetersPerSecond;
     double angleVelRad = chassisSpeeds.omegaRadiansPerSecond;
@@ -383,14 +383,19 @@ public class Drive extends SubsystemBase {
     m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle, 
                                               m_currentAngleYaw-(360),
                                               m_currentAngleYaw+(360));
-    double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;*/
-    double moveVelLeft = inchesToMeters(metersToInches(leftSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
-    double moveVelRight = inchesToMeters(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
+    double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    double moveVel = inchesToTicks(metersToInches(moveVelMPS))/DriveConstants.SECONDS_TO_TICK_TIME;
+    //double moveVelRight = inchesToTicks(metersToInches(rightSpeed))/DriveConstants.SECONDS_TO_TICK_TIME;
 
-    //runDriveStraightVelocityPID(moveVel, targetGyro);
+    //SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
+    //SmartDashboard.putNumber("Move Vel Right", moveVelRight);
 
-    m_rightFrontMotor.set(TalonFXControlMode.Velocity, moveVelRight);
-    m_leftFrontMotor.set(TalonFXControlMode.Velocity, moveVelLeft);
+    runDriveVelocityPID(moveVel*2, targetGyro);
+
+    //m_rightFrontMotor.set(TalonFXControlMode.Velocity, moveVelRight);
+    //m_leftFrontMotor.set(TalonFXControlMode.Velocity, moveVelLeft);
+
+    //m_driveTrain.feedWatchdog();
   }
 
   /**

src/main/java/frc4388/robot/subsystems/Storage.java

@@ -59,9 +59,9 @@ public class Storage extends SubsystemBase {
     final boolean beam_on = m_beamSensors[0].get();
 
     if (beam_on) {
-      System.err.println("Beam on");
+      //System.err.println("Beam on");
     } else {
-      System.err.println("Beam off");
+      //System.err.println("Beam off");
     }
 
   }