From: David Date: Fri, 12 Feb 2016 04:50:49 +0000 (-0800) Subject: fix merge conflicts X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=a96fa9265d41f13d4df85b8b4d73ee4d287c630c;hp=e1107e1e54218d74d842c2c460a24c5a79068433;p=3501%2Fstronghold-2016 fix merge conflicts --- diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 9d97660c..ea7910b6 100644 --- 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 index 00000000..b5fe5649 --- /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 index 00000000..d5319633 --- /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 index 51729b3b..00000000 --- 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 index 00000000..9749ac3c --- /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 index 00000000..6b54111b --- /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 index 00000000..95f5f92e --- /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 index 00000000..d5a0409b --- /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 index 00000000..f89ac0da --- /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 401f09e4..0794b380 100755 --- 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; } /***