fix merge conflicts

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index 9d97660caace18d282b4bef5fdf54e6f2503b47c..ea7910b607f05090ca31e46446fa6f0f33ee2654 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -1,6 +1,7 @@
 package org.usfirst.frc.team3501.robot;
 
 import org.usfirst.frc.team3501.robot.Constants.Defense;
+import org.usfirst.frc.team3501.robot.subsystems.DefenseArm;
 import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
 import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
 import org.usfirst.frc.team3501.robot.subsystems.Scaler;
@@ -12,119 +13,123 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Robot extends IterativeRobot {
-       public static OI oi;
-       public static DriveTrain driveTrain;
-       public static Shooter shooter;
-       public static Scaler scaler;
-       public static IntakeArm intakeArm;
-
-       // Sendable Choosers send a drop down menu to the Smart Dashboard.
-       SendableChooser positionChooser;
-       SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense, positionFourDefense,
-                       positionFiveDefense;
-
-       @Override
-       public void robotInit() {
-               driveTrain = new DriveTrain();
-               oi = new OI();
-               shooter = new Shooter();
-               scaler = new Scaler();
-               intakeArm = new IntakeArm();
-
-               // Sendable Choosers allows the driver to select the position of the
-               // robot
-               // and the positions of the defenses from a drop-down menu on the Smart
-               // Dashboard
-               // make the Sendable Choosers
-               initializeSendableChoosers();
-               addPositionChooserOptions();
-               addDefensesToAllDefenseSendableChooosers();
-               sendSendableChoosersToSmartDashboard();
-
-       }
-
-       private void initializeSendableChoosers() {
-               positionChooser = new SendableChooser();
-               positionOneDefense = new SendableChooser();
-               positionTwoDefense = new SendableChooser();
-               positionThreeDefense = new SendableChooser();
-               positionFourDefense = new SendableChooser();
-               positionFiveDefense = new SendableChooser();
-       }
-
-       private void addPositionChooserOptions() {
-               positionChooser.addDefault("Position 1", 1);
-               positionChooser.addObject("Position 2", 2);
-               positionChooser.addObject("Position 3", 3);
-               positionChooser.addObject("Position 4", 4);
-               positionChooser.addObject("Position 5", 5);
-       }
-
-       private void addDefensesToAllDefenseSendableChooosers() {
-               addDefenseOptions(positionOneDefense);
-               addDefenseOptions(positionTwoDefense);
-               addDefenseOptions(positionThreeDefense);
-               addDefenseOptions(positionFourDefense);
-               addDefenseOptions(positionFiveDefense);
-       }
-
-       private void addDefenseOptions(SendableChooser chooser) {
-               chooser.addDefault("Portcullis", Defense.PORTCULLIS);
-               chooser.addObject("Sally Port", Defense.SALLY_PORT);
-               chooser.addObject("Rough Terrain", Defense.ROUGH_TERRAIN);
-               chooser.addObject("Low Bar", Defense.LOW_BAR);
-               chooser.addObject("Cheval De Frise", Defense.CHEVAL_DE_FRISE);
-               chooser.addObject("Drawbridge", Defense.DRAWBRIDGE);
-               chooser.addObject("Moat", Defense.MOAT);
-               chooser.addObject("Rock Wall", Defense.ROCK_WALL);
-       }
-
-       private void sendSendableChoosersToSmartDashboard() {
-               SmartDashboard.putData("PositionChooser", positionChooser);
-               SmartDashboard.putData("Position One Defense Chooser", positionOneDefense);
-               SmartDashboard.putData("Position Two Defense Chooser", positionTwoDefense);
-               SmartDashboard.putData("Position Three Defense Chooser", positionThreeDefense);
-               SmartDashboard.putData("Position Four Defense Chooser", positionFourDefense);
-               SmartDashboard.putData("Position Five Defense Chooser", positionFiveDefense);
-       }
-
-       @Override
-       public void autonomousInit() {
-               Scheduler.getInstance().run();
-
-               // get options chosen from drop down menu
-               Integer chosenPosition = (Integer) positionChooser.getSelected();
-               Integer chosenDefense = 0;
-
-               switch (chosenPosition) {
-               case 1:
-                       chosenDefense = (Integer) positionOneDefense.getSelected();
-               case 2:
-                       chosenDefense = (Integer) positionTwoDefense.getSelected();
-               case 3:
-                       chosenDefense = (Integer) positionThreeDefense.getSelected();
-               case 4:
-                       chosenDefense = (Integer) positionFourDefense.getSelected();
-               case 5:
-                       chosenDefense = (Integer) positionFiveDefense.getSelected();
-               }
-
-               System.out.println("Chosen Position: " + chosenPosition);
-               System.out.println("Chosen Defense: " + chosenDefense);
-       }
-
-       @Override
-       public void autonomousPeriodic() {
-               Scheduler.getInstance().run();
-       }
-
-       @Override
-       public void teleopInit() {
-       }
-
-       @Override
-       public void teleopPeriodic() {
-               Scheduler.getInstance().run();
-
-       }
+  public static OI oi;
+  public static DriveTrain driveTrain;
+  public static Shooter shooter;
+  public static Scaler scaler;
+  public static IntakeArm intakeArm;
+
+  // Sendable Choosers send a drop down menu to the Smart Dashboard.
+  SendableChooser positionChooser;
+  SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense,
+      positionFourDefense,
+      positionFiveDefense;
+
+  @Override
+  public void robotInit() {
+    driveTrain = new DriveTrain();
+    oi = new OI();
+    shooter = new Shooter();
+    scaler = new Scaler();
+    intakeArm = new IntakeArm();
+
+    // Sendable Choosers allows the driver to select the position of the
+    // robot
+    // and the positions of the defenses from a drop-down menu on the Smart
+    // Dashboard
+    // make the Sendable Choosers
+    initializeSendableChoosers();
+    addPositionChooserOptions();
+    addDefensesToAllDefenseSendableChooosers();
+    sendSendableChoosersToSmartDashboard();
+
+  }
+
+  private void initializeSendableChoosers() {
+    positionChooser = new SendableChooser();
+    positionOneDefense = new SendableChooser();
+    positionTwoDefense = new SendableChooser();
+    positionThreeDefense = new SendableChooser();
+    positionFourDefense = new SendableChooser();
+    positionFiveDefense = new SendableChooser();
+  }
+
+  private void addPositionChooserOptions() {
+    positionChooser.addDefault("Position 1", 1);
+    positionChooser.addObject("Position 2", 2);
+    positionChooser.addObject("Position 3", 3);
+    positionChooser.addObject("Position 4", 4);
+    positionChooser.addObject("Position 5", 5);
+  }
+
+  private void addDefensesToAllDefenseSendableChooosers() {
+    addDefenseOptions(positionOneDefense);
+    addDefenseOptions(positionTwoDefense);
+    addDefenseOptions(positionThreeDefense);
+    addDefenseOptions(positionFourDefense);
+    addDefenseOptions(positionFiveDefense);
+  }
+
+  private void addDefenseOptions(SendableChooser chooser) {
+    chooser.addDefault("Portcullis", Defense.PORTCULLIS);
+    chooser.addObject("Sally Port", Defense.SALLY_PORT);
+    chooser.addObject("Rough Terrain", Defense.ROUGH_TERRAIN);
+    chooser.addObject("Low Bar", Defense.LOW_BAR);
+    chooser.addObject("Cheval De Frise", Defense.CHEVAL_DE_FRISE);
+    chooser.addObject("Drawbridge", Defense.DRAWBRIDGE);
+    chooser.addObject("Moat", Defense.MOAT);
+    chooser.addObject("Rock Wall", Defense.ROCK_WALL);
+  }
+
+  private void sendSendableChoosersToSmartDashboard() {
+    SmartDashboard.putData("PositionChooser", positionChooser);
+    SmartDashboard.putData("Position One Defense Chooser", positionOneDefense);
+    SmartDashboard.putData("Position Two Defense Chooser", positionTwoDefense);
+    SmartDashboard.putData("Position Three Defense Chooser",
+        positionThreeDefense);
+    SmartDashboard
+        .putData("Position Four Defense Chooser", positionFourDefense);
+    SmartDashboard
+        .putData("Position Five Defense Chooser", positionFiveDefense);
+  }
+
+  @Override
+  public void autonomousInit() {
+    Scheduler.getInstance().run();
+
+    // get options chosen from drop down menu
+    Integer chosenPosition = (Integer) positionChooser.getSelected();
+    Integer chosenDefense = 0;
+
+    switch (chosenPosition) {
+    case 1:
+      chosenDefense = (Integer) positionOneDefense.getSelected();
+    case 2:
+      chosenDefense = (Integer) positionTwoDefense.getSelected();
+    case 3:
+      chosenDefense = (Integer) positionThreeDefense.getSelected();
+    case 4:
+      chosenDefense = (Integer) positionFourDefense.getSelected();
+    case 5:
+      chosenDefense = (Integer) positionFiveDefense.getSelected();
+    }
+
+    System.out.println("Chosen Position: " + chosenPosition);
+    System.out.println("Chosen Defense: " + chosenDefense);
+  }
+
+  @Override
+  public void autonomousPeriodic() {
+    Scheduler.getInstance().run();
+  }
+
+  @Override
+  public void teleopInit() {
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    Scheduler.getInstance().run();
+
+  }
 }

