hopefully works

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

@@ -41,7 +41,7 @@ public static final class CanConstants {
 
   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/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,20 @@ 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)
         .whileTrue(m_CloseClawCommand);
     new JoystickButton(m_armController2, XboxController.Button.kRightBumper.value)
         .whileTrue(m_OpenClawCommand);
-
     new JoystickButton(m_armController2, XboxController.Button.kA.value)
         .whileTrue(m_FineDriveCommand);
+    new JoystickButton(m_armController2,XboxController.Button.kX.value)
+        .whileTrue(m_PlaceConeSecondLevelCommand);    
+
   }
 
   public static double getDriverControllerLeftStickY() {

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

@@ -10,7 +10,7 @@
 
 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));
+    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/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() * 0.7);
 
     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,14 @@
 
 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 readyToEnd = false;
+  private boolean firstStep = true;
 
   public PlaceConeSecondLevelCommand(Tank drive, Arm arm) {
     this.drive = drive;
@@ -32,37 +35,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()) < 7 && firstStep == false) {
+        drive.setAllMotors(0.4); // move back so that cone falls in
+        arm.armMotor.set(-0.3);
+      }
+
+      if (Math.abs(drive.getAverageDistance()) >= 7) {
+        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()) >= 7;
   }
 }

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

@@ -26,6 +26,7 @@ public Arm() {
     armMotor.setIdleMode(IdleMode.kBrake);
     armMotor.setSmartCurrentLimit(20, 30);
     armMotor.burnFlash();
+    addChild("Arm Motor", armMotor);
 
     Shuffleboard.getTab("Motors").add("Arm", armMotor);
   }

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

@@ -29,6 +29,7 @@ public Claw() {
     clawMotor.setIdleMode(IdleMode.kBrake);
     clawMotor.setSmartCurrentLimit(18, 11);
     clawMotor.burnFlash();
+    addChild("Claw Motor", clawMotor);
 
     Shuffleboard.getTab("Motors").add("Claw", clawMotor);
 

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

@@ -4,6 +4,7 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -28,9 +29,12 @@ public PID() {
     toleranceDeg = 0.5;
     iLimit = 2.0;
     gyro = new AHRS(SPI.Port.kMXP);
+    addChild("Gyro", gyro);
+    Shuffleboard.getTab("Driver").add("Gyro", gyro);
 
     initPitch = gyro.getPitch();
     pidController = new PIDController(P, I, D);
+    addChild("PID Controller", pidController);
   }
 
   @Override

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

@@ -21,6 +21,7 @@ public Turret() {
     xMotor.setIdleMode(IdleMode.kBrake);
     xMotor.setSmartCurrentLimit(5, 10);
     xMotor.burnFlash();
+    addChild("Turret Motor", xMotor);
     Shuffleboard.getTab("Motors").add("Turret", xMotor);
   }