Alex s/shooter collector functionality

SwerveCode/src/main/java/frc/robot/Collector.java

@@ -3,52 +3,68 @@
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Collector {
 
+
     private DigitalInput lineSensorTop;
     private DigitalInput lineSensorBottom;
     WPI_TalonFX processing;
     WPI_TalonFX staging;
+    WPI_TalonFX collectorArm;
     public enum CollectorState{ONE_BALL_BOTTOM, ONE_BALL_TOP, TWO_BALLS, NO_BALLS_LOADED}
     private CollectorState collectorState;
 
     //This class will contain all new collector specific methods
     public Collector(){
-        WPI_TalonFX processing = new WPI_TalonFX(Constants.newFlywheelCollector);
-        WPI_TalonFX staging = new WPI_TalonFX(Constants.newFlywheelStaging);
+        collectorArm = new WPI_TalonFX(Constants.newCollectorArm);
+        processing = new WPI_TalonFX(Constants.newFlywheelCollector);
+        staging = new WPI_TalonFX(Constants.newFlywheelStaging);
         lineSensorBottom = new DigitalInput(Constants.LINE_BREAK_BOTTOM_SENSOR_PORT);
         lineSensorTop = new DigitalInput(Constants.LINE_BREAK_TOP_SENSOR_PORT);
+
+        processing.setInverted(true);
+        staging.setInverted(true);
+    }
+
+    public void raiseCollectorArm(){
+        this.collectorArm.set(-0.);
+    }
+
+    public void lowerCollectorArm(){
+        this.collectorArm.set(0.05);
     }
 
     //Drives the processing wheels for state machine
-    public void driveProcessingWheels(){
-        processing.set(0.5);
+    public void driveProcessingWheels(double speed){
+        this.processing.set(speed);
     }
 
     //Drives staging wheel for state machine
-    public void driveStagingWheels(){
-        processing.set(0.5);
+    public void driveStagingWheels(double speed){
+        this.staging.set(speed);
     }
 
     //Stops processing wheels for state machine
     public void stopProcessingWheels(){
-        processing.set(0);
+        this.processing.set(0);
     }
 
+
     //Stops Staging wheels for state machine
     public void stopStagingWheels(){
-        processing.set(0);
+        this.staging.set(0);
     }
 
     //Manually reverses the Staging wheels
     public void reverseStagingWheels(){
-        staging.set(-1);
+        this.staging.set(-0.2);
     }
 
     //Manually reverses the Processing wheels
     public void reverseProcessingWheels(){
-        processing.set(-1);
+        this.processing.set(-0.2);
     }
 
     //Stops entire intake system if needed
@@ -59,8 +75,8 @@ public void intakeAllStop(){
 
     //This runs the intake system in its entirety if needed
     public void intakeAllRun(){
-        this.driveProcessingWheels();
-        this.driveStagingWheels();
+        this.driveProcessingWheels(0.2);
+        this.driveStagingWheels(0.2);
     }
 
     //reverses intake entirely in the event we need it
@@ -71,51 +87,68 @@ public void intakeReverse(){
 
     public CollectorState determineCollectorState(){
         boolean topState = lineSensorTop.get();
+        SmartDashboard.getBoolean("LineSensorTop", topState);
         boolean bottomState = lineSensorBottom.get();
-
+        SmartDashboard.getBoolean("LineSensorBottom", bottomState);
 
         //this makes sure to tell which states are which
-        if(bottomState){
-            if(topState){
+        if(!bottomState){
+            if(!topState){
                 collectorState = CollectorState.TWO_BALLS;
             }else{
                 collectorState = CollectorState.ONE_BALL_BOTTOM;
             }
         }else{
-            if(topState){
+            if(!topState){
                 collectorState = CollectorState.ONE_BALL_TOP;
             }else{
                 collectorState = CollectorState.NO_BALLS_LOADED;
             }
         }
 
 
+
+        SmartDashboard.putString("Collector State", collectorState.toString());
         return collectorState;
     }
 
     public void ballControl(){
+
         //this is a simple state machine controlling what ball wheels run and when
         switch(this.determineCollectorState()){
             case NO_BALLS_LOADED: //when there are no balls loaded we want to run the processing wheels to collect 1 ball
-                this.driveProcessingWheels();
+                this.driveProcessingWheels(0.2);
+                this.driveStagingWheels(0.2);
+                this.lowerCollectorArm(); 
             break;
 
             case ONE_BALL_BOTTOM: //when there is one ball at the bottom we want to stop the wheels pushing it in and start driving the middle wheels
-                this.stopProcessingWheels();
-                this.driveStagingWheels();
+                this.driveProcessingWheels(0.2);
+                this.driveStagingWheels(0.2);
             break;
-
+             
             case ONE_BALL_TOP: //when theres one ball at the top we want to make sure that the staging wheels don't move the ball and run the bottom wheels to collect another
                 this.stopStagingWheels();
-                this.driveProcessingWheels();
+                this.driveProcessingWheels(0.2);
             break;
 
             case TWO_BALLS: //when we have 2 balls we don't want to run any of the intake modules
                 this.intakeAllStop();
+                this.raiseCollectorArm();
             break;
-        }
+
+
+        }   
+
+
+
+        SmartDashboard.putBoolean("LineSensorTop", lineSensorTop.get());
+        SmartDashboard.putBoolean("LineSensorBottom", lineSensorBottom.get());
     }
 
+    public void manualControl(){
+
+    }
 
 
 }

SwerveCode/src/main/java/frc/robot/ColorSensor.java

@@ -1,5 +1,5 @@
 //code by Zolton
-package frc.robot;
+/*package frc.robot;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.I2C;
@@ -78,4 +78,5 @@ public String toString(){
         }
         return s;
     }
-}
+}
+*/

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

@@ -58,16 +58,37 @@ public final class Constants {
     //
     // CAN IDs for the ball collector and launching system
     //
-    public static final int newFlywheelLeft      = 22;
+    public static final int newFlywheelLeft      = 23;
     public static final int newFlywheelRight     = 20;
-    public static final int newFlywheelStaging   = 21;
-    public static final int newFlywheelCollector = 23;
+    public static final int newFlywheelStaging   = 22;
+    public static final int newFlywheelCollector = 21;
+    public static final int newCollectorArm = 24; //Change this I don't know what the arm CAN is
 
     //IR sensor ports
-    public static final int LINE_BREAK_TOP_SENSOR_PORT = -2;
-    public static final int LINE_BREAK_BOTTOM_SENSOR_PORT = -1;
+    public static final int LINE_BREAK_TOP_SENSOR_PORT = 0;
+    public static final int LINE_BREAK_BOTTOM_SENSOR_PORT = 1;
 
     public static enum ALLIANCE_COLOR {
         BLUE, RED, NONE
     }
+
+    //ringVision constants
+    public static double RING_LOCK_ERROR       = 1.0;
+    public static double TURRET_ERROR          = 0.5;           // Allowed aiming error in degrees
+    public static double RING_CAMERA_HEIGHT    = 36.0;            // Limelight height above ground (inches)
+    public static double RING_CAMERA_ANGLE     = 0.0;           // Limelight camera angle above horizontal (degrees)
+    public static double RING_TARGET_HEIGHT    = 104;           // Center of target above ground (inches)
+    public static double TURRET_ROTATE_KP      = 1.0;          // 15.6 Proportional constant for turret rotate speed
+    public static String LIMELIGHT_RING        = "limelight-ring";
+
+    //ballVision constants
+    public static double BALL_LOCK_ERROR       = 2.0;
+    public static double BALL_ERROR            = 0.5;           // Allowed aiming error in degrees
+    public static double BALL_CAMERA_HEIGHT    = 0.0;
+    public static double BALL_CAMERA_ANGLE     = 0.0;
+    public static double BALL_TARGET_HEIGHT    = 0.0;
+    public static double BALL_ROTATE_KP        = 7.0;   // Proportional constant for turret rotate speed
+    public static String LIMELIGHT_BALL        = "limelight-ball";
+    public static double MIN_TURN_SPEED        = 0.75;
+    public static double MAX_TURN_SPEED        = 0.5;
 }

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

@@ -13,7 +13,8 @@
 import edu.wpi.first.wpilibj.XboxController; //this puts in the xbox contoller stuff
 import frc.robot.Shooter;
 import frc.robot.Constants.ALLIANCE_COLOR;
-
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.math.geometry.Rotation2d;
 /**
  * The VM is configured to automatically run this class, and to call the
  * functions corresponding to
@@ -34,12 +35,12 @@ public class Robot extends TimedRobot {
 
   public Drivetrain drive;
   public Autonomous autonomous;
-  public Vision vision;
-  public ballTargeting ball;
+  public Vision visionRing;
+  public Vision visionBall;
   public Locality locality; 
   public Shooter shooter;
   public Collector collector;
-  public ColorSensor color;
+ // public ColorSensor color;
 
 
   /**
@@ -54,14 +55,13 @@ public void robotInit() {
     driverController  = new XboxController(0); 
     shooterController = new XboxController(1);
     drive             = new Drivetrain();
-    vision            = new Vision();
-    ball              = new ballTargeting();
+    visionRing        = new Vision(Constants.LIMELIGHT_RING, Constants.RING_LOCK_ERROR, Constants.RING_CAMERA_HEIGHT, Constants.RING_CAMERA_ANGLE, Constants.RING_TARGET_HEIGHT, Constants.TURRET_ROTATE_KP);
+    visionBall        = new Vision(Constants.LIMELIGHT_BALL, Constants.BALL_LOCK_ERROR, Constants.BALL_CAMERA_HEIGHT, Constants.BALL_CAMERA_ANGLE, Constants.BALL_TARGET_HEIGHT, Constants.BALL_ROTATE_KP);
     locality          = new Locality(0, 0);
     shooter           = new Shooter();
     color             = new ColorSensor();
-
-    ball.setLimelightAllianceColor(ALLIANCE_COLOR.RED);
-
+    //ball.setLimelightAllianceColor(ALLIANCE_COLOR.RED);
+    NetworkTableInstance.getDefault().getTable("limelight-ball").getEntry("pipeline").setNumber(1);
   }
 
   /**
@@ -125,40 +125,42 @@ public void teleopPeriodic(){
     double translateY;
     double rotate;
 
-    locality.updatePosition(drive.getYaw(), vision);
+    visionBall.updateVision();
+    visionRing.updateVision();
+
+    locality.updatePosition(drive.getYaw(), visionRing);
 
     // Read gamepad controls
     rotate     = (driverController.getRightX() * Drivetrain.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND) * ConfigRun.ROTATE_POWER;            // Right joystick
     translateX = (driverController.getLeftY() * Drivetrain.MAX_VELOCITY_METERS_PER_SECOND) * ConfigRun.TRANSLATE_POWER;             //X is forward Direction, Forward on Joystick is Y
     translateY = (driverController.getLeftX() * Drivetrain.MAX_VELOCITY_METERS_PER_SECOND) * ConfigRun.TRANSLATE_POWER;
 
-    shooter.testshooter(shooterController);
-
     ball.ballAim();
+    shooter.testshooter(shooterController);
 
     if(driverController.getRightBumper() == true){
-      drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(-joystickDeadband(translateX),-joystickDeadband(translateY), -vision.autoAim() , drive.getGyroscopeRotation()));     //Inverted due to Robot Directions being the opposite of controller directions 
+      drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(-joystickDeadband(translateX),
+      -joystickDeadband(translateY), -visionRing.turnRobot() , drive.getGyroscopeRotation()));     //Inverted due to Robot Directions being the opposite of controller directions 
     } 
     else if(driverController.getLeftBumper() == true) {
-      drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(0,0, -ball.ballAim() , drive.getGyroscopeRotation()));
+      drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(visionBall.moveTowardsTarget(), 0, visionBall.turnRobot(),
+      Rotation2d.fromDegrees(0)));
     }
      else {
-    drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(-joystickDeadband(translateX),-joystickDeadband(translateY), -joystickDeadband(rotate), drive.getGyroscopeRotation()));     //Inverted due to Robot Directions being the opposite of controller directions
+    drive.drive(ChassisSpeeds.fromFieldRelativeSpeeds(-joystickDeadband(translateX),
+    -joystickDeadband(translateY), -joystickDeadband(rotate), drive.getGyroscopeRotation()));     //Inverted due to Robot Directions being the opposite of controller directions
     }
 
-    //this makes sure that when the driver pushes the A button they can control the shooter directly, if not this runs the ball control
-    if(shooterController.getAButton()){
-      shooter.testshooter(shooterController);
-    }else{
-      collector.ballControl();
-    }
+
+    shooter.collectorDriverControl(shooterController);
+
 
     SmartDashboard.putNumber("Rotate", rotate);
-    color.getColors();
+    /*color.getColors();
     if(color.getProximity() > 200){
       color.updateCurrentBallColor();
-    }
-
+      { 
+        */
 
   }
     /** This function is called once when the robot is disabled. */