예제 #1
0
  /**
   * Holonomic Drive method for omni wheel robots
   *
   * <p>This is a modified version of the WPILIB Mecannum code. The formula works but there is no
   * need to invert the rotations because you rotate but increasing the speed of every wheel
   *
   * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
   * @param direction The direction the robot should drive. The direction and maginitute are
   *     independent of the rotation rate.
   * @param rotation The rate of rotation for the robot that is completely independent of the
   *     magnitude or direction. [-1.0..1.0]
   */
  public void holonomicDrive(double magnitude, double direction, double rotation) {
    // Normalized for full power along the Cartesian axes.
    magnitude = limit(magnitude) * Math.sqrt(2.0);
    // The rollers are at 45 degree angles.
    double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
    double cosD = Math.cos(dirInRad);
    double sinD = Math.cos(dirInRad);

    double wheelSpeeds[] = new double[kMaxNumberOfMotors];
    wheelSpeeds[MotorType.kFrontLeft.value] = (sinD * magnitude + rotation);
    wheelSpeeds[MotorType.kRearRight.value] = (sinD * magnitude - rotation);
    wheelSpeeds[MotorType.kFrontRight.value] = (cosD * magnitude + rotation);
    wheelSpeeds[MotorType.kRearLeft.value] = (cosD * magnitude - rotation);

    normalize(wheelSpeeds);

    byte syncGroup = (byte) 0x80;

    m_frontLeftMotor.set(
        wheelSpeeds[MotorType.kFrontLeft.value]
            * m_invertedMotors[MotorType.kFrontLeft.value]
            * m_maxOutput,
        syncGroup);
    m_frontRightMotor.set(
        wheelSpeeds[MotorType.kFrontRight.value]
            * m_invertedMotors[MotorType.kFrontRight.value]
            * m_maxOutput,
        syncGroup);
    m_rearLeftMotor.set(
        wheelSpeeds[MotorType.kRearLeft.value]
            * m_invertedMotors[MotorType.kRearLeft.value]
            * m_maxOutput,
        syncGroup);
    m_rearRightMotor.set(
        wheelSpeeds[MotorType.kRearRight.value]
            * m_invertedMotors[MotorType.kRearRight.value]
            * m_maxOutput,
        syncGroup);

    if (m_isCANInitialized) {
      try {
        CANJaguar.updateSyncGroup(syncGroup);
      } catch (CANNotInitializedException e) {
        m_isCANInitialized = false;
      } catch (CANTimeoutException e) {
      }
    }

    if (m_safetyHelper != null) {
      m_safetyHelper.feed();
    }
  }
예제 #2
0
  /**
   * Move the arm
   *
   * @param rpm the speed of the arm
   */
  public void moveArm(double speed) {
    try {
      if (turbomode) {
        jag.setX(.6 * (speed));
        try {
          CANJaguar.updateSyncGroup(syncGroup);
        } catch (CANNotInitializedException e) {
        }
      } else {
        jag.setX(.3 * (speed));
        try {
          CANJaguar.updateSyncGroup(syncGroup);
        } catch (CANNotInitializedException e) {
        }
      }

    } catch (CANTimeoutException ex) {
      //                  ex.printStackTrace();
      System.out.println("CANNDriveTrain.java:133");
    }
    if (m_safetyHelper != null) m_safetyHelper.feed();
  }