Added ability to drive while finding ball. (Not Tested!)

src/org/team484/fluffy/OI.java

@@ -20,6 +20,7 @@
 import org.team484.fluffy.commands.PickupWheelHalf;
 import org.team484.fluffy.commands.PickupWheelOff;
 import org.team484.fluffy.commands.PickupWheelOn;
+import org.team484.fluffy.commands.SetShootBall;
 import org.team484.fluffy.commands.ZeroGyro;
 
 /**
@@ -41,7 +42,7 @@ public class OI {
     Button magTest = new JoystickButton(shootStick, 8);
     Button kickerOut = new JoystickButton(driveStick, 2);
     Button pickupWheelBack = new JoystickButton(shootStick, RobotMap.pickupWheelBack);
-    Button followBall = new JoystickButton(driveStick, 7);
+    Button followBall = new JoystickButton(driveStick, 3);
     Button safetyOverride = new JoystickButton(shootStick, 11);
     public double getDriveX() {
         return driveStick.getX();
@@ -60,8 +61,10 @@ public OI() {
         //highShot.whenPressed(new PickupArmDown()); //added
         //lobShot.whenPressed(new PickupArmDown()); //added
         //pickupBall.whenPressed(new PickupArmDown()); //added
-        highShot.whenPressed(new HighShot());
-        lobShot.whenPressed(new LobShot());
+        highShot.whileHeld(new HighShot());
+        highShot.whenReleased(new PickupArmDown());
+        lobShot.whileHeld(new LobShot());
+        lobShot.whenReleased(new PickupArmDown());
         lowerArm.whenPressed(new PickupArmDown());
         raiseArm.whenPressed(new PickupArmUp());
         pickupBall.whenPressed(new PickupArmDown());

src/org/team484/fluffy/Robot.java

@@ -16,11 +16,13 @@
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import org.team484.fluffy.commands.Autonomous;
+import org.team484.fluffy.commands.Autonomous2;
 import org.team484.fluffy.commands.CommandBase;
 
 public class Robot extends IterativeRobot {
 
     Command autonomousCommand;
+    Command autonomous2Command;
     Compressor compressor;
     protected DriverStation ds;
     Relay LEDs;
@@ -49,13 +51,18 @@ public void robotInit() {
         }
         camLED.set(Relay.Value.kReverse);
         autonomousCommand = new Autonomous();
+        autonomous2Command = new Autonomous2();
         CommandBase.init();
         compressor = new Compressor(RobotMap.pressureSwitch, RobotMap.compressorRelay);
         compressor.start();
     }
 
     public void autonomousInit() {
-        autonomousCommand.start();
+        if (ds.getDigitalIn(1)) {
+            autonomousCommand.start();
+        } else {
+            autonomous2Command.start();
+        }
         if (ds.getAlliance().value == 0) {
             //Red Team
             System.out.println("Go Red!");
@@ -78,6 +85,8 @@ public void autonomousPeriodic() {
     }
 
     public void teleopInit() {
+        autonomous2Command.cancel();
+        autonomousCommand.cancel();
         if (ds.getAlliance().value == 0) {
             //Red Team
             System.out.println("Go Red!");

src/org/team484/fluffy/commands/Autonomous2.java

@@ -20,17 +20,17 @@ public Autonomous2() {
         addSequential(new DriveForTime(), 1.6);
         addSequential(new WaitCommand(0.5), 0.5);
         addSequential(new ShooterUp(), 1);
-        addSequential(new SetStartAuto());
         addParallel(new ShooterDown());
         addSequential(new DriveForTimeBack(), 1.6);
-        addSequential(new FindAndPickupAuto(), 1.9);
-        addSequential(new SetStartAuto());
+        addParallel(new PickupWheelOn());
+        addSequential(new FollowBall(), 1.9);
         addParallel(new PickupWheelOn());
         addSequential(new KickerChecked(), 0.3);
         addParallel (new KickerIn());
         addParallel(new PickupWheelOff());
-        addSequential(new DriveForTime(), 1.6);
+        addSequential(new DriveForTime(), 1.7);
         addSequential(new WaitCommand(0.5), 0.5);
+        addSequential(new SetShootBall(), 0.05);
         addSequential(new ShooterUp(), 1);
         addParallel(new ShooterDown());
     }

src/org/team484/fluffy/commands/FindAndPickup.java

@@ -33,8 +33,8 @@ public FindAndPickup() {
         addSequential(new ZeroGyro(), 0.1);
         addParallel(new PickupArmDown());
         addParallel(new PickupWheelOn());
-        addSequential(new WaitCommand(0.8), 0.8);
-        addSequential(new FollowBall());
+        //addSequential(new WaitCommand(0.8), 0.8);
+        addSequential(new FollowBallTele());
         addParallel(new PickupWheelOn());
         addSequential (new WaitCommand(0.5), 0.5);
         addSequential(new KickerChecked(), 0.3);

src/org/team484/fluffy/commands/FollowBallTele.java

@@ -0,0 +1,41 @@
+/*
+ * To change this template, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package org.team484.fluffy.commands;
+
+/**
+ *
+ * @author Team484
+ */
+public class FollowBallTele extends CommandBase {
+
+    public FollowBallTele() {
+        // Use requires() here to declare subsystem dependencies
+        // eg. requires(chassis);
+        requires(drivetrain);
+    }
+
+    // Called just before this Command runs the first time
+    protected void initialize() {
+    }
+
+    // Called repeatedly when this Command is scheduled to run
+    protected void execute() {
+        drivetrain.followBallTele();
+    }
+
+    // Make this return true when this Command no longer needs to run execute()
+    protected boolean isFinished() {
+        return false;
+    }
+
+    // Called once after isFinished returns true
+    protected void end() {
+    }
+
+    // Called when another command which requires one or more of the same
+    // subsystems is scheduled to run
+    protected void interrupted() {
+    }
+}

