Beispiel #1
0
  protected void initIMU() {

    // The nav6 IMU serial port configuration is 8 data bits, no parity, one stop bit.
    // No flow control is used.
    // Conveniently, these are the defaults used by the WPILib's SerialPort class.
    //
    // In addition, the WPILib's SerialPort class also defaults to:
    //
    // Timeout period of 5 seconds
    // Termination ('\n' character)
    // Transmit immediately

    initializeYawHistory();
    user_yaw_offset = 0;

    // set the nav6 into the desired update mode
    byte stream_command_buffer[] = new byte[256];
    int packet_length =
        IMUProtocol.encodeStreamCommand(stream_command_buffer, update_type, update_rate_hz);
    try {
      serial_port.write(stream_command_buffer, packet_length);
    } catch (RuntimeException ex) {
      ex.printStackTrace();
    }
  }
  /**
   * Code for thread that reads range values from the serial port.
   *
   * <p>Values are in the form RxxxxCR where: R is character 'R' xxxx is 3 or 4 decimal digits CR is
   * a carriage return character
   */
  @Override
  public void run() {
    // Reset serial port to empty buffers:
    serialPort.reset();

    // Start with an empty character buffer:
    byte[] data = new byte[0];
    int index = 0;

    int value = 0;
    boolean startCharacterFound = false;

    // Loop forever reading and processing characters from the serial port.
    while (true) {
      // If all previously read characters have been processed, read more characters:
      if (index >= data.length) {
        data = serialPort.read(serialPort.getBytesReceived());
        if (0 == data.length) {
          continue;
        }
        index = 0;
      }

      // Process a single character:
      byte c = data[index++];
      if (startCharacterFound) {
        // If we have seen a 'R', look for a CR or digit:
        if ('\r' == c) {
          serialRangeCm = value / serialRangeFactorCm;
          value = 0;
          startCharacterFound = false;
          serialSampleRate.addSample();
        } else {
          // Add current digit to value being accumulated:
          value = value * 10 + (c - '0');
        }
      } else {
        // See if character is the 'R' indicating the start of a value:
        startCharacterFound = 'R' == c;
      }
    }
  }
Beispiel #3
0
 /**
  * Constructs the IMU class, overriding the default update rate with a custom rate which may be
  * from 4 to 100, representing the number of updates per second sent by the nav6 IMU.
  *
  * <p>Note that increasing the update rate may increase the CPU utilization.
  *
  * @param serial_port BufferingSerialPort object to use
  * @param update_rate_hz Custom Update Rate (Hz)
  */
 public IMU(SerialPort serial_port, byte update_rate_hz) {
   ypr_update_data = new IMUProtocol.YPRUpdate();
   this.update_rate_hz = update_rate_hz;
   flags = 0;
   accel_fsr_g = DEFAULT_ACCEL_FSR_G;
   gyro_fsr_dps = DEFAULT_GYRO_FSR_DPS;
   this.serial_port = serial_port;
   yaw_history = new float[YAW_HISTORY_LENGTH];
   yaw = (float) 0.0;
   pitch = (float) 0.0;
   roll = (float) 0.0;
   try {
     serial_port.reset();
   } catch (RuntimeException ex) {
     ex.printStackTrace();
   }
   initIMU();
   m_thread = new Thread(this);
   m_thread.start();
 }
