Пример #1
0
  /**
   * This method implements mecanum drive where x controls how fast the robot will go in the x
   * direction, and y controls how fast the robot will go in the y direction. Rotation controls how
   * fast the robot rotates and gyroAngle specifies the heading the robot should maintain.
   *
   * @param x specifies the x power.
   * @param y specifies the y power.
   * @param rotation specifies the rotating power.
   * @param inverted specifies true to invert control (i.e. robot front becomes robot back).
   * @param gyroAngle specifies the gyro angle to maintain.
   */
  public void mecanumDrive_Cartesian(
      double x, double y, double rotation, boolean inverted, double gyroAngle) {
    final String funcName = "mecanumDrive_Cartesian";

    if (debugEnabled) {
      dbgTrace.traceEnter(
          funcName,
          TrcDbgTrace.TraceLevel.API,
          "x=%f,y=%f,rot=%f,inverted=%s,angle=%f",
          x,
          y,
          rotation,
          Boolean.toString(inverted),
          gyroAngle);
      dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
    }

    if (numMotors != MAX_NUM_MOTORS) {
      throw new IllegalArgumentException("Mecanum drive requires 4 motors");
    }

    x = TrcUtil.limit(x);
    y = TrcUtil.limit(y);
    rotation = TrcUtil.limit(rotation);

    if (inverted) {
      x = -x;
      y = -y;
    }

    double cosA = Math.cos(Math.toRadians(gyroAngle));
    double sinA = Math.sin(Math.toRadians(gyroAngle));
    x = x * cosA - y * sinA;
    y = x * sinA + y * cosA;

    double wheelSpeeds[] = new double[MAX_NUM_MOTORS];
    wheelSpeeds[MotorType.kFrontLeft_val] = x + y + rotation;
    wheelSpeeds[MotorType.kFrontRight_val] = -x + y - rotation;
    wheelSpeeds[MotorType.kRearLeft_val] = -x + y + rotation;
    wheelSpeeds[MotorType.kRearRight_val] = x + y - rotation;
    normalize(wheelSpeeds);

    if (frontLeftMotor != null) {
      frontLeftMotor.setPower(wheelSpeeds[MotorType.kFrontLeft_val]);
    }

    if (frontRightMotor != null) {
      frontRightMotor.setPower(wheelSpeeds[MotorType.kFrontRight_val]);
    }

    if (rearLeftMotor != null) {
      rearLeftMotor.setPower(wheelSpeeds[MotorType.kRearLeft_val]);
    }

    if (rearRightMotor != null) {
      rearRightMotor.setPower(wheelSpeeds[MotorType.kRearRight_val]);
    }
  } // mecanumDrive_Cartesian
Пример #2
0
  /**
   * This method implements arcade drive where drivePower controls how fast the robot goes in the
   * y-axis and turnPower controls how fast it will turn.
   *
   * @param drivePower specifies the drive power value.
   * @param turnPower specifies the turn power value.
   * @param inverted specifies true to invert control (i.e. robot front becomes robot back).
   */
  public void arcadeDrive(double drivePower, double turnPower, boolean inverted) {
    final String funcName = "arcadeDrive";
    double leftPower;
    double rightPower;

    if (debugEnabled) {
      dbgTrace.traceEnter(
          funcName,
          TrcDbgTrace.TraceLevel.API,
          "drivePower=%f,turnPower=%f,inverted=%s",
          drivePower,
          turnPower,
          Boolean.toString(inverted));
      dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
    }

    drivePower = TrcUtil.limit(drivePower);
    turnPower = TrcUtil.limit(turnPower);

    if (drivePower + turnPower > MOTOR_MAX_VALUE) {
      //
      // Forward right:
      //  left = drive + turn - (drive + turn - MOTOR_MAX_VALUE)
      //  right = drive - turn - (drive + turn - MOTOR_MAX_VALUE)
      //
      leftPower = MOTOR_MAX_VALUE;
      rightPower = -2 * turnPower + MOTOR_MAX_VALUE;
    } else if (drivePower - turnPower > MOTOR_MAX_VALUE) {
      //
      // Forward left:
      //  left = drive + turn - (drive - turn - MOTOR_MAX_VALUE)
      //  right = drive - turn - (drive - turn - MOTOR_MAX_VALUE)
      //
      leftPower = 2 * turnPower + MOTOR_MAX_VALUE;
      rightPower = MOTOR_MAX_VALUE;
    } else if (drivePower + turnPower < MOTOR_MIN_VALUE) {
      //
      // Backward left:
      //  left = drive + turn - (drive + turn - MOTOR_MIN_VALUE)
      //  right = drive - turn - (drive + turn - MOTOR_MIN_VALUE)
      //
      leftPower = MOTOR_MIN_VALUE;
      rightPower = -2 * turnPower + MOTOR_MIN_VALUE;
    } else if (drivePower - turnPower < MOTOR_MIN_VALUE) {
      //
      // Backward right:
      //  left = drive + turn - (drive - turn - MOTOR_MIN_VALUE)
      //  right = drive - turn - (drive - turn - MOTOR_MIN_VALUE)
      //
      leftPower = 2 * turnPower + MOTOR_MIN_VALUE;
      rightPower = MOTOR_MIN_VALUE;
    } else {
      leftPower = drivePower + turnPower;
      rightPower = drivePower - turnPower;
    }
    tankDrive(leftPower, rightPower, inverted);
  } // arcadeDrive
