remove nonexistant intake (#39)

WTMCRobotics · Mar 7, 2024 · c901cc9 · c901cc9
1 parent 529a06f
commit c901cc9
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -5,6 +5,9 @@
 package frc.robot;
 
 import com.kauailabs.navx.frc.AHRS;
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.UsbCamera;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -17,6 +20,7 @@
 import frc.robot.auton.SequentialActionRunner;
 import frc.robot.motor.MotorController;
 import frc.robot.motor.MotorControllerFactory;
+import frc.robot.vision.Vision;
 // import frc.robot.vision.AprilTagHighlighter;
 import java.util.ArrayDeque;
 
@@ -71,7 +75,7 @@ public class Robot extends TimedRobot {
         Constants.RIGHT_CLIMB_ID,
         MotorController.Type.SparkMaxBrushed
     );
-    MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMaxBrushed);
+    // MotorController intake = MotorControllerFactory.create(Constants.INTAKE_ID, MotorController.Type.SparkMaxBrushed);
     MotorController hoodAdjuster = null;
     // = MotorControllerFactory.create(
     //     Constants.SHOOTER_HOOD_ADJUSTERER_ID,
@@ -100,6 +104,7 @@ public static AHRS getGyroscope() {
      */
     @Override
     public void robotInit() {
+        Vision vision = new Vision();
         initializeSmartMotion(driveLeftParent, Constants.NORMAL_ROBOT_GAINS);
         initializeSmartMotion(driveRightParent, Constants.NORMAL_ROBOT_GAINS);
 
@@ -112,10 +117,10 @@ public void robotInit() {
         autonRouteChooser.addOption("backup turn backup", AutonRoutes.BACKUP_TURN_BACKUP);
         autonRouteChooser.addOption("shoot and back up", AutonRoutes.SHOOT_AND_BACK_UP);
 
-        // autonRouteChooser.addOption("Test old rotation PID", AutonRoutes.TEST_ROTATION);
-        // autonRouteChooser.addOption("Test new rotation PID", AutonRoutes.TEST_ROTATION_WITH_PID_COMMAND);
-        // autonRouteChooser.addOption("Test SmartMotion movement", AutonRoutes.TEST_SMART_MOTION_MOVEMENT);
-        // autonRouteChooser.addOption("Test Manual PID movement", AutonRoutes.TEST_PID_MOVEMENT);
+        autonRouteChooser.addOption("Test old rotation PID", AutonRoutes.TEST_ROTATION);
+        autonRouteChooser.addOption("Test new rotation PID", AutonRoutes.TEST_ROTATION_WITH_PID_COMMAND);
+        autonRouteChooser.addOption("Test SmartMotion movement", AutonRoutes.TEST_SMART_MOTION_MOVEMENT);
+        autonRouteChooser.addOption("Test Manual PID movement", AutonRoutes.TEST_PID_MOVEMENT);
 
         SmartDashboard.putData("Auton Routes", autonRouteChooser);
 
@@ -157,7 +162,7 @@ public void robotInit() {
                 .rightFlywheel(rightFlywheel)
                 .leftClimb(leftClimb)
                 .rightClimb(rightClimb)
-                .intake(intake)
+                // .intake(intake)
                 .hoodAdjuster(hoodAdjuster);
         getGyroscope().reset();
         System.out.println(Constants.APRIL_TAG_CONFIDENCE_FRAMES);
@@ -169,6 +174,15 @@ public void robotInit() {
 
         System.out.println("Is drive right parent inverted at end?? " + driveRightParent.getInverted());
 
+        UsbCamera camera = CameraServer.startAutomaticCapture("Front Cam", 0);
+        camera.setFPS(15);
+
+        // Set the resolution
+        camera.setResolution(Constants.CAMERA_WIDTH, Constants.CAMERA_HEIGHT);
+
+        // Get a CvSink. This will capture Mats from the camera
+        CvSink cvSink = CameraServer.getVideo();
+
         InputtedCoDriverControls.setCoDriverController(coDriverController);
         InputtedDriverControls.setDriverController(driverController);
     }

diff --git a/src/main/java/frc/robot/RobotComponents/Intake.java b/src/main/java/frc/robot/RobotComponents/Intake.java
@@ -8,20 +8,24 @@ public class Intake {
 
     public static void autoIntake() {
         Robot.getTeleopActionRunner().removeActionsOfType(AutonIntake.class);
-        Robot.getTeleopActionRunner().addActionToRun(new AutonIntake());
+        // Robot.getTeleopActionRunner().addActionToRun(new AutonIntake());
     }
 
     public static void startFloorIntake() {
-        Robot.motors.getIntake().set(Constants.INTAKE_SPEED);
+        // Robot.motors.getIntake().set(Constants.INTAKE_SPEED);
+        Robot.motors.getLeftFlywheel().set(-0.25);
+        Robot.motors.getRightFlywheel().set(-0.25);
     }
 
     /* This will spin the motor backwards in an attempt to eject a stuck note*/
     public static void backtrack() {
-        Robot.motors.getIntake().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT);
+        // Robot.motors.getIntake().set(Constants.MOTOR_BACKTRACK_SPEED_PERCENT);
     }
 
     public static void stopFloorIntake() {
         Robot.getTeleopActionRunner().removeActionsOfType(AutonIntake.class);
-        Robot.motors.getIntake().stopMotor();
+        // Robot.motors.getIntake().stopMotor();
+        Robot.motors.getLeftFlywheel().set(0);
+        Robot.motors.getRightFlywheel().set(0);
     }
 }
diff --git a/src/main/java/frc/robot/auton/AutonIntake.java b/src/main/java/frc/robot/auton/AutonIntake.java
@@ -15,11 +15,11 @@ public boolean isDone() {
 
     @Override
     public void initiate() {
-        Robot.motors.getIntake().set(Constants.INTAKE_SPEED);
+        // Robot.motors.getIntake().set(Constants.INTAKE_SPEED);
     }
 
     @Override
     public void shutdown() {
-        Robot.motors.getIntake().stopMotor();
+        // Robot.motors.getIntake().stopMotor();
     }
 }
diff --git a/src/main/java/frc/robot/auton/AutonShoot.java b/src/main/java/frc/robot/auton/AutonShoot.java
@@ -12,7 +12,7 @@ public class AutonShoot extends AutonAction {
     public boolean isDone() {
         if (Timer.getFPGATimestamp() >= revFinishedTime) {
             Robot.motors.getFeeder().set(0.2);
-            Robot.motors.getIntake().set(0.2);
+            // Robot.motors.getIntake().set(0.2);
         }
 
         return Timer.getFPGATimestamp() >= revFinishedTime + 1.0;