code review changes

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index e6ad60d6482c586bd9221e7aa1f42db1b6d17ed4..24eca3c88ea8f863ad58c6b612f7d527dae930e6 100644 (file)
--- 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 (file)
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 (file)
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 18e3568a4a824c0cb8308edf0541a330014d97f3..cd2fe3371ba471ac43b4998582e2242e493b9932 100644 (file)
--- 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 717f15151d5b8512218b2e865acade3403cd9ced..b95c8223a69fccad10d0f3d3cc2bc882129bb6ed 100755 (executable)
--- 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 08ead3ac4bb6b52dfb8fd9cc974fd9d80d85724f..d2d386ab040422c72ac3a7863dcc7d8262022f67 100644 (file)
--- 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 32ec7b221ab237ef17747dfa46f47f8246ace31b..5422e973078ba2d99b0d9556732de89a0d8d4b31 100644 (file)
--- 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 6397292049a78637fe675af773033a382d605268..adfdb341862f933a03094392848cdfe80ec37328 100644 (file)
--- 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;