From 6b4bc77a806a17326148ca66c943c832543b829f Mon Sep 17 00:00:00 2001 From: Kevin Date: Tue, 8 Apr 2014 20:14:57 -0400 Subject: [PATCH] Added ability to drive while finding ball. (Not Tested!) --- src/org/team484/fluffy/OI.java | 9 ++-- src/org/team484/fluffy/Robot.java | 11 ++++- .../team484/fluffy/commands/Autonomous2.java | 8 ++-- .../fluffy/commands/FindAndPickup.java | 4 +- .../fluffy/commands/FollowBallTele.java | 41 +++++++++++++++++++ src/org/team484/fluffy/commands/LobShot.java | 2 +- .../team484/fluffy/subsystems/DriveTrain.java | 36 +++++++++++++++- 7 files changed, 99 insertions(+), 12 deletions(-) create mode 100644 src/org/team484/fluffy/commands/FollowBallTele.java diff --git a/src/org/team484/fluffy/OI.java b/src/org/team484/fluffy/OI.java index 589d3b4..88e8917 100644 --- a/src/org/team484/fluffy/OI.java +++ b/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()); diff --git a/src/org/team484/fluffy/Robot.java b/src/org/team484/fluffy/Robot.java index 9eaa550..64676e6 100644 --- a/src/org/team484/fluffy/Robot.java +++ b/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!"); diff --git a/src/org/team484/fluffy/commands/Autonomous2.java b/src/org/team484/fluffy/commands/Autonomous2.java index 464d3cd..99b737b 100644 --- a/src/org/team484/fluffy/commands/Autonomous2.java +++ b/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()); } diff --git a/src/org/team484/fluffy/commands/FindAndPickup.java b/src/org/team484/fluffy/commands/FindAndPickup.java index 67ebdab..d2310a9 100644 --- a/src/org/team484/fluffy/commands/FindAndPickup.java +++ b/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); diff --git a/src/org/team484/fluffy/commands/FollowBallTele.java b/src/org/team484/fluffy/commands/FollowBallTele.java new file mode 100644 index 0000000..91769a5 --- /dev/null +++ b/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() { + } +} \ No newline at end of file diff --git a/src/org/team484/fluffy/commands/LobShot.java b/src/org/team484/fluffy/commands/LobShot.java index d0af1d9..64a88b9 100644 --- a/src/org/team484/fluffy/commands/LobShot.java +++ b/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); } diff --git a/src/org/team484/fluffy/subsystems/DriveTrain.java b/src/org/team484/fluffy/subsystems/DriveTrain.java index f8efb53..c1776c5 100644 --- a/src/org/team484/fluffy/subsystems/DriveTrain.java +++ b/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;