Reverted Drive code to master

2026-06-09 08:48:01 -06:00 · 2020-02-01 13:38:23 -07:00
parent 43a454f9de
commit 4dd70bf1fe
6 changed files with 6 additions and 294 deletions
@@ -25,12 +25,7 @@ public final class Constants {
        public static final int DRIVE_RIGHT_BACK_CAN_ID = 5;
        public static final int PIGEON_ID = 6;

-        /* PID Constants Drive*/
-        public static final int DRIVE_SLOT_IDX = 0;
-        public static final int DRIVE_PID_LOOP_IDX = 0;
-        public static final int DRIVE_TIMEOUT_MS = 30;
-        public static final Gains DRIVE_GAINS = new Gains(0.2, 0.0, 0.0, 0.2, 0, 1.0);
-        
+           
        public static final double ENCODER_TICKS_PER_REV = 2048;
    }
    
@@ -38,18 +33,7 @@ public final class Constants {
        public static final int INTAKE_SPARK_ID = 1;
    }

-    public static final class ElevatorConstants {
-        public static final int TALON_1 = 7;
-        public static final int TALON_2 = 8;

-        /* PID Constants Elevator */
-        public static final int ELEVATOR_SLOT_IDX = 0;
-        public static final int ELEVATOR_PID_LOOP_IDX = 1;
-        public static final int ELEVATOR_TIMEOUT_MS = 30;
-        public static final Gains ELEVATOR_GAINS = new Gains(0.1, 0.0, 0.0, 0.2, 0, 1.0);
-        
-        public static final double ENCODER_TICKS_PER_REV = 2048;
-    }

    public static final class ShooterConstants {
        public static final int SHOOTER_FALCON_ID = 8;
@@ -15,9 +15,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
-import frc4388.robot.commands.DriveAtVelocityPID;
-import frc4388.robot.commands.DriveToDistanceMM;
-import frc4388.robot.commands.DriveToDistancePID;
 import frc4388.robot.commands.DriveWithJoystick;
 import frc4388.robot.commands.RunIntakeWithTriggers;
 import frc4388.robot.commands.ShooterVelocityControlPID;
@@ -81,15 +78,6 @@ public class RobotContainer {
            .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
            .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));

-        new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON)
-            .whenPressed(new DriveToDistancePID(m_robotDrive, 5000));
-
-        new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
-            .whenPressed(new DriveToDistanceMM(m_robotDrive, 5000));
-
-        new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
-            .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000));
-
        new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON)
            .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 13200));

@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Drive;
-
-public class DriveAtVelocityPID extends CommandBase {
-  Drive m_drive;
-  double m_targetVel;
-  double m_leftTarget;
-  double m_rightTarget;
-  /**
-   * Creates a new DriveAtVelocityPID.
-   */
-  public DriveAtVelocityPID(Drive subsystem, double targetVel) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    m_drive = subsystem;
-    m_targetVel = targetVel;
-    addRequirements(m_drive);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    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.runDriveVelocityPID(m_drive.m_leftFrontMotor, m_leftTarget);
-    m_drive.runDriveVelocityPID(m_drive.m_rightFrontMotor, m_rightTarget);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return false;
-  }
-}
@@ -1,57 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Drive;
-
-public class DriveToDistanceMM extends CommandBase {
-  Drive m_drive;
-  double m_distance;
-  double m_leftTarget;
-  double m_rightTarget;
-  
-  /**
-   * Creates a new DriveToDistancePID.
-   */
-  public DriveToDistanceMM(Drive subsystem, double distance) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    m_drive = subsystem;
-    m_distance = distance;
-    addRequirements(m_drive);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance;
-    m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance);
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_drive.runDriveMotionMagicPID(m_drive.m_leftFrontMotor, m_leftTarget);
-    m_drive.runDriveMotionMagicPID(m_drive.m_rightFrontMotor, m_rightTarget);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){
-      return true;
-    } else {
-      return false;
-    }
-  }
-}
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc4388.robot.commands;
-
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Drive;
-
-public class DriveToDistancePID extends CommandBase {
-  Drive m_drive;
-  double m_distance;
-  double m_leftTarget;
-  double m_rightTarget;
-  
-  /**
-   * Creates a new DriveToDistancePID.
-   */
-  public DriveToDistancePID(Drive subsystem, double distance) {
-    // Use addRequirements() here to declare subsystem dependencies.
-    m_drive = subsystem;
-    m_distance = distance;
-    addRequirements(m_drive);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    m_leftTarget = m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() + m_distance;
-    m_rightTarget = -(m_drive.m_rightFrontMotor.getActiveTrajectoryPosition() + m_distance);
-    SmartDashboard.putNumber("Left Target", m_leftTarget);
-    SmartDashboard.putNumber("Right Target", m_rightTarget);
-  }
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    m_drive.runDrivePositionPID(m_drive.m_leftFrontMotor, m_leftTarget);
-    m_drive.runDrivePositionPID(m_drive.m_rightFrontMotor, m_rightTarget);
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    if (Math.abs(m_drive.m_leftFrontMotor.getActiveTrajectoryPosition() - m_leftTarget) < 100){
-      return true;
-    } else {
-      return false;
-    }
-  }
-}
@@ -34,8 +34,6 @@ public class Drive extends SubsystemBase {

  public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);

