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  * Given input target x and y positions of the tip of the defense arm apparatus:
   9  * first, calculates what the angle of the arm and hand corresponding to the
  10  * given x-y coordinate is
  11  * next, with speed of arm set to a constant, determine what the speed of hand
  12  * should be if proportional to speed of arm (with the ratio equal to the ratio
  13  * between arm angle to be adjusted and hand angle to be adjusted
  14  */
  15 public class MoveDefenseArm extends CommandGroup {
  16
  17   double horizontalLimit, height;
  18
  19   public static final double ARM_SPEED = 0.6;
  20
  21   public MoveDefenseArm(double horizontalLimit, double height) {
  22     this.horizontalLimit = horizontalLimit;
  23     this.height = height;
  24     double targetArmAngle = calculateTargetArmAngle();
  25     double targetHandAngle = calculateTargetHandAngle();
  26     double handSpeed = ARM_SPEED * targetHandAngle / targetArmAngle;
  27     addParallel(new SetArmToAngle(ARM_SPEED, calculateTargetArmAngle()));
  28     addParallel(new SetHandToAngle(handSpeed, calculateTargetHandAngle()));
  29   }
  30
  31   protected double calculateTargetArmAngle() {
  32     double armAngle;
  33     armAngle = square(horizontalLimit) + square(height)
  34         + square(Constants.DefenseArm.ARM_LENGTH)
  35         - square(Constants.DefenseArm.HAND_LENGTH);
  36     armAngle /= 2 * Math.sqrt(square(horizontalLimit) + square(height))
  37         * Constants.DefenseArm.ARM_LENGTH;
  38     armAngle = Math.acos(armAngle);
  39     armAngle = Math.atan(height / horizontalLimit) - armAngle;
  40     return armAngle;
  41   }
  42
  43   protected double calculateTargetHandAngle() {
  44     double handAngle;
  45     handAngle = square(horizontalLimit) + square(height)
  46         + square(Constants.DefenseArm.HAND_LENGTH)
  47         - square(Constants.DefenseArm.ARM_LENGTH);
  48     handAngle /= 2 * Math.sqrt(square(horizontalLimit) + square(height))
  49         * Constants.DefenseArm.HAND_LENGTH;
  50     handAngle = Math.acos(handAngle);
  51     handAngle = handAngle + 90 - Math.atan(horizontalLimit / height);
  52     return handAngle;
  53   }
  54
  55   public double square(double num) {
  56     return num * num;
  57   }
  58 }