work please +1

Author: twistybit

src/main/java/frc/team5115/Auto/AutoCommands/AssistedShootHighGoal.java

@@ -0,0 +1,69 @@
+package frc.team5115.Auto.AutoCommands;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.team5115.Subsystems.Drivetrain;
+import frc.team5115.Subsystems.Limelight;
+import frc.team5115.Subsystems.Locationator;
+import frc.team5115.Subsystems.Shooter;
+
+import static frc.team5115.Constants.JOYSTICK_Y_AXIS_ID;
+import static frc.team5115.Constants.Pipeline;
+
+
+public class AssistedShootHighGoal extends CommandBase {
+
+    Drivetrain drivetrain;
+    Locationator locationator;
+    Shooter shooter;
+    Limelight limelight;
+    Joystick joystick;
+
+    /*
+    1. Aim at the thing using the limelight
+    2. Shoot the balls while stopped.
+     */
+
+    public AssistedShootHighGoal(Drivetrain drivetrain, Shooter shooter, Limelight limelight, Joystick joystick) {
+        this.drivetrain = drivetrain;
+        this.shooter = shooter;
+        this.limelight = limelight;
+        this.joystick = joystick;
+        addRequirements(drivetrain, shooter);
+        limelight.setPipeline(Pipeline.GreenLedMode);
+        System.out.println("Starting goal assist but not throttle constructor");
+
+    }
+
+    @Override
+    public void initialize() {
+        System.out.println("Starting High Goal Aiming but not throttle");
+    }
+
+    @Override
+    public void execute() {
+        double angle = 127;
+        if (limelight.hasTarget() && limelight.getYAngle() > 0) { // if we don't have a target
+            angle = limelight.getXAngle();
+        } else {
+            System.out.println("Error No Target Found"); //todome set to shuffleboard.
+            System.out.println(limelight.getYAngle());
+            drivetrain.XBoxDrive(joystick);
+            return;
+        }
+        //System.out.println("angle = " + (angle - locationator.getAngle()));
+        drivetrain.relativeAngleHold(angle, -joystick.getRawAxis(JOYSTICK_Y_AXIS_ID));
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        if (interrupted) System.out.println("Error: Interrupted");
+        drivetrain.stop();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false; //never needed, command ends itself.
+    }
+}

src/main/java/frc/team5115/Auto/AutoCommands/DriveDistance.java

@@ -32,7 +32,7 @@ public void execute() {
         System.out.println("targetLocation = " + targetLocation);
         double angle = locationator.getCurrentLocation().angleFrom(targetLocation);
         double throttle = Drivetrain.clamp(locationator.getCurrentLocation().distanceFrom(targetLocation)/50 * Drivetrain.clamp(Math.abs((25/(angle - locationator.getAngle()))),1),
-                Constants.MAX_AUTO_THROTTLE);
+                Constants.AUTO_MAX_THROTTLE);
         System.out.println("Angle hold: " + angle + "  Throttle: " + throttle);
         drivetrain.angleHold(angle,throttle);
     }

src/main/java/frc/team5115/Auto/AutoCommands/FollowGreen.java

@@ -57,8 +57,8 @@ public void execute() {
                 return;
             }
 
-            throttle = -(SHOOTIN_DISTANCE - limelight.calculateDistanceFromBase())/100;
-            throttle = Drivetrain.clamp(throttle, MAX_AUTO_THROTTLE); //max speed 0.5. Also add a minimum speed of 0.1.
+            throttle = -(AUTO_SHOOTIN_DISTANCE - limelight.calculateDistanceFromBase())/100;
+            throttle = Drivetrain.clamp(throttle, AUTO_MAX_THROTTLE); //max speed 0.5. Also add a minimum speed of 0.1.
             //System.out.println("Distance from the base: " + limelight.calculateDistanceFromBase() + " throttle: " + throttle);
             throttle = 0; //eliminate to return forward backward handling.
             drivetrain.angleHold(angle, throttle);

