fixed prints and cameras

Author: Jonathan

src/main/java/frc/team5115/Classes/Hardware/HardwareIntake.java

@@ -59,7 +59,7 @@ public void close(){
     private void setLights(boolean wantsCones) {
         coneLight.set(wantsCones);
         cubeLight.set(!wantsCones);
-        System.out.println(wantsCones);
+        //System.out.println(wantsCones);
     }
 
 }

src/main/java/frc/team5115/Classes/Software/Drivetrain.java

@@ -154,7 +154,7 @@ else if (Math.abs(y) > 1){
 
     public boolean TankDriveToAngle(double angleDegrees) { 
         double rotationDegrees = navx.getYawDeg();
-        System.out.println(rotationDegrees-angleDegrees);
+        System.out.println("remaining degrees: " + (rotationDegrees-angleDegrees));
         double turn = MathUtil.clamp(anglePID.calculate(rotationDegrees, angleDegrees), -0.6, 0.6);
         leftSpeed = turn;
         rightSpeed = -turn;

src/main/java/frc/team5115/Commands/Auto/AutoCommandGroup.java

@@ -25,7 +25,6 @@ public AutoCommandGroup(Drivetrain drivetrain, Arm arm, HardwareArm hArm, Hardwa
         this.drivetrain = drivetrain;
         this.hArm = hArm;
         this.hIntake = hIntake;
-        System.out.println(inIdealPosition);
 /* 
         addCommands(
             new Stow(arm, hArm, hIntake),

src/main/java/frc/team5115/Commands/Intake/RawIntakeCommands/IntakeExtend_v2.java

@@ -46,7 +46,7 @@ public void initialize() {
             //topLength_target = 20;
             //bottomLength_target = 20;
         }
-        System.out.println(intake.h.open);
+        //System.out.println(intake.h.open);
     }
 
 
@@ -212,18 +212,15 @@ else if((is_overhung)){
                 if(is_extending){
                     step_bottom();
                     pause_top();
-                    System.out.println("Extending: " + is_extending);
                 }
                 else{
                     pause_bottom();
                     step_top();   
-                    System.out.println("Extending: " + is_extending);
                 }
             }
         else {// if((Math.abs(delta_length) < Math.abs(max_delta_length_when_seperated)) && Math.abs(min_delta_length_when_seperated) < Math.abs(delta_length))
             step_bottom();
             step_top();
-            System.out.println(is_overhung);
         }
 
     }
@@ -234,7 +231,7 @@ else if((is_overhung)){
 
     public void end(boolean interrupted){
         intake.resetPID();
-        System.out.println("Stopped");
+        System.out.println("Stopped in IntakeExtend_v2");
     }
 
     public boolean isFinished() {

src/main/java/frc/team5115/Commands/Intake/RawIntakeCommands/IntakeTurn.java

@@ -24,11 +24,11 @@ public void initialize() {
     }
 
     public void execute(){
-        System.out.println(intake.getTurnDeg());
+        System.out.println("Arm Rotation Degrees: " + intake.getTurnDeg());
     }
 
     public void end(boolean interrupted){
-        System.out.println("Stopped");
+        System.out.println("Stopped in IntakeTurn");
     }
 
     public boolean isFinished() {

src/main/java/frc/team5115/Commands/Intake/Startup_Angle.java

@@ -29,7 +29,7 @@ public void execute(){
     }
 
     public void end(boolean interrupted){
-        System.out.println("Stopped");
+        System.out.println("Stopped in Startup_Angle");
         intake.zeroArm();
         intake.stop();
         intake.setLength(0);

src/main/java/frc/team5115/Commands/Intake/Startup_Intake.java

@@ -33,7 +33,7 @@ public void execute(){
     }
 
     public void end(boolean interrupted){
-        System.out.println("Stopped");
+        System.out.println("Stopped in Startup_Intake");
         intake.zeroArm();
         intake.stop();
         intake.setLength(0);

src/main/java/frc/team5115/Robot/Robot.java

@@ -25,7 +25,7 @@ public void robotPeriodic() {
     public void disabledInit() {
         robotContainer.disabledInit();
         robotContainer.stopEverything();
-        CameraServer.startAutomaticCapture();
+        //CameraServer.startAutomaticCapture();
     }
 
     @Override
@@ -34,7 +34,7 @@ public void disabledPeriodic() {
 
     @Override
     public void autonomousInit() {
-        CameraServer.startAutomaticCapture();
+      //  CameraServer.startAutomaticCapture();
         robotContainer.startAuto();
     }
 
@@ -45,7 +45,7 @@ public void autonomousPeriodic() {
 
     public void teleopInit () {
         robotContainer.startTeleop();
-        CameraServer.startAutomaticCapture();
+        //CameraServer.startAutomaticCapture();
     }
 
     public void teleopPeriodic () {

src/main/java/frc/team5115/Robot/RobotContainer.java

@@ -117,6 +117,7 @@ public void startAuto(){
         drivetrain.resetNAVx();
         goodAuto = good.getBoolean(false);
         //startup.schedule();
+        System.out.println("Good auto? " + goodAuto + "!!!!!!!");
         drivetrain.stop();
         autoCommandGroup = new AutoCommandGroup(drivetrain, arm, hardwareArm, intake, goodAuto);
         autoCommandGroup.schedule();