diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 3d6ad6a..b362cd8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -52,7 +52,7 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); + //m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives motor with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // continually sends updates to the Blinkin LED controller to keep the lights on @@ -79,7 +79,7 @@ public class RobotContainer { /* PID Test Command */ new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whileHeld(new RunCommand(() -> m_robotDrive.runVelocityPID(2000), m_robotDrive)); + .whileHeld(new DriveAtVelocityPID(m_robotDrive, 50)); new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) .whenPressed(new InstantCommand(() -> m_robotDrive.resetGyroYaw(), m_robotDrive)); diff --git a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java index 229a7b6..f8c8669 100644 --- a/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveAtVelocityPID.java @@ -9,6 +9,7 @@ package frc4388.robot.commands; import com.ctre.phoenix.motorcontrol.FollowerType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; @@ -18,6 +19,7 @@ public class DriveAtVelocityPID extends CommandBase { double m_targetVel; double m_leftTarget; double m_rightTarget; + double m_copiedTargetVel; /** * Creates a new DriveAtVelocityPID. * @param subsystem drive subsystem @@ -26,23 +28,27 @@ public class DriveAtVelocityPID extends CommandBase { public DriveAtVelocityPID(Drive subsystem, double targetVel) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetVel = targetVel /* DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME;*/; + m_targetVel = targetVel * DriveConstants.TICKS_PER_INCH/DriveConstants.SECONDS_TO_TICK_TIME; + m_copiedTargetVel = targetVel; addRequirements(m_drive); } // Called when the command is initially scheduled. @Override public void initialize() { - m_leftTarget = -m_targetVel; + m_leftTarget = m_targetVel; m_rightTarget = m_targetVel; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drive.runVelocityPID(m_rightTarget); - m_drive.runVelocityPID(m_leftTarget); - m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput); + m_drive.runVelocityPID(m_targetVel); + + SmartDashboard.putNumber("Input Target Velocity", m_copiedTargetVel); + SmartDashboard.putNumber("Output Target Velocity", m_targetVel); + //m_drive.runVelocityPID(m_leftTarget); + //m_drive.m_leftFrontMotor.follow(m_drive.m_rightFrontMotor, FollowerType.PercentOutput); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 21b3c9f..097f9ed 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -75,42 +75,45 @@ public class Drive extends SubsystemBase { m_leftFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.IntegratedSensor, // Local Feedback Source + DriveConstants.PID_PRIMARY, // PID Index for Source [0, 1] + DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout /* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */ - m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source + /*m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source RemoteSensorSource.TalonSRX_SelectedSensor, // Remote Feedback Source DriveConstants.REMOTE_0, // Source number [0, 1] - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + DriveConstants.DRIVE_TIMEOUT_MS); */ // Configuration Timeout /* Configure the Pigeon IMU to the other Remote Slot available on the right Talon */ - m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), + /*m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw, DriveConstants.REMOTE_1, - DriveConstants.DRIVE_TIMEOUT_MS); + DriveConstants.DRIVE_TIMEOUT_MS);*/ /* Setup Sum signal to be used for Distance */ - m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS); /* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */ - m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorSum, - DriveConstants.PID_PRIMARY, - DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorSum, + // DriveConstants.PID_PRIMARY, + // DriveConstants.DRIVE_TIMEOUT_MS); /* Scale Feedback by 0.5 to half the sum of Distance */ - m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient - DriveConstants.PID_PRIMARY, // PID Slot of Source - DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout + //m_rightFrontMotor.configSelectedFeedbackCoefficient( 1.0, // Coefficient + // DriveConstants.PID_PRIMARY, // PID Slot of Source + // DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout /* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) */ - m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, - DriveConstants.PID_TURN, - DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, + // DriveConstants.PID_TURN, + // DriveConstants.DRIVE_TIMEOUT_MS); /* Scale the Feedback Sensor using a coefficient */ - m_leftFrontMotor.configSelectedFeedbackCoefficient( 1.0, - DriveConstants.PID_PRIMARY, - DriveConstants.DRIVE_TIMEOUT_MS); + //m_leftFrontMotor.configSelectedFeedbackCoefficient( 1.0, + // DriveConstants.PID_PRIMARY, + // DriveConstants.DRIVE_TIMEOUT_MS); /* flip input so forward becomes back, etc */ m_leftFrontMotor.setInverted(false); @@ -123,10 +126,10 @@ public class Drive extends SubsystemBase { m_leftBackMotor.setSensorPhase(true);*/ /* Set status frame periods to ensure we don't have stale data */ - m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS); + //m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS); //m_rightFrontMotor.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS); /* deadbands */ @@ -170,10 +173,10 @@ public class Drive extends SubsystemBase { * Max out the peak output (for all modes). * However you can limit the output of a given PID object with configClosedLoopPeakOutput(). */ - m_leftFrontMotor.configPeakOutputForward(+0.5, DriveConstants.DRIVE_TIMEOUT_MS); - m_leftFrontMotor.configPeakOutputReverse(-0.5, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputForward(+0.5, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configPeakOutputReverse(-0.5, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); + m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputForward(+1, DriveConstants.DRIVE_TIMEOUT_MS); + m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS); /** * 1ms per loop. PID loop can be slowed down if need be. @@ -184,7 +187,7 @@ public class Drive extends SubsystemBase { */ int closedLoopTimeMs = 1; m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_PRIMARY, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); - m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); + //m_rightFrontMotor.configClosedLoopPeriod(DriveConstants.PID_TURN, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS); /** * configAuxPIDPolarity(boolean invert, int timeoutMs) @@ -296,9 +299,12 @@ public class Drive extends SubsystemBase { public void runVelocityPID(double targetVel) { m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); - m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); - m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0); + //m_leftFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY); + //m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN); + //m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVelLeft, DemandType.AuxPID, 0); m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1); + m_rightFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0); + //m_leftFrontMotor.set(TalonFXControlMode.Velocity, targetVel, DemandType.AuxPID, 0); } public void runMotionMagicPID(WPI_TalonFX talon, double targetPos){