wip

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

@@ -31,7 +31,7 @@ public static final class CanConstants {
     public static final int kLeftRearMotorPort = 2;
     public static final int kRightFrontMotorPort = 4;
     public static final int kRightRearMotorPort = 5;
-    public static final double kRampRate = 0;
+    public static final double kRampRate = 0.1;
 
     public static double maxSpeed = 1;
     public static final double maxSpeedIncrement = 0.05;

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

@@ -21,35 +21,36 @@
  * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+  public static boolean inverted = false;
+
   public static final Joystick m_armController = new Joystick(0);
   public static final XboxController m_armController2 = new XboxController(1);
   public static final XboxController m_driverController = new XboxController(2);
 
   // INSTANTIATES ALL SUBSYSTEMS
   private final Arm m_Arm = new Arm();
-  private final Claw m_claw = new Claw();
+  private final Claw m_Claw = new Claw();
   private final Tank m_Tank = new Tank();
   private final PID m_PID = new PID();
   private final PDP m_PDP = new PDP();
-  private final LineupPID m_LineupPID = new LineupPID();
+
   private final Vision m_Vision = new Vision();
 
   // INSTANTIATES ALL COMMANDS
-  private final OpenClawCommand m_OpenClawCommand = new OpenClawCommand(m_claw);
-  private final CloseClawCommand m_CloseClawCommand = new CloseClawCommand(m_claw);
-
+  private final MoveClawCommand m_MoveClawCommand = new MoveClawCommand(m_Claw);
   private final MoveArmYCommand m_MoveArmYCommand = new MoveArmYCommand(m_Arm);
   private final DriveCommand m_DriveCommand = new DriveCommand(m_Tank);
   private final BalanceCommand m_BalanceCommand = new BalanceCommand(m_PID, m_Tank);
-  private final AutoCommand m_AutoCommand = new AutoCommand(m_PID, m_Tank, m_claw, m_Arm);
+  private final AutoCommand m_AutoCommand = new AutoCommand(m_PID, m_Tank, m_Claw, m_Arm);
+  private final InvertDriveCommand m_InvertDriveCommand = new InvertDriveCommand(m_Tank, this);
   private final PlaceConeSecondLevelCommand m_PlaceConeSecondLevelCommand =
       new PlaceConeSecondLevelCommand(m_Tank, m_Arm);
   private final IncreaseMaxSpeedCommand m_IncreaseMaxSpeedCommand =
       new IncreaseMaxSpeedCommand(m_Tank);
   private final DecreaseMaxSpeedCommand m_DecreaseMaxSpeedCommand =
       new DecreaseMaxSpeedCommand(m_Tank);
   private final FineDriveCommand m_FineDriveCommand = new FineDriveCommand(m_Tank);
-  private final LineupCommand m_LineupCommand = new LineupCommand(m_LineupPID);
+
 
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
@@ -63,22 +64,20 @@ public RobotContainer() {
     tab.add(m_Arm);
     tab.add(m_AutoCommand);
     tab.add(m_BalanceCommand);
-    tab.add(m_CloseClawCommand);
     tab.add(m_DecreaseMaxSpeedCommand);
     tab.add(m_DriveCommand);
     tab.add(m_FineDriveCommand);
     tab.add(m_IncreaseMaxSpeedCommand);
     tab.add(m_MoveArmYCommand);
     tab.add(m_PDP);
-    tab.add(m_OpenClawCommand);
-    tab.add(m_LineupPID);
-    tab.add(m_LineupCommand);
+
+
 
 
     tab.add(m_PID);
     tab.add(m_PlaceConeSecondLevelCommand);
     tab.add(m_Tank);
-    tab.add(m_claw);
+    tab.add(m_Claw);
 
     Logger.configureLoggingAndConfig(this, false);
   }
@@ -95,13 +94,13 @@ private void configureButtonBindings() {
         .whileTrue(m_IncreaseMaxSpeedCommand);
     new JoystickButton(m_driverController, XboxController.Button.kBack.value)
         .whileTrue(m_DecreaseMaxSpeedCommand);
-    new JoystickButton(m_driverController, XboxController.Button.kA.value)
-        .whileTrue(m_LineupCommand);
+    new JoystickButton(m_driverController, XboxController.Button.kX.value)
+        .toggleOnTrue(m_InvertDriveCommand);
 
-    new JoystickButton(m_armController2, XboxController.Button.kX.value)
-        .whileTrue(m_CloseClawCommand);
-    new JoystickButton(m_armController2, XboxController.Button.kY.value)
-        .whileTrue(m_OpenClawCommand);
+    // new JoystickButton(m_armController2, XboxController.Button.kX.value)
+    //     .whileTrue(m_CloseClawCommand);
+    // new JoystickButton(m_armController2, XboxController.Button.kY.value)
+    //     .whileTrue(m_OpenClawCommand);
     new JoystickButton(m_armController2, XboxController.Button.kLeftBumper.value)
         .whileTrue(m_FineDriveCommand);
     new JoystickButton(m_armController2, XboxController.Button.kB.value)
@@ -183,17 +182,17 @@ public static double getControllerRightStickX() {
     return axis;
   }
 
-  public static double getJoystickArmY() {
-    double axis = m_armController.getRawAxis(1);
-    if (Math.abs(axis) < 0.15) {
+  public static double getControllerRightTrigger() {
+    double axis = m_armController2.getRightTriggerAxis();
+    if (Math.abs(axis) < 0.01) {
       axis = 0;
     }
     return axis;
   }
 
-  public static double getJoystickArmTwist() {
-    double axis = m_armController.getRawAxis(2);
-    if (Math.abs(axis) < 0.15) {
+  public static double getControllerLeftTrigger() {
+    double axis = m_armController2.getLeftTriggerAxis();
+    if (Math.abs(axis) < 0.01) {
       axis = 0;
     }
     return axis;
@@ -205,10 +204,13 @@ public Command getAutonomousCommand() {
   }
 
   public void initTeleop() {
-    // Set the default tank command to DriveCommand
-    m_Tank.setDefaultCommand(m_DriveCommand);
-
+    if(inverted){
+      m_Tank.setDefaultCommand(m_InvertDriveCommand);
+    } else{
+      m_Tank.setDefaultCommand(m_DriveCommand);
+    }
     m_Arm.setDefaultCommand(m_MoveArmYCommand);
+    m_Claw.setDefaultCommand(m_MoveClawCommand);
   }
 
   public void initTest() {

src/main/java/frc/robot/commands/AutoCommand.java

@@ -9,9 +9,9 @@
 
 public class AutoCommand extends SequentialCommandGroup {
   public AutoCommand(PID m_pid, Tank m_driveBase, Claw m_claw, Arm m_arm) {
-    addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm));
+    addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm), 
 
-    // new BalanceCommand(m_pid, m_driveBase);
+    new BalanceCommand(m_pid, m_driveBase));
     // new PlaceCubeFirstLevelCommand(m_driveBase, m_claw, m_arm)
     // new PlaceConeSecondLevelCommand(m_driveBase, m_arm)
   }

src/main/java/frc/robot/commands/DriveCommand.java

@@ -29,7 +29,7 @@ public void execute() {
     // double check getMaxSpeed(), might be wrong
     Tank.arcadeDrive(
         RobotContainer.getDriverControllerLeftStickYAdjusted() * Constants.CanConstants.maxSpeed,
-        RobotContainer.getDriverControllerRightStickXAdjusted() * Constants.CanConstants.maxSpeed);
+        RobotContainer.getDriverControllerRightStickXAdjusted() * 0.75);
 
     SmartDashboard.putNumber("maxSpeed", Constants.CanConstants.maxSpeed);
   }

src/main/java/frc/robot/commands/InvertDriveCommand.java

@@ -0,0 +1,56 @@
+// 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;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.Tank;
+
+public class InvertDriveCommand extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final Tank m_drivebase;
+  private final RobotContainer robot;
+
+  public InvertDriveCommand(Tank subsystem, RobotContainer robot) {
+    m_drivebase = subsystem;
+    this.robot = robot;
+
+    addRequirements(m_drivebase);
+  }
+
+  @Override
+  public void initialize() {
+    RobotContainer.inverted = true;
+    robot.initTeleop();
+  }
+
+  @Override
+  public void execute() {
+    // double check getMaxSpeed(), might be wrong
+    m_drivebase.arcadeDrive(
+        -RobotContainer.getDriverControllerLeftStickYAdjusted() * Constants.CanConstants.maxSpeed,
+        RobotContainer.getDriverControllerRightStickXAdjusted() * 0.75);
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    m_drivebase.arcadeDrive(0, 0);
+    RobotContainer.inverted = false;
+    robot.initTeleop();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return false;
+  }
+}

src/main/java/frc/robot/commands/MoveArmYCommand.java

@@ -22,7 +22,7 @@ public void initialize() {}
 
   @Override
   public void execute() {
-    m_subsystem.moveArm(RobotContainer.getControllerLeftStickY() * 1.5);
+    m_subsystem.moveArm(RobotContainer.getControllerLeftStickY());
   }
 
   @Override

src/main/java/frc/robot/commands/CloseClawCommand.java → src/main/java/frc/robot/commands/MoveClawCommand.java

@@ -5,13 +5,14 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.RobotContainer;
 import frc.robot.subsystems.Claw;
 
-public class CloseClawCommand extends CommandBase {
+public class MoveClawCommand extends CommandBase {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
   private final Claw m_subsystem;
 
-  public CloseClawCommand(Claw subsystem) {
+  public MoveClawCommand(Claw subsystem) {
     m_subsystem = subsystem;
     addRequirements(subsystem);
   }
@@ -21,9 +22,11 @@ public void initialize() {}
 
   @Override
   public void execute() {
-    // check if the power draw for the motor is over 2 amps -- if so, end the command
-    // if (m_subsystem.get)
-    m_subsystem.closeClaw();
+    if(Math.abs(RobotContainer.getControllerLeftTrigger()) < 0.2){
+        m_subsystem.clawMotor.set(RobotContainer.getControllerRightTrigger() * 0.7);
+    } else{
+        m_subsystem.clawMotor.set(-RobotContainer.getControllerLeftTrigger() * 0.7);
+    }
   }
 
   @Override

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

@@ -7,11 +7,14 @@
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.RobotContainer;
 import frc.robot.Interfaces.*;
 import java.util.Map;
 
@@ -27,7 +30,7 @@ public class Claw extends SubsystemBase {
   public Claw() {
     clawMotor = new CANSparkMax(8, MotorType.kBrushless);
     clawMotor.setIdleMode(IdleMode.kBrake);
-    clawMotor.setSmartCurrentLimit(10);
+    clawMotor.setSmartCurrentLimit(15);
     clawMotor.burnFlash();
     addChild("Claw Motor", clawMotor);
 
@@ -51,7 +54,7 @@ public Claw() {
 
   @Override
   public void periodic() {
-
+    SmartDashboard.putNumber("rightTrigger", RobotContainer.getControllerRightTrigger());
   }
 
   public void closeClaw() {