From b70398a7d5ac5f4ce64156d84227ccb4828c0615 Mon Sep 17 00:00:00 2001 From: Cindy Zhang Date: Tue, 21 Feb 2017 15:29:43 -0800 Subject: [PATCH] code review changes --- src/org/usfirst/frc/team3501/robot/Robot.java | 2 +- .../robot/commandgroups/AutonHopperShoot.java | 45 +++++++++++++++++++ .../robot/commandgroups/AutonShoot.java | 26 +++++++++++ .../team3501/robot/commandgroups/Shoot.java | 27 ++++------- .../robot/commands/driving/TurnForAngle.java | 12 +++-- .../shooter/RunIndexWheelContinuous.java | 17 +------ .../team3501/robot/subsystems/DriveTrain.java | 11 +++-- .../team3501/robot/subsystems/Shooter.java | 2 +- 8 files changed, 100 insertions(+), 42 deletions(-) create mode 100644 src/org/usfirst/frc/team3501/robot/commandgroups/AutonHopperShoot.java create mode 100644 src/org/usfirst/frc/team3501/robot/commandgroups/AutonShoot.java diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index e6ad60d..24eca3c 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -71,6 +71,6 @@ public class Robot extends IterativeRobot { SmartDashboard.putNumber("voltage", DriverStation.getInstance().getBatteryVoltage()); SmartDashboard.putNumber("rpm", shooter.getShooterRPM()); - SmartDashboard.putNumber("motor value", shooter.getCurrentShootingSpeed()); + SmartDashboard.putNumber("motor value", shooter.getTargetShootingSpeed()); } } diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonHopperShoot.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonHopperShoot.java new file mode 100644 index 0000000..eb412d2 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonHopperShoot.java @@ -0,0 +1,45 @@ +package org.usfirst.frc.team3501.robot.commandgroups; + +import org.usfirst.frc.team3501.robot.Constants; +import org.usfirst.frc.team3501.robot.Constants.Direction; +import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; +import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** + * // Robot starts in middle, goes to the hopper, then boiler,then shoots during + * auton + */ +public class AutonHopperShoot extends CommandGroup { + // If red, direction is right; if blue, direction is left + private static final Direction DIRECTION_TO_HOPPER = Constants.Direction.LEFT; + // If red, direction is left; if blue, direction is right + private static final Direction DIRECTION_TO_BOILER = Constants.Direction.RIGHT; + + private Timer timer; + + public AutonHopperShoot() { + timer = new Timer(); + // Robot drives from center to front of airship + addSequential(new DriveDistance(78.5, 2.7)); + // Robot turns towards hopper + addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 2.5)); + // Robot drives into hopper switch + addSequential(new DriveDistance(42.12, 5.25)); + addSequential(new WaitCommand(1)); + // Robot backs up from switch + addSequential(new DriveDistance(-25.0, 2.9)); + // Robot turns towards the boiler + addSequential(new TurnForAngle(90.0, DIRECTION_TO_HOPPER, 5.0)); + // Robot drives to boiler + addSequential(new DriveDistance(90, 5.0)); + // Robot turns parallel to boiler + addSequential(new TurnForAngle(45, DIRECTION_TO_BOILER, 5.0)); + // Shoot + addSequential(new Shoot(15 - timeSinceInitialized())); + } + +} diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonShoot.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonShoot.java new file mode 100644 index 0000000..c2af771 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonShoot.java @@ -0,0 +1,26 @@ +package org.usfirst.frc.team3501.robot.commandgroups; + +import org.usfirst.frc.team3501.robot.Constants.Direction; +import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; +import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** + * + */ +public class AutonShoot extends CommandGroup { + + private static final int ROBOT_WIDTH = 40; // inches + private static final int ROBOT_LENGTH = 36; // inches + + public AutonShoot() { + addSequential( + new DriveDistance(37.12 + (ROBOT_WIDTH / 2) - (ROBOT_LENGTH / 2), 5)); + addSequential(new TurnForAngle(135, Direction.LEFT, 10)); + addSequential(new DriveDistance( + (37.12 + (ROBOT_WIDTH / 2)) * Math.sqrt(2) - 26.25 - (ROBOT_LENGTH / 2), + 10)); + addSequential(new Shoot(15 - timeSinceInitialized())); + } +} diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/Shoot.java b/src/org/usfirst/frc/team3501/robot/commandgroups/Shoot.java index 18e3568..cd2fe33 100644 --- a/src/org/usfirst/frc/team3501/robot/commandgroups/Shoot.java +++ b/src/org/usfirst/frc/team3501/robot/commandgroups/Shoot.java @@ -1,28 +1,19 @@ package org.usfirst.frc.team3501.robot.commandgroups; +import org.usfirst.frc.team3501.robot.commands.shooter.RunFlyWheel; +import org.usfirst.frc.team3501.robot.commands.shooter.RunIndexWheel; + import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; /** * */ public class Shoot extends CommandGroup { - public Shoot() { - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } + public Shoot(double time) { + addParallel(new RunFlyWheel(time)); + addSequential(new WaitCommand(2)); + addParallel(new RunIndexWheel(time - 2)); + } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java index 717f151..b95c822 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java @@ -36,9 +36,15 @@ public class TurnForAngle extends Command { this.maxTimeOut = maxTimeOut; this.target = Math.abs(angle); - this.gyroP = driveTrain.turnP; - this.gyroI = driveTrain.turnI; - this.gyroD = driveTrain.turnD; + if (angle > 90) { + this.gyroP = driveTrain.largeTurnP; + this.gyroI = driveTrain.largeTurnI; + this.gyroD = driveTrain.largeTurnD; + } else { + this.gyroP = driveTrain.smallTurnP; + this.gyroI = driveTrain.smallTurnI; + this.gyroD = driveTrain.smallTurnD; + } this.gyroController = new PIDController(this.gyroP, this.gyroI, this.gyroD); this.gyroController.setDoneRange(1); diff --git a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java index 08ead3a..d2d386a 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java +++ b/src/org/usfirst/frc/team3501/robot/commands/shooter/RunIndexWheelContinuous.java @@ -1,6 +1,5 @@ package org.usfirst.frc.team3501.robot.commands.shooter; -import org.usfirst.frc.team3501.robot.Constants; import org.usfirst.frc.team3501.robot.Robot; import org.usfirst.frc.team3501.robot.subsystems.Shooter; @@ -37,20 +36,8 @@ public class RunIndexWheelContinuous extends Command { double shooterSpeed = shooter.getShooterRPM(); double targetShooterSpeed = shooter.getTargetShootingSpeed(); double threshold = shooter.getRPMThreshold(); - // if (Math.abs(shooterSpeed - targetShooterSpeed) <= threshold) - - if (timeSinceInitialized() % 0.5 <= 0.02) { - - if (Robot.getDriveTrain() - .getLeftGearPistonValue() == Constants.DriveTrain.LOW_GEAR) { - System.out.println("shifting to low gear " + timeSinceInitialized()); - Robot.getDriveTrain().setHighGear(); - } else { - System.out.println("shifting to high gear " + timeSinceInitialized()); - Robot.getDriveTrain().setLowGear(); - } - } - shooter.runIndexWheel(); + if (Math.abs(shooterSpeed - targetShooterSpeed) <= threshold) + shooter.runIndexWheel(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 32ec7b2..5422e97 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -14,8 +14,10 @@ import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.command.Subsystem; public class DriveTrain extends Subsystem { - public static double driveP = 0.006, driveI = 0.0011, driveD = -0.002; - public static double turnP = 0.004, turnI = 0.0013, turnD = -0.005; + public static double driveP = 0.012, driveI = 0.0011, driveD = -0.002; + public static double smallTurnP = 0.004, smallTurnI = 0.0013, + smallTurnD = 0.005; + public static double largeTurnP = .003, largeTurnI = .0012, largeTurnD = .006; public static double driveStraightGyroP = 0.01; public static final double WHEEL_DIAMETER = 4; // inches @@ -77,8 +79,8 @@ public class DriveTrain extends Subsystem { // DRIVE METHODS public void setMotorValues(double left, double right) { - left = MathLib.restrictToRange(left, 0.0, 1.0); - right = MathLib.restrictToRange(right, 0.0, 1.0); + left = MathLib.restrictToRange(left, -1.0, 1.0); + right = MathLib.restrictToRange(right, -1.0, 1.0); frontLeft.set(left); rearLeft.set(left); @@ -183,6 +185,7 @@ public class DriveTrain extends Subsystem { * Changes the gear to a DoubleSolenoid.Value */ private void changeGear(DoubleSolenoid.Value gear) { + System.out.println(gear); leftGearPiston.set(gear); rightGearPiston.set(gear); } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java index 6397292..adfdb34 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/Shooter.java @@ -19,7 +19,7 @@ public class Shooter extends Subsystem { private static final double RPM_THRESHOLD = 10; private static final double DEFAULT_INDEXING_MOTOR_VALUE = -0.75; - private static final double DEFAULT_SHOOTING_SPEED = 2700; // rpm + private static final double DEFAULT_SHOOTING_SPEED = 2800; // rpm private static final double SHOOTING_SPEED_INCREMENT = 25; private double targetShootingSpeed = DEFAULT_SHOOTING_SPEED; -- 2.30.2