src/org/team484/fluffy/commands/LobShot.java

@@ -32,7 +32,7 @@ public LobShot() {
         }
         addSequential(new SetShootBall(), 0.1);
         addSequential(new ShooterUp(), 0.29);
-        addSequential(new ShooterDown(), 0.1);
+        addParallel(new ShooterDown());
         //addSequential(new WaitCommand(1), 1);
         //addSequential(new PickupArmUp(), 0.1);
     }

src/org/team484/fluffy/subsystems/DriveTrain.java

@@ -133,7 +133,7 @@ public void followBall() {
         double leftIn = (MathUtils.pow(currentLeft, -1.22)*6627);
         double rightIn = (MathUtils.pow(currentRight, -1.22)*6627);
         System.out.println("Left: " + leftIn + " Right: " + rightIn);
-        if ((leftIn < 40 || rightIn < 40) && SmartDashboard.getBoolean("No Ball", true)) {
+        if ((leftIn < 32 || rightIn < 32) && SmartDashboard.getBoolean("No Ball", true)) {
             double mech = MathUtils.log(Math.abs(leftIn - rightIn)) / 20;
             if (leftIn - rightIn < 0) {
                 mech = 0 - mech;
@@ -157,6 +157,40 @@ public void followBall() {
             robotDrive.mecanumDrive_Cartesian(mech, speed, rotation, 0);
         }
     }
+    public void followBallTele() {
+        double rotation = -(gyro.getAngle() / 160);
+        double speed = 0;
+        int currentLeft = leftIR.getValue();
+        int currentRight = rightIR.getValue();
+        double leftIn = (MathUtils.pow(currentLeft, -1.22)*6627);
+        double rightIn = (MathUtils.pow(currentRight, -1.22)*6627);
+        System.out.println("Left: " + leftIn + " Right: " + rightIn);
+        if ((leftIn < 38 || rightIn < 38) && SmartDashboard.getBoolean("No Ball", true)) {
+            double mech = MathUtils.log(Math.abs(leftIn - rightIn)) / 20;
+            if (leftIn - rightIn < 0) {
+                mech = 0 - mech;
+            }
+            if (mech > 0.5) {
+                mech = 0.5;
+            } else if (mech < -0.5) {
+                mech = -0.5;
+            } else if (mech > -0.1 && mech < 0) {
+                mech = 0;
+            } else if (mech > 0 && mech < 0.1) {
+                mech = 0;
+            }
+            if (leftIn < rightIn && leftIn > 12) {
+                speed = (leftIn - 12) / 20;
+            } else if (leftIn > rightIn && rightIn > 12) {
+                speed = (rightIn - 12) / 20;
+            } else {
+                speed = 0;
+            }
+            robotDrive.mecanumDrive_Cartesian(mech, speed, rotation, 0);
+        } else {
+            robotDrive.mecanumDrive_Cartesian(0, driveStick.getY(), driveStick.getX(), 0);
+        }
+    }
     public void mechanumDrive(double x, double y, double rotation, boolean autonomous, boolean mechanum) {
         if (mechanum && !this.wasMech) {
             this.wasMech = true;