X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fsubsystems%2FDefenseArm.java;h=24037413086b1219d188142190fe01dddd182094;hb=72fc3c9bd8099194cc4b2c8a747ed9f2be06f1c8;hp=8e49508981cc1bc5d7a2444787b567b421758b5f;hpb=f661626fd47ad3fa3b6c744e84c6d0b4a4130857;p=3501%2Fstronghold-2016 diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java index 8e495089..24037413 100755 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DefenseArm.java @@ -1,55 +1,160 @@ -package org.usfirst.frc.team3501.robot.subsystems; - -import org.usfirst.frc.team3501.robot.Constants; - -import edu.wpi.first.wpilibj.AnalogPotentiometer; -import edu.wpi.first.wpilibj.CANTalon; -import edu.wpi.first.wpilibj.command.Subsystem; - -public class DefenseArm extends Subsystem { - // Defense arm related objects - public AnalogPotentiometer defenseArmPotentiometer; - public AnalogPotentiometer defenseHandPotentiometer; - public CANTalon defenseArmMotor; - public CANTalon defenseHandMotor; - public double hookHeight; - - // Defense arm specific constants that relate to the degrees per pulse value - // for the potentiometers - // private final static double PULSES_PER_ROTATION = 1; // in pulses - public final static double FULL_RANGE = 270.0; // in degrees - public final static double OFFSET = -135.0; // in degrees - public final static double[] armPotValue = { 0.0, 45.0, 90.0 }; // 3 level - - // array; - - // do we want to use a hashmap?? - // angles at 0,45,90 (Potentiometer value readings) - // degrees - - public DefenseArm() { - defenseArmPotentiometer = new AnalogPotentiometer( - Constants.DefenseArm.ARM_CHANNEL, - FULL_RANGE, OFFSET); - Constants.DefenseArm.HAND_CHANNEL); - defenseArmMotor = new CANTalon(Constants.DefenseArm.ARM_PORT); - defenseHandMotor = new CANTalon(Constants.DefenseArm.HAND_PORT); - } - - /*** - * This method gets the height of the hook from the ground. The hook is - * attached to the end of the hand, which is attached to the arm. - * - * @return hookHeight gets height of hook from ground. The hook is attached to - * the end of the hand, which is attached the arm. - * - */ - - public double getHookHeight() { - return hookHeight; - } - - @Override - protected void initDefaultCommand() { - } -} +package org.usfirst.frc.team3501.robot.subsystems; + +import org.usfirst.frc.team3501.robot.Constants; + +import edu.wpi.first.wpilibj.AnalogPotentiometer; +import edu.wpi.first.wpilibj.CANTalon; +import edu.wpi.first.wpilibj.command.Subsystem; + +public class DefenseArm extends Subsystem { + private AnalogPotentiometer defenseArmPotentiometer; + private AnalogPotentiometer defenseHandPotentiometer; + private CANTalon defenseArm; + private CANTalon defenseHand; + private double hookHeight; + private double footHeight; + + private double[] potHandAngles; + private double[] potArmAngles; + private double[] potAngles; + + // angles corresponding to pre-determined heights we will need + + public DefenseArm() { + defenseArmPotentiometer = new AnalogPotentiometer( + Constants.DefenseArm.ARM_CHANNEL, Constants.DefenseArm.FULL_RANGE, + Constants.DefenseArm.OFFSET); + defenseHandPotentiometer = new AnalogPotentiometer( + Constants.DefenseArm.HAND_CHANNEL, Constants.DefenseArm.FULL_RANGE, + Constants.DefenseArm.OFFSET); + + defenseArm = new CANTalon(Constants.DefenseArm.ARM_PORT); + defenseHand = new CANTalon(Constants.DefenseArm.HAND_PORT); + potHandAngles = createHandPotArray(); + potArmAngles = createArmPotArray(); + } + + public double getArmPotAngle() { + return defenseArmPotentiometer.get(); + } + + public double getHandPotAngle() { + return defenseHandPotentiometer.get(); + } + + /*** + * This method takes an arm location as input (range of [0,2]) Returns the + * angle of the arm corresponding to that arm location + * + * @param desiredArmLocation + * takes an arm location ranging from [0,2] 0 is the lowest position + * of arm 2 is the highest position of arm + * @return the angle of the arm corresponding to that arm location + */ + 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; + } + + /*** + * This method sets the voltage of the arm motor. The range is from [-1,1]. A + * negative voltage makes the direction of the motor go backwards. + * + * @param speed + * The voltage that you set the motor at. The range of the voltage of + * the arm motor is from [-1,1]. A negative voltage makes the + * direction of the motor go backwards. + */ + + public void setArmSpeed(double speed) { + if (speed > 1) + speed = 1; + else if (speed < -1) + speed = -1; + + defenseArm.set(speed); + } + + /*** + * This method sets the voltage of the hand motor. The range is from [-1,1]. A + * negative voltage makes the direction of the motor go backwards. + * + * @param speed + * The voltage that you set the motor at. The range of the voltage of + * the hand motor is from [-1,1]. A negative voltage makes the + * direction of the motor go backwards. + */ + + public void setHandSpeed(double speed) { + if (speed > 1) + speed = 1; + else if (speed < -1) + speed = -1; + + defenseHand.set(speed); + } + + // TODO: figure out if measurements are all in inches + public double getArmHorizontalDisplacement() { + double armHorizontalDisplacement = Constants.DefenseArm.ARM_LENGTH + * Math.cos(getArmPotAngle()); + double handHorizontalDisplacement = Constants.DefenseArm.HAND_LENGTH + * Math.cos(getHandPotAngle()); + return (armHorizontalDisplacement + handHorizontalDisplacement); + } + + public double getArmVerticalDisplacement() { + double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT; + double armVerticalDisplacement = Constants.DefenseArm.ARM_LENGTH + * Math.sin(getArmPotAngle()); + double handVerticalDisplacement = Constants.DefenseArm.HAND_LENGTH + * Math.sin(getHandPotAngle()); + return (armMounted + armVerticalDisplacement + handVerticalDisplacement); + } + + public double getArmHorizontalDist() { + double arm = Constants.DefenseArm.ARM_LENGTH * Math.cos(getArmPotAngle()); + double hand = Constants.DefenseArm.HAND_LENGTH + * Math.cos(getHandPotAngle()); + return (arm + hand); + } + + public double getArmHeight() { + double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT; + double arm = Constants.DefenseArm.ARM_LENGTH * Math.sin(getArmPotAngle()); + double hand = Constants.DefenseArm.HAND_LENGTH + * Math.sin(getHandPotAngle()); + return (armMounted + arm + hand); + } + + public boolean isOutsideRange() { + if (getArmHorizontalDist() < 15) + return false; + return true; + } + + @Override + protected void initDefaultCommand() { + } +}