From 0e696c73ed295e9276221255063ead475b202345 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Fri, 14 Feb 2020 08:32:32 -0700 Subject: [PATCH] Debugging ramsete controller --- src/main/java/frc4388/robot/Robot.java | 8 ++++++-- .../java/frc4388/robot/RobotContainer.java | 11 ++++++----- .../java/frc4388/robot/subsystems/Drive.java | 19 ++++++++++++------- .../frc4388/robot/subsystems/Storage.java | 4 ++-- 4 files changed, 26 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1335ec6..a726e56 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/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"); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 08c3ec4..ce4e549 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index b3afa0d..15dd296 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/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(); } /** diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 84f01ec..e61cbef 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/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"); } }