Place game piece comms to final day

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,17 +31,17 @@ 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";
   }
 
   public static final class OperatorConstants {
     public static final int kDriverControllerPort = 0;
-    public static final double Pconstant = 0.00882;
+    public static final double Pconstant = 0.00635;
     public static final String m_driveControllerType = "accurateTank";
   }
 

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

@@ -39,15 +39,13 @@ public class RobotContainer {
   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 PlaceCubeFirstLevelCommand m_PlaceGamePieceCommand =
-      new PlaceCubeFirstLevelCommand(m_Tank, m_claw, m_Arm);
   private final AutoCommand m_AutoCommand = new AutoCommand(m_PID, m_Tank, m_claw, m_Arm);
+  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 SwitchIdleModeCommmand m_SwitchIdleModeCommmand =
-      new SwitchIdleModeCommmand(m_Tank);
   private final FineDriveCommand m_FineDriveCommand = new FineDriveCommand(m_Tank);
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
@@ -57,12 +55,23 @@ public RobotContainer() {
 
     // add a few things to the shuffleboard
     // joysticks
-    Shuffleboard.getTab("Driver").add("Left Stick Y", m_driverController.getRawAxis(1));
-    Shuffleboard.getTab("Driver").add("Right Stick X", m_driverController.getRawAxis(4));
-    Shuffleboard.getTab("Driver")
-        .add("Left Stick Y Adjusted", getDriverControllerLeftStickYAdjusted());
-    Shuffleboard.getTab("Driver")
-        .add("Right Stick X Adjusted", getDriverControllerRightStickXAdjusted());
+    var tab = Shuffleboard.getTab("Subsystems");
+    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_PID);
+    tab.add(m_PlaceConeSecondLevelCommand);
+    tab.add(m_Tank);
+    tab.add(m_Turret);
+    tab.add(m_claw);
   }
 
   /**
@@ -73,23 +82,19 @@ public RobotContainer() {
   private void configureButtonBindings() {
     new JoystickButton(m_driverController, XboxController.Button.kY.value)
         .whileTrue(m_BalanceCommand);
-    new JoystickButton(m_driverController, XboxController.Button.kA.value)
-        .whileTrue(m_PlaceGamePieceCommand);
-
     new JoystickButton(m_driverController, XboxController.Button.kStart.value)
         .whileTrue(m_IncreaseMaxSpeedCommand);
     new JoystickButton(m_driverController, XboxController.Button.kBack.value)
         .whileTrue(m_DecreaseMaxSpeedCommand);
-    new JoystickButton(m_driverController, XboxController.Button.kX.value)
-        .whileTrue(m_SwitchIdleModeCommmand);
 
-    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.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 PlaceCubeFirstLevelCommand(m_driveBase, m_claw, 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.65);
+        RobotContainer.getDriverControllerRightStickXAdjusted() * Constants.CanConstants.maxSpeed);
 
     SmartDashboard.putNumber("maxSpeed", Constants.CanConstants.maxSpeed);
   }

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

@@ -22,7 +22,7 @@ public void initialize() {}
 
   @Override
   public void execute() {
-    m_subsystem.rotateTurret(RobotContainer.getControllerRightStickX());
+    m_subsystem.rotateTurret(RobotContainer.getControllerRightStickX() / 1.5);
   }
 
   @Override

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

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

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

@@ -9,11 +9,11 @@
 
 public class PlaceConeSecondLevelCommand extends CommandBase {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final Tank drive; 
+  private final Tank drive;
+
   private final Arm arm;
 
-  private boolean readyToMoveArmBack = false;
-  private boolean end = false;
+  private boolean firstStep = true;
 
   public PlaceConeSecondLevelCommand(Tank drive, Arm arm) {
     this.drive = drive;
@@ -32,37 +32,46 @@ public void initialize() {
     arm.armMotor.getEncoder().setPosition(0);
   }
 
-
-  //double check constants
+  // double check constants
   @Override
   public void execute() {
-    if (Math.abs(arm.armMotor.getEncoder().getPosition()) < 80) {
-          arm.armMotor.set(0.4); //move the arm until cone gets in 
-    } else {
-      if(Math.abs(drive.getAverageDistance()) < 10){
-        drive.arcadeDrive(-2, 0); //move back so that cone falls in
-      } else{
-        drive.arcadeDrive(0, 0);; //stop moving 
-        readyToMoveArmBack = true; //ready to move the arm back and continue balance
+    if (firstStep) {
+      if (Math.abs(arm.armMotor.getEncoder().getPosition()) < 105) {
+        arm.armMotor.set(0.4);
+      }
+
+      if (Math.abs(arm.armMotor.getEncoder().getPosition()) >= 105) {
+        firstStep = false;
+        arm.armMotor.set(0);
         arm.armMotor.getEncoder().setPosition(0);
       }
-    }
 
-    if(readyToMoveArmBack){
-      if(Math.abs(arm.armMotor.getEncoder().getPosition()) < 80){
-        arm.armMotor.set(-0.4); //moves the arm back to starting position
-        end = true;
+      // go over charge station
+      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()) >= 4.6) {
+        drive.setAllMotors(0);
       }
     }
   }
 
   @Override
   public void end(boolean interrupted) {
+    drive.leftFront.getEncoder().setPosition(0);
+    drive.leftRear.getEncoder().setPosition(0);
+    drive.rightFront.getEncoder().setPosition(0);
+    drive.rightRear.getEncoder().setPosition(0);
+    arm.armMotor.getEncoder().setPosition(0);
+
+    drive.setAllMotors(0);
     arm.armMotor.set(0); // stop the arm motor
   }
 
   @Override
   public boolean isFinished() {
-    return end;
+    return Math.abs(drive.getAverageDistance()) >= 4.6;
   }
 }