Esempio 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");
  }
  public void exec() {
    if (robot().GetDriveCore().tankFieldOriented.get()) {

      // If the rotation joystick is being actuated, use that. Otherwise use the heading maintainer.
      XYPair rightJoy =
          new XYPair(robot().GetOICore().getRightJoyX(), robot().GetOICore().getRightJoyY());
      double rotation = 0;

      if (rightJoy.internalAbsoluteSum() > tankDriveDeadbandRightJoystick.get()) {
        rotation = DriveCore.remapMinusDeadband(rightJoy.X, tankDriveDeadbandRightJoystick.get());

        // Set the desired heading to the current heading so we don't thrash once the joystick is
        // released.
        robot().GetDriveCore().SetDesiredHeading(robot().GetSensorCore().getCurrentHeading());
      } else {
        // We'll first get the joystick vector, and set a desired
        // heading for the robot which maximizes speed towards that vector.
        this.orientForSpeed.exec();

        // Get the heading we should drive the arcade at from the PID.
        // This value is the rotation speed that we pass to the arcade drive.
        rotation = this.headingMaintainer.MaintainHeading();
      }

      // Our translation speed should be the magnitude of the joystick vector,
      // projected onto the robot heading.
      // So when the robot is at 90 degrees to the joystick heading,
      // it rotates rather than translating.
      // Conversely, when the robot is exactly the same as the joystick
      // heading, the rotation speed should be 0 and the translation speed should be 1.
      // Magnitude will be capped at 1.
      double currentRobotHeading = robot().GetSensorCore().getCurrentHeading();

      XYPair robotOrientation = XYPair.fromPolarCoordinates(1, currentRobotHeading);

      double movement = robotOrientation.project(robot().GetOICore().getLeftJoyVector());
      if (movement > 1.0) {
        movement = 1.0;
      } else if (movement < -1.0) {
        movement = -1.0;
      }

      robot().GetDriveCore().arcadeDrive(movement, rotation);

    } else {
      robot()
          .GetDriveCore()
          .tankDrive(robot().GetOICore().getLeftJoyY(), robot().GetOICore().getRightJoyY());
    }
  }
Esempio 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());
  }