示例#1
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());
  }
示例#2
0
 public void init() {
   headingMaintainer.SetSetpoint(robot().GetSensorCore().getCurrentHeading());
   headingMaintainer.init();
   pidArray.reset();
 }