challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/commands/MoveDefenseArm.java

   1 package org.usfirst.frc.team3501.robot.commands;
   2
   3 import org.usfirst.frc.team3501.robot.Constants;
   4
   5 import edu.wpi.first.wpilibj.command.CommandGroup;
   6
   7 /**
   8  *
   9  */
  10 public class MoveDefenseArm extends CommandGroup {
  11
  12   double horizontalLimit, height;
  13
  14   public static final double ARM_SPEED = 0.6;
  15
  16   public MoveDefenseArm(double horizontalLimit, double height) {
  17     this.horizontalLimit = horizontalLimit;
  18     this.height = height;
  19     double targetArmAngle = calculateTargetArmAngle();
  20     double targetHandAngle = calculateTargetHandAngle();
  21     double handSpeed = ARM_SPEED * targetHandAngle / targetArmAngle;
  22     addParallel(new SetArmToAngle(ARM_SPEED, calculateTargetArmAngle()));
  23     addParallel(new SetHandToAngle(handSpeed, calculateTargetHandAngle()));
  24   }
  25
  26   protected double calculateTargetArmAngle() {
  27     double armAngle;
  28     armAngle = square(horizontalLimit) + square(height)
  29         + square(Constants.DefenseArm.ARM_LENGTH)
  30         - square(Constants.DefenseArm.HAND_LENGTH);
  31     armAngle /= 2 * Math.sqrt(square(horizontalLimit) + square(height))
  32         * Constants.DefenseArm.ARM_LENGTH;
  33     armAngle = Math.acos(armAngle);
  34     armAngle = Math.atan(height / horizontalLimit) - armAngle;
  35     return armAngle;
  36   }
  37
  38   protected double calculateTargetHandAngle() {
  39     double handAngle;
  40     handAngle = square(horizontalLimit) + square(height)
  41         + square(Constants.DefenseArm.HAND_LENGTH)
  42         - square(Constants.DefenseArm.ARM_LENGTH);
  43     handAngle /= 2 * Math.sqrt(square(horizontalLimit) + square(height))
  44         * Constants.DefenseArm.HAND_LENGTH;
  45     handAngle = Math.acos(handAngle);
  46     handAngle = handAngle + 90 - Math.atan(horizontalLimit / height);
  47     return handAngle;
  48   }
  49
  50   public double square(double num) {
  51     return num * num;
  52   }
  53 }