Пример #3
0
  /**
   * This method implements mecanum drive where magnitude controls how fast the robot will go in the
   * given direction and how fast it will robote.
   *
   * @param magnitude specifies the magnitude combining x and y axes.
   * @param direction specifies the direction in degrees.
   * @param rotation specifies the rotation power.
   * @param inverted specifies true to invert control (i.e. robot front becomes robot back).
   */
  public void mecanumDrive_Polar(
      double magnitude, double direction, double rotation, boolean inverted) {
    final String funcName = "mecanumDrive_Polar";

    if (debugEnabled) {
      dbgTrace.traceEnter(
          funcName,
          TrcDbgTrace.TraceLevel.API,
          "mag=%f,dir=%f,rot=%f,inverted=%s",
          magnitude,
          direction,
          rotation,
          Boolean.toString(inverted));
      dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
    }

    if (numMotors != MAX_NUM_MOTORS) {
      throw new IllegalArgumentException("Mecanum drive requires 4 motors");
    }

    magnitude = TrcUtil.limit(magnitude) * Math.sqrt(2.0);
    if (inverted) {
      direction += 180.0;
      direction %= 360.0;
    }

    double dirInRad = Math.toRadians(direction + 45.0);
    double cosD = Math.cos(dirInRad);
    double sinD = Math.sin(dirInRad);

    double wheelSpeeds[] = new double[MAX_NUM_MOTORS];
    wheelSpeeds[MotorType.kFrontLeft_val] = (sinD * magnitude + rotation);
    wheelSpeeds[MotorType.kFrontRight_val] = (cosD * magnitude - rotation);
    wheelSpeeds[MotorType.kRearLeft_val] = (cosD * magnitude + rotation);
    wheelSpeeds[MotorType.kRearRight_val] = (sinD * magnitude - rotation);
    normalize(wheelSpeeds);

    if (frontLeftMotor != null) {
      frontLeftMotor.setPower(wheelSpeeds[MotorType.kFrontLeft_val]);
    }

    if (frontRightMotor != null) {
      frontRightMotor.setPower(wheelSpeeds[MotorType.kFrontRight_val]);
    }

    if (rearLeftMotor != null) {
      rearLeftMotor.setPower(wheelSpeeds[MotorType.kRearLeft_val]);
    }

    if (rearRightMotor != null) {
      rearRightMotor.setPower(wheelSpeeds[MotorType.kRearRight_val]);
    }
  } // mecanumDrive_Polar
Пример #4
0
  /**
   * This method implements tank drive where leftPower controls the left motors and right power
   * controls the right motors.
   *
   * @param leftPower specifies left power value.
   * @param rightPower specifies right power value.
   * @param inverted specifies true to invert control (i.e. robot front becomes robot back).
   */
  public void tankDrive(double leftPower, double rightPower, boolean inverted) {
    final String funcName = "tankDrive";

    if (debugEnabled) {
      dbgTrace.traceEnter(
          funcName,
          TrcDbgTrace.TraceLevel.API,
          "leftPower=%f,rightPower=%f,inverted=%s",
          leftPower,
          rightPower,
          Boolean.toString(inverted));
      dbgTrace.traceExit(funcName, TrcDbgTrace.TraceLevel.API);
    }

    leftPower = TrcUtil.limit(leftPower);
    rightPower = TrcUtil.limit(rightPower);

    if (inverted) {
      double swap = leftPower;
      leftPower = -rightPower;
      rightPower = -swap;
    }

    if (frontLeftMotor != null) {
      frontLeftMotor.setPower(leftPower);
    }

    if (frontRightMotor != null) {
      frontRightMotor.setPower(rightPower);
    }

    if (rearLeftMotor != null) {
      rearLeftMotor.setPower(leftPower);
    }

    if (rearRightMotor != null) {
      rearRightMotor.setPower(rightPower);
    }
  } // tankDrive