Merge pull request #10 from Team6593/DriveTrain_and_LimitSwitch_rewrite

Drive train and limit switch rewrite

networktables.json

@@ -0,0 +1 @@
+[]

src/main/java/frc/robot/Constants.java

@@ -25,6 +25,12 @@ public static final class CameraVision {
         public final int ViewportWidth = 640;
         public final int ViewportHeight = 480;
     }
+    public static final class LimitSwitchesPorts{
+
+        public final int ElevatorTopLimitSwitchPort = 0;
+        public final int ElevatorLowLimitSwitchPort = 1;
+
+    }
 
     public static final class InputMap {
 
@@ -51,6 +57,12 @@ public static final class Motors {
         public final int MasterLeft = 3;
         public final int FollowerLeft = 4;
         public final double falconUnitsPerRevolution = 2048;
+
+        public final int ElevatorMotorID = 6;
+    }
+
+    public static final class SpeedsForMotors{
+        public final double elevator_setSpeed = 0.2;
     }
 
 

src/main/java/frc/robot/RobotContainer.java

@@ -4,18 +4,25 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.XboxController;
+import frc.robot.Constants.SpeedsForMotors;
+import frc.robot.Constants.InputMap.xBox;
 import frc.robot.Utils.MemoryMonitor;
-import frc.robot.commands.ExampleCommand;
-import frc.robot.commands.autonomous.DriveToChargeStation;
-import frc.robot.commands.autonomous.TaxiWithGyro;
+
+import frc.robot.commands.ElevatorCommands.ElevatorDownCommand;
+import frc.robot.commands.ElevatorCommands.ElevatorStopCommand;
+import frc.robot.commands.ElevatorCommands.ElevatorUpCommand;
+
 import frc.robot.commands.drivetrain.DriveTrain_DefaultCommnad;
 import frc.robot.subsystems.DriveTrain;
-import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.vision.CamRIO;
 import frc.robot.subsystems.vision.LimeLight;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -26,31 +33,44 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   public final DriveTrain driveTrain;
-
+  public final Elevator elevator;
   // Make sure this is public so you can call camInit()
   public final CamRIO rioCamera;
   public final LimeLight limeLight;
 
-  private final ExampleCommand exampleCommand;
-  private final ExampleSubsystem exampleSubsystem;
+  private Compressor compressor;
 
   //Util classes
   public final MemoryMonitor memoryMonitor;
 
   private Constants constants = new Constants();
+  private xBox xbox = new xBox();
+  private SpeedsForMotors speedsForMotors = new SpeedsForMotors();
   //IO
   private XboxController xboxController = new XboxController(constants.XboxController_Port);
-
+  private JoystickButton rightButtonClick, leftButtonClick, aButton, xButton, yButton;
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
-  public RobotContainer() {
+  public RobotContainer() {    
+    //examples
+
+    //instances of classes
     limeLight = new LimeLight();
     memoryMonitor = new MemoryMonitor();
     rioCamera = new CamRIO();
     driveTrain = new DriveTrain();
-    exampleSubsystem = new ExampleSubsystem();
-    exampleCommand = new ExampleCommand();
+
+    elevator = new Elevator();
+
+    aButton = new JoystickButton(xboxController, xbox.Abutton);
+    xButton = new JoystickButton(xboxController, xbox.Bbutton);
+    yButton = new JoystickButton(xboxController, xbox.Ybutton);
+
+    compressor = new Compressor(PneumaticsModuleType.CTREPCM);
+
     driveTrain.setDefaultCommand(new DriveTrain_DefaultCommnad(driveTrain, xboxController));
-
+
+    //xbox buttons
+
     // Configure the button bindings
     configureButtonBindings();
   }
@@ -62,6 +82,10 @@ public RobotContainer() {
    * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
+    aButton.onTrue(new ElevatorDownCommand(elevator, speedsForMotors.elevator_setSpeed));
+    yButton.onTrue(new ElevatorUpCommand(elevator, speedsForMotors.elevator_setSpeed));
+    xButton.onTrue(new ElevatorStopCommand(elevator));
+
 
   }
 

src/main/java/frc/robot/commands/ExampleCommand.java → src/main/java/frc/robot/commands/ElevatorCommands/ElevatorDownCommand.java

@@ -2,13 +2,21 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc.robot.commands;
+package frc.robot.commands.ElevatorCommands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Elevator;
 
-public class ExampleCommand extends CommandBase {
-  /** Creates a new ExampleCommand. */
-  public ExampleCommand() {
+public class ElevatorDownCommand extends CommandBase {
+  /** Creates a new ElevatorDownCommand. */
+  private Elevator elevator;
+  private double elevatorSpeed;
+
+  public ElevatorDownCommand(Elevator elevator, double elevatorSpeed) {
+    this.elevator = elevator;
+    this.elevatorSpeed = elevatorSpeed;
+
+    addRequirements(elevator);
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
@@ -18,11 +26,15 @@ public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    elevator.elevatorDown(elevatorSpeed);
+  }
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    elevator.elevatorStop();
+  }
 
   // Returns true when the command should end.
   @Override

src/main/java/frc/robot/commands/ElevatorCommands/ElevatorStopCommand.java

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ElevatorCommands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.Elevator;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class ElevatorStopCommand extends InstantCommand {
+
+  private Elevator elevator;
+
+  public ElevatorStopCommand(Elevator elevator) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    this.elevator = elevator;
+
+    addRequirements(elevator);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    elevator.elevatorStop();
+
+  }
+}

src/main/java/frc/robot/commands/ElevatorCommands/ElevatorUpCommand.java

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.ElevatorCommands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.Elevator;
+
+public class ElevatorUpCommand extends CommandBase {
+
+  private Elevator elevator;
+  private double elevatorSpeed;
+  /** Creates a new ElevatorUpCommand. */
+  public ElevatorUpCommand(Elevator elevator, double elevatorSpeed){
+    this.elevator = elevator;
+    this.elevatorSpeed = elevatorSpeed;
+
+    addRequirements(elevator);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    elevator.elevatorUP(elevatorSpeed);
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    elevator.elevatorStop();
+    //elevator.elevatorBrake();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/autonomous/TaxiWithGyro.java

@@ -29,7 +29,7 @@ public TaxiWithGyro(DriveTrain driveTrain, double motorSpeed) {
   @Override
   public void initialize() {
     startTime = Timer.getFPGATimestamp();
-    driveTrain.dtInit();
+    //driveTrain.dtInit();
     driveTrain.resetGyro();
     //timer.reset();
     timer.start();

src/main/java/frc/robot/subsystems/DriveTrain.java

@@ -61,13 +61,13 @@ public class DriveTrain extends SubsystemBase {
   /** Creates a new DriveTrain. */
   public DriveTrain() {
 
-    followerRight.follow(masterRight);    
-    followerLeft.follow(masterLeft);
+    // followerRight.follow(masterRight);    
+    // followerLeft.follow(masterLeft);
 
     masterRight.setInverted(true);
     masterLeft.setInverted(false);
-    followerLeft.setInverted(InvertType.FollowMaster);
-    followerRight.setInverted(InvertType.FollowMaster);
+    followerLeft.setInverted(false);
+    followerRight.setInverted(true);
     //NavX Gyro setup
     try {
         gyro = new AHRS(SPI.Port.kMXP);
@@ -128,14 +128,23 @@ public void tankDrive(double speed, double rotation) {
     }
 
     public void arcadeDrive(double xSpd, double zRot) {
-      Drive.arcadeDrive(xSpd, zRot, false);
+
+        Drive.arcadeDrive(xSpd, zRot);
     }
 
     public void autonDrive(double speed) {
       DtRight.set(speed);
       DtLeft.set(speed);
+
+    }
+
+
+    // SOLENOID/SHIFTERS
+    public void highGear(){
+      dtShifter.set(Value.kForward);
     }
 
+
     public void drive(double motorspeed) {
       masterLeft.set(ControlMode.PercentOutput, motorspeed);
       masterRight.set(ControlMode.PercentOutput, motorspeed);
@@ -187,10 +196,14 @@ public void dtInit() {
 
       // set integrated sensor for PID, this doesn't matter even if PID isn't used
       config.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor;
-      masterRight.setSelectedSensorPosition(0);
-      masterLeft.setSelectedSensorPosition(0);
-      followerRight.setSelectedSensorPosition(0);
-      followerLeft.setSelectedSensorPosition(0);
+
+
+
+    masterRight.setSelectedSensorPosition(0);
+    masterLeft.setSelectedSensorPosition(0);
+    followerRight.setSelectedSensorPosition(0);
+    followerLeft.setSelectedSensorPosition(0);
+
   }
 
   // MOTOR POSITION/SENSOR
@@ -282,9 +295,10 @@ public void displayTalonData() {
     SmartDashboard.putNumber("Follower Left Stator Current", followerLeftStatorCurrent);
   }
 
-  /**
-   * displays TalonFX sensor data to rioLog, this method should be called in periodic()
-   */
+
+   /**
+    * displays TalonFX sensor data to rioLog, this method should be called in periodic()
+    */
   public void printTalonData() {
     // TODO: change motor naming conventions to Master/Follower here
     System.out.println("Sensor position, master right" + masterRight.getSelectedSensorPosition());
@@ -307,14 +321,15 @@ public void printTalonData() {
     System.out.println("Stator Current, Master Left" + masterLeft.getStatorCurrent());
     System.out.println("Stator Current, Slave Left" + followerLeft.getStatorCurrent());
 
-    masterRight.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 10);
-    masterLeft.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 10);
-    followerRight.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 10);
-    followerLeft.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 10);
+    masterRight.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 350);
+    masterLeft.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 350);
+    followerRight.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 350);
+    followerLeft.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 350);
   }
 
   @Override
   public void periodic() {
+
     displayTalonData();
   }
 }