Beispiel #4
0
  public void run() {

    stop = false;
    boolean stream_response_received = false;
    double last_valid_packet_time = 0.0;
    int partial_binary_packet_count = 0;
    int stream_response_receive_count = 0;
    int timeout_count = 0;
    int discarded_bytes_count = 0;
    int port_reset_count = 0;
    double last_stream_command_sent_timestamp = 0.0;
    int updates_in_last_second = 0;
    double last_second_start_time = 0;
    try {
      serial_port.setReadBufferSize(512);
      serial_port.setTimeout(1.0);
      serial_port.enableTermination('\n');
      serial_port.flush();
      serial_port.reset();
    } catch (RuntimeException ex) {
      ex.printStackTrace();
    }

    IMUProtocol.StreamResponse response = new IMUProtocol.StreamResponse();

    byte[] stream_command = new byte[256];

    int cmd_packet_length =
        IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz);
    try {
      serial_port.reset();
      serial_port.write(stream_command, cmd_packet_length);
      serial_port.flush();
      port_reset_count++;
      // SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count);
      last_stream_command_sent_timestamp = Timer.getFPGATimestamp();
    } catch (RuntimeException ex) {
      ex.printStackTrace();
    }

    while (!stop) {
      try {

        // Wait, with delays to conserve CPU resources, until
        // bytes have arrived.

        while (!stop && (serial_port.getBytesReceived() < 1)) {
          Timer.delay(1.0 / update_rate_hz);
        }

        int packets_received = 0;
        byte[] received_data = serial_port.read(256);
        int bytes_read = received_data.length;
        if (bytes_read > 0) {
          byte_count += bytes_read;
          int i = 0;
          // Scan the buffer looking for valid packets
          while (i < bytes_read) {

            // Attempt to decode a packet

            int bytes_remaining = bytes_read - i;

            if (received_data[i] != IMUProtocol.PACKET_START_CHAR) {
              /* Skip over received bytes until a packet start is detected. */
              i++;
              discarded_bytes_count++;
              // SmartDashboard.putNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
              continue;
            } else {
              if ((bytes_remaining > 2) && (received_data[i + 1] == (byte) '#')) {
                /* Binary packet received; next byte is packet length-2 */
                byte total_expected_binary_data_bytes = received_data[i + 2];
                total_expected_binary_data_bytes += 2;
                while (bytes_remaining < total_expected_binary_data_bytes) {

                  /* This binary packet contains an embedded     */
                  /* end-of-line character.  Continue to receive */
                  /* more data until entire packet is received.  */
                  byte[] additional_received_data = serial_port.read(256);
                  byte_count += additional_received_data.length;
                  bytes_remaining += additional_received_data.length;

                  /* Resize array to hold existing and new data */
                  byte[] c = new byte[received_data.length + additional_received_data.length];
                  if (c.length > 0) {
                    System.arraycopy(received_data, 0, c, 0, received_data.length);
                    System.arraycopy(
                        additional_received_data,
                        0,
                        c,
                        received_data.length,
                        additional_received_data.length);
                    received_data = c;
                  } else {
                    /* Timeout waiting for remainder of binary packet */
                    i++;
                    bytes_remaining--;
                    partial_binary_packet_count++;
                    // SmartDashboard.putNumber("nav6 Partial Binary Packets",
                    // (double)partial_binary_packet_count);
                    continue;
                  }
                }
              }
            }

            int packet_length = decodePacketHandler(received_data, i, bytes_remaining);
            if (packet_length > 0) {
              packets_received++;
              update_count++;
              last_valid_packet_time = Timer.getFPGATimestamp();
              updates_in_last_second++;
              if ((last_valid_packet_time - last_second_start_time) > 1.0) {
                // SmartDashboard.putNumber("nav6 UpdatesPerSec", (double)updates_in_last_second);
                updates_in_last_second = 0;
                last_second_start_time = last_valid_packet_time;
              }
              i += packet_length;
            } else {
              packet_length =
                  IMUProtocol.decodeStreamResponse(received_data, i, bytes_remaining, response);
              if (packet_length > 0) {
                packets_received++;
                setStreamResponse(response);
                stream_response_received = true;
                i += packet_length;
                stream_response_receive_count++;
                // SmartDashboard.putNumber("nav6 Stream Responses",
                // (double)stream_response_receive_count);
              } else {
                // current index is not the start of a valid packet; increment
                i++;
              }
            }
          }

          if ((packets_received == 0) && (bytes_read == 256)) {
            // Workaround for issue found in SerialPort implementation:
            // No packets received and 256 bytes received; this
            // condition occurs in the SerialPort.  In this case,
            // reset the serial port.
            serial_port.reset();
            port_reset_count++;
            // SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count);
          }

          // If a stream configuration response has not been received within three seconds
          // of operation, (re)send a stream configuration request

          if (!stream_response_received
              && ((Timer.getFPGATimestamp() - last_stream_command_sent_timestamp) > 3.0)) {
            cmd_packet_length =
                IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz);
            try {
              last_stream_command_sent_timestamp = Timer.getFPGATimestamp();
              serial_port.write(stream_command, cmd_packet_length);
              serial_port.flush();
            } catch (RuntimeException ex2) {
              ex2.printStackTrace();
            }
          } else {
            // If no bytes remain in the buffer, and not awaiting a response, sleep a bit
            if (stream_response_received && (serial_port.getBytesReceived() == 0)) {
              Timer.delay(1.0 / update_rate_hz);
            }
          }

          /* If receiving data, but no valid packets have been received in the last second */
          /* the navX MXP may have been reset, but no exception has been detected.         */
          /* In this case , trigger transmission of a new stream_command, to ensure the    */
          /* streaming packet type is configured correctly.                                */

          if ((Timer.getFPGATimestamp() - last_valid_packet_time) > 1.0) {
            stream_response_received = false;
          }
        }
      } catch (RuntimeException ex) {
        // This exception typically indicates a Timeout
        stream_response_received = false;
        timeout_count++;
        // SmartDashboard.putNumber("nav6 Serial Port Timeouts", (double)timeout_count);
        // SmartDashboard.putString("LastNavExceptionBacktrace",ex.getStackTrace().toString());
        // SmartDashboard.putString("LastNavException", ex.getMessage() + "; " + ex.toString());
      }
    }
  }
Beispiel #5
0
 private void sendID(int id, String data) {
   arduino.writeString(String.valueOf(id) + deliminator + data);
 }