Merge branch 'place-game-piece-comms' of https://github.com/Bearbots6…

…964/FRC2023 into place-game-piece-comms

simgui.json

@@ -5,14 +5,15 @@
         "header": {
           "open": true
         }
-      },
-      "navX-Sensor[0]": {
-        "header": {
-          "open": true
-        }
       }
     },
     "RoboRIO": {
+      "6V Rail": {
+        "open": true
+      },
+      "RoboRIO Input": {
+        "open": true
+      },
       "window": {
         "visible": true
       }
@@ -31,6 +32,23 @@
       "/Shuffleboard/Motors/Right": "Motor Controller",
       "/Shuffleboard/PID/Accelerometer": "Gyro",
       "/Shuffleboard/PID/Gyro": "Gyro",
+      "/Shuffleboard/Subsystems/Arm": "Subsystem",
+      "/Shuffleboard/Subsystems/AutoCommand": "Command",
+      "/Shuffleboard/Subsystems/BalanceCommand": "Command",
+      "/Shuffleboard/Subsystems/Claw": "Subsystem",
+      "/Shuffleboard/Subsystems/CloseClawCommand": "Command",
+      "/Shuffleboard/Subsystems/DecreaseMaxSpeedCommand": "Command",
+      "/Shuffleboard/Subsystems/DriveCommand": "Command",
+      "/Shuffleboard/Subsystems/FineDriveCommand": "Command",
+      "/Shuffleboard/Subsystems/IncreaseMaxSpeedCommand": "Command",
+      "/Shuffleboard/Subsystems/MoveArmYCommand": "Command",
+      "/Shuffleboard/Subsystems/PDP": "Subsystem",
+      "/Shuffleboard/Subsystems/PID": "Subsystem",
+      "/Shuffleboard/Subsystems/PlaceConeSecondLevelCommand": "Command",
+      "/Shuffleboard/Subsystems/PlaceGamePieceCommand": "Command",
+      "/Shuffleboard/Subsystems/SwitchIdleModeCommmand": "Command",
+      "/Shuffleboard/Subsystems/Tank": "Subsystem",
+      "/Shuffleboard/Subsystems/Turret": "Subsystem",
       "/SmartDashboard/Scheduler": "Scheduler"
     },
     "windows": {
@@ -48,8 +66,14 @@
   },
   "NetworkTables": {
     "transitory": {
+      "LiveWindow": {
+        "open": true
+      },
       "Shuffleboard": {
         "open": true
+      },
+      "SmartDashboard": {
+        "open": true
       }
     }
   }

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

@@ -31,9 +31,9 @@ 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.1;
+    public static final double kRampRate = 0;
 
-    public static double maxSpeed = 0.8;
+    public static double maxSpeed = 1;
     public static final double maxSpeedIncrement = 0.05;
 
     public static final String kBaseType = "tank";

src/main/java/frc/robot/Interfaces/CANSparkMax.java

@@ -1,5 +1,6 @@
 package frc.robot.Interfaces;
 