diff --git a/src/org/usfirst/frc/team3501/robot/commands/LowerDefenseArmContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/LowerDefenseArmContinuous.java
new file mode 100755 (executable)
index 0000000..b5fe564
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/LowerDefenseArmContinuous.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/***
+ * This command is intended to be run from OI using button.whileHeld(...).
+ * It lowers the defenseArm continually while the button is being held down.
+ * 
+ * @author shaina
+ *
+ */
+public class LowerDefenseArmContinuous extends Command {
+
+  private double speed;
+
+  public LowerDefenseArmContinuous(double speed) {
+    requires(Robot.defenseArm);
+    this.speed = -speed;
+  }
+
+  @Override
+  protected void initialize() {
+    Robot.defenseArm.setArmSpeed(speed);
+  }
+
+  @Override
+  protected void execute() {
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  protected void end() {
+    Robot.defenseArm.setArmSpeed(0);
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/LowerDefenseWristContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/LowerDefenseWristContinuous.java
new file mode 100755 (executable)
index 0000000..d531963
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/LowerDefenseWristContinuous.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/***
+ * This command is intended to be run from OI using button.whileHeld(...).
+ * It lowers the defenseWrist continually while the button is being held down.
+ *
+ * @author shaina
+ *
+ */
+public class LowerDefenseWristContinuous extends Command {
+
+  private double speed;
+
+  public LowerDefenseWristContinuous(double speed) {
+    requires(Robot.defenseArm);
+    this.speed = -speed;
+  }
+
+  @Override
+  protected void initialize() {
+    Robot.defenseArm.setHandSpeed(speed);
+  }
+
+  @Override
+  protected void execute() {
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  protected void end() {
+    Robot.defenseArm.setHandSpeed(0);
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmUp.java b/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmUp.java
deleted file mode 100755 (executable)
index 51729b3..0000000
--- a/src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArmUp.java
+++ /dev/null
@@ -1,50 +0,0 @@
-package org.usfirst.frc.team3501.robot.commands;
-
-import edu.wpi.first.wpilibj.CANTalon;
-import edu.wpi.first.wpilibj.command.Command;
-
-public class MoveDefenseArmUp extends Command {
-  // put variables here
-  private int armLevel;
-  public CANTalon defenseArmMotor;
-
-  // make motor run until arm reaches level
-  public MoveDefenseArmUp(CANTalon defenseArmMotor, int armLevel) {
-    this.armLevel = armLevel;
-    this.defenseArmMotor = defenseArmMotor;
-
-  }
-
-  @Override
-  protected void initialize() {
-    // TODO Auto-generated method stub
-    this.start();
-
-  }
-
-  @Override
-  protected void execute() {
-    // TODO Auto-generated method stub
-    // put what the arm is doing
-    defenseArmMotor.enable();
-  }
-
-  @Override
-  protected void interrupted() {
-    // TODO Auto-generated method stub
-
-  }
-
-  @Override
-  protected boolean isFinished() {
-    // TODO Auto-generated method stub
-    return true;
-  }
-
-  @Override
-  protected void end() {
-    // TODO Auto-generated method stub
-
-  }
-
-}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/RaiseDefenseArmContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/RaiseDefenseArmContinuous.java
new file mode 100755 (executable)
index 0000000..9749ac3
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/RaiseDefenseArmContinuous.java
@@ -0,0 +1,45 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/***
+ * This command is intended to be run from OI using button.whileHeld(...).
+ * It raises the defenseArm continually while the button is being held down.
+ * 
+ * @author shaina
+ */
+public class RaiseDefenseArmContinuous extends Command {
+
+  private double speed;
+
+  public RaiseDefenseArmContinuous(double speed) {
+    requires(Robot.defenseArm);
+    this.speed = speed;
+  }
+
+  @Override
+  protected void initialize() {
+    Robot.defenseArm.setArmSpeed(speed);
+  }
+
+  @Override
+  protected void execute() {
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  protected void end() {
+    Robot.defenseArm.setArmSpeed(0);
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/RaiseDefenseWristContinuous.java b/src/org/usfirst/frc/team3501/robot/commands/RaiseDefenseWristContinuous.java
new file mode 100755 (executable)
index 0000000..6b54111
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/RaiseDefenseWristContinuous.java
@@ -0,0 +1,46 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/***
+ * This command is intended to be run from OI using button.whileHeld(...).
+ * It raises the defenseWrist continually while the button is being held down.
+ *
+ * @author shaina
+ *
+ */
+public class RaiseDefenseWristContinuous extends Command {
+
+  private double speed;
+
+  public RaiseDefenseWristContinuous(double speed) {
+    requires(Robot.defenseArm);
+    this.speed = speed;
+  }
+
+  @Override
+  protected void initialize() {
+    Robot.defenseArm.setHandSpeed(speed);
+  }
+
+  @Override
+  protected void execute() {
+  }
+
+  @Override
+  protected boolean isFinished() {
+    return true;
+  }
+
+  @Override
+  protected void end() {
+    Robot.defenseArm.setHandSpeed(0);
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java b/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java
new file mode 100755 (executable)
index 0000000..95f5f92
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/RetractDefenseArm.java
@@ -0,0 +1,24 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/***
+ * This command group simultaneously move arm and hand to retracted position.
+ *
+ * @author shaina
+ *
+ */
+public class RetractDefenseArm extends CommandGroup {
+  static final double ARM_FULLY_RETRACTED_ANGLE = 0; // change value once tested
+  static final double HAND_FULLY_RETRACTED_ANGLE = 0; // change value once
+                                                      // tested
+
+  public RetractDefenseArm(double speed) {
+    requires(Robot.defenseArm);
+
+    addParallel(new SetArmToAngle(speed, ARM_FULLY_RETRACTED_ANGLE));
+    addParallel(new SetHandToAngle(speed, HAND_FULLY_RETRACTED_ANGLE));
+  }
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/SetArmToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/SetArmToAngle.java
new file mode 100755 (executable)
index 0000000..d5a0409
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/SetArmToAngle.java
@@ -0,0 +1,67 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/***
+ * This command moves the defense arm to a input angle position.
+ * Requires input of a targetPosition and a speed.
+ *
+ * @author shaina
+ *
+ */
+public class SetArmToAngle extends Command {
+  private static final double THRESHOLD = 0.1;
+  private double speed;
+  private double targetPosition;
+  private double currentPosition;
+  private boolean isDecreasing = false;
+
+  public SetArmToAngle(double speed, double targetPosition) {
+    requires(Robot.defenseArm);
+
+    this.speed = speed;
+    this.targetPosition = targetPosition;
+  }
+
+  @Override
+  public void initialize() {
+    currentPosition = Robot.defenseArm.getArmPotAngle();
+
+    if (currentPosition > targetPosition) {
+      Robot.defenseArm.setArmSpeed(-speed);
+      isDecreasing = true;
+    } else {
+      Robot.defenseArm.setArmSpeed(speed);
+      isDecreasing = false;
+    }
+  }
+
+  @Override
+  public void execute() {
+
+  }
+
+  @Override
+  public boolean isFinished() {
+    currentPosition = Robot.defenseArm.getArmPotAngle();
+
+    if (isDecreasing == true) {
+      return (currentPosition <= targetPosition + THRESHOLD);
+    } else {
+      return (currentPosition >= targetPosition - THRESHOLD);
+    }
+  }
+
+  @Override
+  public void end() {
+    Robot.defenseArm.setArmSpeed(0);
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+
+}

diff --git a/src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java
new file mode 100755 (executable)
index 0000000..f89ac0d
--- /dev/null
+++ b/src/org/usfirst/frc/team3501/robot/commands/SetHandToAngle.java
@@ -0,0 +1,67 @@
+package org.usfirst.frc.team3501.robot.commands;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/***
+ * This command moves the defense arm to a input angle position.
+ * Requires input of a targetPosition and a speed.
+ * 
+ * @author shaina
+ *
+ */
+public class SetHandToAngle extends Command {
+  private static final double THRESHOLD = 0.1;
+  private double speed;
+  private double targetPosition;
+  private double currentPosition;
+  private boolean isDecreasing = false;
+
+  public SetHandToAngle(double speed, double targetPosition) {
+    requires(Robot.defenseArm);
+
+    this.speed = speed;
+    this.targetPosition = targetPosition;
+  }
+
+  @Override
+  public void initialize() {
+    currentPosition = Robot.defenseArm.getHandPotAngle();
+
+    if (currentPosition > targetPosition) {
+      Robot.defenseArm.setArmSpeed(-speed);
+      isDecreasing = true;
+    } else {
+      Robot.defenseArm.setArmSpeed(speed);
+      isDecreasing = false;
+    }
+  }
+
+  @Override
+  public void execute() {
+
+  }
+
+  @Override
+  public boolean isFinished() {
+    currentPosition = Robot.defenseArm.getHandPotAngle();
+
+    if (isDecreasing == true) {
+      return (currentPosition <= targetPosition + THRESHOLD);
+    } else {
+      return (currentPosition >= targetPosition - THRESHOLD);
+    }
+  }
+
+  @Override
+  public void end() {
+    Robot.defenseArm.setArmSpeed(0);
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+
+}

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java
index 401f09e447487e9684868baaa8355c667d67405e..0794b380a3ec01fbf51d05c9ec70ed27577274c1 100755 (executable)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java
@@ -13,7 +13,8 @@ public class DefenseArm extends Subsystem {
   private CANTalon defenseHand;
   private double hookHeight;
   private double footHeight;
-  private double[] potAngles = { 0, 45, 90 };
+  private double[] potHandAngles;
+  private double[] potArmAngles;
 
   // angles corresponding to pre-determined heights we will need
 
@@ -29,6 +30,8 @@ public class DefenseArm extends Subsystem {
 
     defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT);
     defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT);
+    potHandAngles = createHandPotArray();
+    potArmAngles = createArmPotArray();
   }
 
   public double getArmPotAngle() {
@@ -50,11 +53,30 @@ public class DefenseArm extends Subsystem {
    * @return
    *         the angle of the arm corresponding to that arm location
    */
-  public double getLevelValue(int level) {
-    if (level >= potAngles.length)
-      return potAngles[level];
-    else
-      return 0;
+  public double getAngleForHandLocation(int desiredArmLocation) {
+    return potHandAngles[desiredArmLocation];
+  }
+
+  public double getAngleForArmLocation(int desiredArmLocation) {
+    return potArmAngles[desiredArmLocation];
+  }
+
+  public double[] createHandPotArray() {
+    double[] arr = new double[3];
+
+    for (int i = 0; i < 3; i++) {
+      arr[i] = 45 * i;
+    }
+    return arr;
+  }
+
+  public double[] createArmPotArray() {
+    double[] arr = new double[3];
+
+    for (int i = 0; i < 3; i++) {
+      arr[i] = 45 * i;
+    }
+    return arr;
   }
 
   /***