-  public static Gains m_driveGains = DriveConstants.DRIVE_GAINS;
-
  /**
   * Add your docs here.
   */
@@ -53,63 +51,17 @@ public class Drive extends SubsystemBase {
    m_rightBackMotor.follow(m_rightFrontMotor);

    /* set neutral mode */
-    setDriveTrainNeutralMode(NeutralMode.Brake);
+    setDriveTrainNeutralMode(NeutralMode.Coast);

    m_leftBackMotor.configNeutralDeadband(0.0); // DO NOT CHANGE
    m_rightBackMotor.configNeutralDeadband(0.0); //Ensures motors run at the same speed    
    
    /* flip input so forward becomes back, etc */
    m_leftFrontMotor.setInverted(false);
-    m_rightFrontMotor.setInverted(false);
+    m_rightFrontMotor.setInverted(true);
+    m_driveTrain.setRightSideInverted(true);
    m_leftBackMotor.setInverted(InvertType.FollowMaster);
    m_rightBackMotor.setInverted(InvertType.FollowMaster);
-
-    /* Motor Encoders */
-    //m_leftFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
-    //m_rightFrontMotor.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
-  
-    /*m_leftFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 10, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    m_leftFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configNominalOutputForward(0, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configNominalOutputReverse(0, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configPeakOutputForward(1, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configPeakOutputReverse(-1, DriveConstants.DRIVE_TIMEOUT_MS);*/
-
-    setDriveTrainGains();
-
-    m_leftFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configMotionCruiseVelocity(15000, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.configMotionAcceleration(6000, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    m_leftFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.setSelectedSensorPosition(0, DriveConstants.DRIVE_PID_LOOP_IDX, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    /* Smart Dashboard Initial Values */
-    SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
-    SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
-    SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
-
-    SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
-    SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
-    SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition(0));
-    SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition());
-
-    SmartDashboard.putNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP);
-    SmartDashboard.putNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI);
-    SmartDashboard.putNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD);
-    SmartDashboard.putNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF);
-
-    int closedLoopTimeMs = 1;
-    m_leftFrontMotor.configClosedLoopPeriod(0, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftFrontMotor.configClosedLoopPeriod(1, closedLoopTimeMs, DriveConstants.DRIVE_TIMEOUT_MS);
-    
-
  }

  @Override
@@ -118,23 +70,9 @@ public class Drive extends SubsystemBase {
      SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
      SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
      SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
-
-      SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity(0));
-      SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
-      SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
-      SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
-
-      m_driveGains.kP = SmartDashboard.getNumber("P Value Drive", DriveConstants.DRIVE_GAINS.kP);
-      m_driveGains.kI = SmartDashboard.getNumber("I Value Drive", DriveConstants.DRIVE_GAINS.kI);
-      m_driveGains.kD = SmartDashboard.getNumber("D Value Drive", DriveConstants.DRIVE_GAINS.kD);
-      m_driveGains.kF = SmartDashboard.getNumber("F Value Drive", DriveConstants.DRIVE_GAINS.kF);
-
-      setDriveTrainGains();
-
    } catch (Exception e) {
-
-      System.err.println("The programming team has failed successfully in the Drive Subsystem.");
-    
+      System.err.println("The programming team has failed successfully in the Drive Subsystem in Periodic.");
+      e.printStackTrace(System.err);
    }
  }

@@ -149,23 +87,6 @@ public class Drive extends SubsystemBase {
    m_rightFrontMotor.setNeutralMode(mode);
  }

-  /**
-   * Add your docs here.
-   */
-  public void setDriveTrainGains(){
-    m_leftFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX);
-    m_leftFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_leftFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS);
-
-    m_rightFrontMotor.selectProfileSlot(DriveConstants.DRIVE_SLOT_IDX, DriveConstants.DRIVE_PID_LOOP_IDX);
-    m_rightFrontMotor.config_kF(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kF, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kP(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kP, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kI(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kI, DriveConstants.DRIVE_TIMEOUT_MS);
-    m_rightFrontMotor.config_kD(DriveConstants.DRIVE_SLOT_IDX, m_driveGains.kD, DriveConstants.DRIVE_TIMEOUT_MS);
-  }
-
  /**
   * Add your docs here.
   */
@@ -173,18 +94,6 @@ public class Drive extends SubsystemBase {
    m_driveTrain.arcadeDrive(move, steer);
  }

-  public void runDrivePositionPID(WPI_TalonFX talon, double targetPos) {
-    talon.set(TalonFXControlMode.Position, targetPos);
-  }
-
-  public void runDriveVelocityPID(WPI_TalonFX talon, double targetVel) {
-    talon.set(TalonFXControlMode.Velocity, targetVel);
-  }
-
-  public void runDriveMotionMagicPID(WPI_TalonFX talon, double targetPos){
-    talon.set(TalonFXControlMode.MotionMagic, targetPos);
-  }
-
  public double getGyroYaw() {
    double[] ypr = new double[3];
    m_pigeon.getYawPitchRoll(ypr);