+import com.revrobotics.REVLibError;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 
@@ -253,6 +254,9 @@ public void initSendable(SendableBuilder builder) {
     // and I don't think it'll work. If I put outside, I'll be able to call it in other places too.
     builder.addBooleanProperty("Brake Mode", this::isBraking, this::setBrakeMode);
     builder.addBooleanProperty("Coast Mode", this::isCoasting, this::setCoastMode);
+
+    // current limits
+    builder.addIntegerProperty("Current Limit", null, this::setSmartCurrentLimit);
   }
 
   /**
@@ -311,4 +315,9 @@ public void setCoastMode(boolean coast) {
       setIdleMode(IdleMode.kBrake);
     }
   }
+
+  public REVLibError setSmartCurrentLimit(Long limit) {
+    throwIfClosed();
+    return setSmartCurrentLimit(limit.intValue(), 0, 20000);
+  }
 }

src/main/java/frc/robot/Robot.java

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -31,6 +33,9 @@ public void robotInit() {
     m_robotContainer = new RobotContainer();
     // Test values for the servo on startup (This is just a test that should be removed if it
     // actually works)
+
+    DataLogManager.start();
+    DriverStation.startDataLog(DataLogManager.getLog());
   }
 
   /**
@@ -58,7 +63,6 @@ public void disabledInit() {}
   public void disabledPeriodic() {}
 
   /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
-
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();

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

@@ -40,7 +40,7 @@ public class RobotContainer {
   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 PlaceConeSecondLevelCommand m_PlaceConeSecondLevelCommand = 
+  private final PlaceConeSecondLevelCommand m_PlaceConeSecondLevelCommand =
       new PlaceConeSecondLevelCommand(m_Tank, m_Arm);
   private final IncreaseMaxSpeedCommand m_IncreaseMaxSpeedCommand =
       new IncreaseMaxSpeedCommand(m_Tank);
@@ -87,15 +87,14 @@ private void configureButtonBindings() {
     new JoystickButton(m_driverController, XboxController.Button.kBack.value)
         .whileTrue(m_DecreaseMaxSpeedCommand);
 
-    new JoystickButton(m_armController2, XboxController.Button.kLeftBumper.value)
+    new JoystickButton(m_armController2, XboxController.Button.kX.value)
         .whileTrue(m_CloseClawCommand);
-    new JoystickButton(m_armController2, XboxController.Button.kRightBumper.value)
+    new JoystickButton(m_armController2, XboxController.Button.kY.value)
         .whileTrue(m_OpenClawCommand);
-    new JoystickButton(m_armController2, XboxController.Button.kA.value)
+    new JoystickButton(m_armController2, XboxController.Button.kLeftBumper.value)
         .whileTrue(m_FineDriveCommand);
-    new JoystickButton(m_armController2,XboxController.Button.kX.value)
-        .whileTrue(m_PlaceConeSecondLevelCommand);    
-
+    new JoystickButton(m_armController2, XboxController.Button.kB.value)
+        .whileTrue(m_PlaceConeSecondLevelCommand);
   }
 
   public static double getDriverControllerLeftStickY() {

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

@@ -7,12 +7,12 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.subsystems.*;
 
-
 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), new BalanceCommand(m_pid, m_driveBase));
-
-    //new PlaceCubeFirstLevelCommand(m_driveBase, m_claw, m_arm)
-    //new PlaceConeSecondLevelCommand(m_driveBase, m_arm)
+    addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm));
+
+    // 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/BalanceCommand.java

@@ -44,15 +44,15 @@ public void initialize() {
   // avoid while loops inside execute
   @Override
   public void execute() {
-    double pitchOffset = pid.gyro.getPitch() - initPitch;
+    double pitchOffset = initPitch - pid.gyro.getPitch();
     SmartDashboard.putNumber("pitch offset", pitchOffset);
 
     if (pitchOffset > max) {
       max = pitchOffset;
     }
     SmartDashboard.putNumber("max offset", max);
 
-    if (pitchOffset < 17 && !onRamp) {
+    if (pitchOffset < 16 && !onRamp) {
       driveBase.setAllMotors(0.5);
       SmartDashboard.putBoolean("onRamp", onRamp);
 
@@ -63,8 +63,7 @@ public void execute() {
       if (Math.abs(pitchOffset) < 2) {
         driveBase.setAllMotors(0);
       } else {
-        driveBase.setAllMotors(
-            Constants.OperatorConstants.Pconstant * (pitchOffset));
+        driveBase.setAllMotors(Constants.OperatorConstants.Pconstant * (pitchOffset));
 
         SmartDashboard.putNumber(
             "motor speed", Constants.OperatorConstants.Pconstant * (pitchOffset));

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

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

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

@@ -47,12 +47,12 @@ public void execute() {
       }
 
       // go over charge station
-      if (Math.abs(drive.getAverageDistance()) < 7 && firstStep == false) {
-        drive.setAllMotors(0.4); // move back so that cone falls in
-        arm.armMotor.set(-0.3);
+      if (Math.abs(drive.getAverageDistance()) < 4.6 && firstStep == false) {
+        drive.setAllMotors(-0.3); // move back so that cone falls in
+        arm.armMotor.set(-0.45);
       }
 
-      if (Math.abs(drive.getAverageDistance()) >= 7) {
+      if (Math.abs(drive.getAverageDistance()) >= 4.6) {
         drive.setAllMotors(0);
       }
     }
@@ -72,6 +72,6 @@ public void end(boolean interrupted) {
 
   @Override
   public boolean isFinished() {
-    return Math.abs(drive.getAverageDistance()) >= 7;
+    return Math.abs(drive.getAverageDistance()) >= 4.6;
   }
 }

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

@@ -24,7 +24,7 @@ public class Arm extends SubsystemBase {
   public Arm() {
     armMotor = new CANSparkMax(7, MotorType.kBrushless);
     armMotor.setIdleMode(IdleMode.kBrake);
-    armMotor.setSmartCurrentLimit(20, 30);
+    armMotor.setSmartCurrentLimit(10, 40);
     armMotor.burnFlash();
     addChild("Arm Motor", armMotor);
 

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

@@ -27,7 +27,7 @@ public class Claw extends SubsystemBase {
   public Claw() {
     clawMotor = new CANSparkMax(8, MotorType.kBrushless);
     clawMotor.setIdleMode(IdleMode.kBrake);
-    clawMotor.setSmartCurrentLimit(18, 11);
+    clawMotor.setSmartCurrentLimit(20, 11);
     clawMotor.burnFlash();
     addChild("Claw Motor", clawMotor);
 

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

@@ -54,6 +54,8 @@ public class Tank extends SubsystemBase {
   private GenericEntry allWidget;
 
   private GenericEntry driveWidget;
+
+  private int initialCurrentLimit = 50;
   /** */
   public Tank() {
     motorsTab = Shuffleboard.getTab("Motors");
@@ -62,8 +64,8 @@ public Tank() {
     leftFront.restoreFactoryDefaults();
     leftFront.setInverted(true);
     leftFront.setIdleMode(IdleMode.kBrake);
-    leftFront.setSmartCurrentLimit(40);
-    // leftFront.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
+    leftFront.setSmartCurrentLimit(initialCurrentLimit);
+    leftFront.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
     leftFront.burnFlash();
     motorsTab.add("Left Front", leftFront);
     addChild("Left Front", leftFront);
@@ -72,8 +74,8 @@ public Tank() {
     leftRear.restoreFactoryDefaults();
     leftRear.setInverted(true);
     leftRear.setIdleMode(IdleMode.kBrake);
-    leftRear.setSmartCurrentLimit(40);
-    // leftRear.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
+    leftRear.setSmartCurrentLimit(initialCurrentLimit);
+    leftRear.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
     leftRear.burnFlash();
     motorsTab.add("Left Rear", leftRear);
     addChild("Left Rear", leftRear);
@@ -87,8 +89,8 @@ public Tank() {
     rightFront.setInverted(false);
 
     rightFront.setIdleMode(IdleMode.kBrake);
-    rightFront.setSmartCurrentLimit(40);
-    // rightFront.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
+    rightFront.setSmartCurrentLimit(initialCurrentLimit);
+    rightFront.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
     rightFront.burnFlash();
     motorsTab.add("Right Front", rightFront);
     addChild("Right Front", rightFront);
@@ -97,8 +99,8 @@ public Tank() {
     rightRear.restoreFactoryDefaults();
     rightRear.setInverted(false);
     rightRear.setIdleMode(IdleMode.kBrake);
-    rightRear.setSmartCurrentLimit(40);
-    // rightRear.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
+    rightRear.setSmartCurrentLimit(initialCurrentLimit);
+    rightRear.setOpenLoopRampRate(Constants.CanConstants.kRampRate);
     rightRear.burnFlash();
     motorsTab.add("Right Rear", rightRear);