src/main/java/frc/team5115/Auto/AutoCommands/PickupBallAuto.java

@@ -1,36 +1,27 @@
 package frc.team5115.Auto.AutoCommands;
 
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.team5115.Constants;
 import frc.team5115.Subsystems.Drivetrain;
+import frc.team5115.Subsystems.Feeder;
 import frc.team5115.Subsystems.Limelight;
 import frc.team5115.Subsystems.Locationator;
 
-import static frc.team5115.Constants.MAX_AUTO_THROTTLE;
+import static frc.team5115.Constants.*;
 
 public class PickupBallAuto extends CommandBase {
 
     Drivetrain drivetrain;
     Locationator locationator;
     Limelight limelight;
-    private final Joystick joystick;
+    Feeder feeder;
     boolean foundBall;
 
-    public PickupBallAuto(Drivetrain drivetrain, Locationator locationator, Limelight limelight, Joystick joystick) {
-        this.drivetrain = drivetrain;
-        this.locationator = locationator;
-        this.limelight = limelight;
-        this.joystick = joystick;
-        foundBall = false;
-    }
 
-    public PickupBallAuto(Drivetrain drivetrain, Locationator locationator, Limelight limelight) {
+    public PickupBallAuto(Drivetrain drivetrain, Locationator locationator, Limelight limelight, Feeder feeder) {
         this.drivetrain = drivetrain;
         this.locationator = locationator;
         this.limelight = limelight;
-        joystick = null;
+        this.feeder = feeder;
         foundBall = false;
     }
     /*
@@ -40,7 +31,7 @@ public PickupBallAuto(Drivetrain drivetrain, Locationator locationator, Limeligh
 
     @Override
     public void initialize() {
-        limelight.setPipeline(Constants.Pipeline.CustomGripPipeline);
+        limelight.setPipeline(Pipeline.Balls);
         drivetrain.stop();
     }
     /*on loop:
@@ -49,44 +40,37 @@ public void initialize() {
     double lastAngle;
     @Override
     public void execute() {
+        limelight.setPipeline(Pipeline.Balls);
+        System.out.println("working");
         double angle;
-        if (limelight.hasTarget() && limelight.getYAngle() + Constants.CAMERA_ANGLE < 0) { // if we have a target
+        if (limelight.hasTarget()) { // if we have a target
+            System.out.println("Have target");
             angle = limelight.getXAngle() + locationator.getAngle();
             foundBall = true;
             lastAngle = angle;
         } else {
-            System.out.println("Can't find a ball.");
+
             if(foundBall) //if we have found that shit before, go there.
-                drivetrain.angleHold(lastAngle, MAX_AUTO_THROTTLE);
+                drivetrain.angleHold(lastAngle, AUTO_MAX_THROTTLE);
             else
                 drivetrain.stop();
             return;
         }
-        boolean targetForDistance = false;
+
         //true means it rolls after it. It will stop if it stops. False means it just goes at a constant speed.
-        double throttle;
-        if(targetForDistance) {
-            double distanceFromBall = limelight.calculateDistanceFromBall();
-            throttle = -(30 - limelight.calculateDistanceFromBase())
-                    / 30;
-        }else {
-            throttle = MAX_AUTO_THROTTLE;
-        }
+        double throttle = AUTO_MAX_THROTTLE;
         System.out.println("throttle = " + throttle);
-        throttle = Drivetrain.clamp(throttle, MAX_AUTO_THROTTLE); //max speed 0.5. Also add a minimum speed of 0.1.
         //throttle = 0;
         drivetrain.angleHold(angle, throttle);
     }
 
     @Override
     public void end(boolean interrupted) {
-        if (interrupted) System.out.println("Error: Interrupted");
-        System.out.println("Done!");
         drivetrain.stop();
     }
 
     @Override
     public boolean isFinished() {
-        return false;
+        return false;//feeder.isBall();
     } //todome make this from the intake sensor math.
 }

src/main/java/frc/team5115/Auto/AutoCommands/ShootHighGoal.java

@@ -53,9 +53,9 @@ public void execute() {
 //                System.out.println("limelight = " + limelight.calculateDistanceFromBase());
 //                System.out.println("locationator = " + locationator.getUltrasonicDistanceInches());
 
-                throttle = -(SHOOTIN_DISTANCE - limelight.calculateDistanceFromBase())
+                throttle = -(AUTO_SHOOTIN_DISTANCE - limelight.calculateDistanceFromBase())
                         / 50;
-                throttle = Drivetrain.clamp(throttle, MAX_AUTO_THROTTLE); //max speed 0.5. Also add a minimum speed of 0.1.
+                throttle = Drivetrain.clamp(throttle, AUTO_MAX_THROTTLE); //max speed 0.5. Also add a minimum speed of 0.1.
                 System.out.println("Distance from the base: " + limelight.calculateDistanceFromBase() + " throttle: " + throttle);
                 //System.out.println("angle = " + (angle - locationator.getAngle()));
             } else {

src/main/java/frc/team5115/Auto/AutoSeries.java

@@ -1,10 +1,8 @@
-
 package frc.team5115.Auto;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.team5115.Auto.AutoCommands.DriveDistance;
-import frc.team5115.Auto.AutoCommands.PickupBallAuto;
 import frc.team5115.Auto.AutoCommands.ShootHighGoal;
+import frc.team5115.Constants;
 import frc.team5115.Subsystems.Drivetrain;
 import frc.team5115.Subsystems.Limelight;
 import frc.team5115.Subsystems.Locationator;
@@ -14,7 +12,8 @@ public class AutoSeries extends SequentialCommandGroup {
 
     public AutoSeries(Drivetrain drivetrain, Locationator locationator, Shooter shooter, Limelight limelight) {
 
-        System.out.println("Creating auto series.");
+        limelight.setPipeline(Constants.Pipeline.DriveCamera);
+
         final Loc2D overLineLocation = new Loc2D(
                 locationator.getCurrentLocation().getX(),  //goes strait forward.
                 120);
@@ -23,23 +22,12 @@ public AutoSeries(Drivetrain drivetrain, Locationator locationator, Shooter shoo
 
 
         //These commands do a basic auto series.
-        if(true) {
         addCommands(
                 new ShootHighGoal(drivetrain,
                         locationator,
                         shooter,
                         limelight)
-                /*
-                // Drive backward the specified distance
-                new DriveDistance(afterShootLocation,
-                        drivetrain,
-                        locationator)
-                 */
+                //new PickupBallAuto(drivetrain,locationator,limelight)
         );
-        } else {
-            addCommands(
-                    new PickupBallAuto(drivetrain, locationator, limelight)
-            );
-        }
     }
-}
+}

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

@@ -1,7 +1,6 @@
 package frc.team5115.Commands.Intake;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.team5115.Robot.RobotContainer;
 import frc.team5115.Subsystems.Intake;
 
 public class DriverIntake extends CommandBase {

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

@@ -1,7 +1,6 @@
 package frc.team5115.Commands.Intake;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.team5115.Robot.RobotContainer;
 import frc.team5115.Subsystems.Intake;
 
 public class IntakeBalls extends CommandBase {

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

@@ -1,7 +1,6 @@
 package frc.team5115.Commands.Intake;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.team5115.Robot.RobotContainer;
 import frc.team5115.Subsystems.Intake;
 
 public class ReverseIntake extends CommandBase {

src/main/java/frc/team5115/Commands/Shooter/Shoot.java

@@ -1,6 +1,5 @@
 package frc.team5115.Commands.Shooter;
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.team5115.Robot.*;
 import frc.team5115.Subsystems.Shooter;
 
 public class Shoot extends CommandBase {