Merge branch 'incorrectBallNew' into AlexS/Shooter_CollectorFunctiona…

…lity
FRC8592 · Mar 4, 2022 · 4173cc1 · 4173cc1
2 parents 0568b62 + cbe566c
commit 4173cc1
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 9 deletions.
diff --git a/SwerveCode/src/main/java/frc/robot/Robot.java b/SwerveCode/src/main/java/frc/robot/Robot.java
@@ -39,7 +39,7 @@ public class Robot extends TimedRobot {
   public Locality locality; 
   public Shooter shooter;
   public Collector collector;
- // public ColorSensor color;
+  public ColorSensor cSensor;
 
 
   /**
@@ -58,7 +58,7 @@ public void robotInit() {
     ball              = new ballTargeting();
     locality          = new Locality(0, 0);
     shooter           = new Shooter();
-    //color             = new ColorSensor();
+    cSensor             = new ColorSensor();
 
     //ball.setLimelightAllianceColor(ALLIANCE_COLOR.RED);
 
@@ -149,11 +149,12 @@ else if(driverController.getLeftBumper() == true) {
 
 
     SmartDashboard.putNumber("Rotate", rotate);
-    /*color.getColors();
-    if(color.getProximity() > 200){
-      color.updateCurrentBallColor();
-      { 
-        */
+    cSensor.getColors();
+    if(cSensor.getProximity() > 200){
+      cSensor.updateCurrentBallColor();
+    }
+
+    shooter.compareBallToAlliance();
 
   }
     /** This function is called once when the robot is disabled. */

diff --git a/SwerveCode/src/main/java/frc/robot/Shooter.java b/SwerveCode/src/main/java/frc/robot/Shooter.java
@@ -29,6 +29,9 @@ public class Shooter{
     public WPI_TalonFX flyWheelLeft;
     public WPI_TalonFX flyWheelRight;
     public WPI_TalonFX staging;
+    public ColorSensor cSensor;
+    //motor groups
+    MotorControllerGroup flyWheel;
 
     public enum ShooterState{AUTONOMOUS,SHOOT}
     private ShooterState shooterState;
@@ -95,9 +98,16 @@ public void manualControl(){
         collector.driveProcessingWheels(1);
         SmartDashboard.putNumber("Running Manual", 1);
     }
-}
 
 
-
+    public void compareBallToAlliance(XboxController shooterController){
+        if(cSensor.compareBallToAlliance() == false){
+            flyWheel.set(shooterController.getRightTriggerAxis()*0.2);
+        }
+        if(cSensor.compareBallToAlliance() == true){
+            flyWheel.set(flyWheel);
+        }
+    }
+}