Exemplo n.º 1
0
  public void exec() {

    // pull data from vision
    double[] xyScaledDisplacement = robot().GetVisionCore().GetBallDisplacementFromCenter();

    // need to invert the X goal, so the robot heads towards the ball (not away)
    // for example: if the ball is to the left (negative), the robot needs to move to the left
    // (negative)
    // if the ball is high (negative, since it is already inverted by visionCore), the robot needs
    // to move back (negative)

    xyScaledDisplacement[0] = -xyScaledDisplacement[0];
    xyScaledDisplacement[1] = -xyScaledDisplacement[1];

    // give data to drive
    // goal, current, pid
    double[] goal = new double[] {0, 0};
    double[] power =
        followPid.calculate(
            goal, xyScaledDisplacement, FlyingBallP.get(), FlyingBallI.get(), FlyingBallD.get());

    // use PID? or just raw driving.
    // Don't use field-oriented drive here - too many frames of reference would need to change.
    //  instead, just use robot-relative drive, since the location of the ball is robot-relative.
    robot()
        .GetDriveCore()
        .mecanumDrive_Cartesian(
            power[0], power[1], 0, robot().GetSensorCore().GetDefaultOrientation());
    // Verbose("Robot followed ball");
  }
Exemplo n.º 2
0
  /** @return */
  public ExecState isFinished() {
    boolean currentOnTarget = pidArray.isOnTarget(0.5);
    if (!previousOnTarget && currentOnTarget) {
      enteredOnTargetMarker = CommonTools.Get().time.GetMarker();
    } else if (previousOnTarget
        && currentOnTarget
        && CommonTools.Get().time.GetElapsedMSecFromMarker(enteredOnTargetMarker)
            > onTargetWaitTimeMS) {
      return ExecState.SUCCESS;
    }

    previousOnTarget = currentOnTarget;

    return ExecState.NOT_DONE;
  }
Exemplo n.º 3
0
  public void exec() {
    XYPair curPos = robot().GetSensorCore().odometry.GetFieldOrientedPosition();
    double[] output =
        pidArray.calculate(
            target.toArray(),
            curPos.toArray(),
            translationPositionP.get(),
            translationPositionI.get(),
            translationPositionD.get());

    double rotationalForce = headingMaintainer.MaintainHeading();

    robot()
        .GetDriveCore()
        .mecanumDrive_Cartesian(
            output[0], output[1], rotationalForce, robot().GetSensorCore().getCurrentHeading());
  }
Exemplo n.º 4
0
 public void init() {
   headingMaintainer.SetSetpoint(robot().GetSensorCore().getCurrentHeading());
   headingMaintainer.init();
   pidArray.reset();
 }
Exemplo n.º 5
0
 /** @param target */
 public void setSetpoint(XYPair target) {
   pidArray.reset();
   this.target = target;
 }