pedal.md - dingdongdengdong/astra_ws GitHub Wiki

#include "comm.h" // 통신 관련 함수 및 타입 정의를 포함하는 헤더 파일
#include <string.h> // memcpy 함수를 사용하기 위한 헤더 파일
#include <Arduino.h> // Arduino 플랫폼에서 Serial 통신 등을 사용하기 위한 헤더 파일

// @brief 시리얼 통신을 초기화하는 함수.
// 로봇 시스템과 외부 제어 장치(예: 페달 인터페이스) 간의 통신을 설정합니다.
// 시리얼 포트를 지정된 보드 속도로 시작하고 포트가 준비될 때까지 기다립니다.
// @return 오류 발생 시 true, 정상 시 false를 반환합니다. (이 코드에서는 항상 false 반환)
bool serial_init(void) {
  Serial.begin(921600); // 시리얼 통신 시작 (보드 속도 921600 bps). 통신 속도는 시스템 요구사항에 따라 조정됩니다.
  while (!Serial) delay(1); // 시리얼 포트가 열릴 때까지 대기. 통신 준비 상태 확인.
  return false; // 초기화 성공을 나타냄
}

// @brief 단일 문자를 블로킹 방식으로 시리얼 포트로 전송하는 함수.
// 페달 상태와 같은 제어 명령이나 피드백 데이터를 구성하는 바이트를 전송하는 데 사용됩니다.
// 문자가 전송될 때까지 함수 실행이 차단됩니다.
// @param c 전송할 1바이트 데이터입니다.
// @return 오류 발생 시 true, 정상 시 false를 반환합니다. (이 코드에서는 항상 false 반환)
bool serial_send_blocking(uint8_t c) {
  Serial.write(c); // 시리얼 포트로 1바이트 데이터 전송
  return false; // 오류가 없음을 나타냄
}

// @brief 시리얼 수신 버퍼에서 단일 문자를 논블로킹 방식으로 폴링하는 함수.
// 외부 시스템으로부터 제어 명령 등을 수신할 때 사용됩니다.
// 수신된 데이터가 없으면 즉시 반환됩니다.
// @param c 수신된 데이터를 저장할 uint8_t 포인터입니다.
// @return 데이터 수신 시 false, 수신할 데이터가 없거나 오류 발생 시 true를 반환합니다.
bool serial_recv_poll(uint8_t *c) {
  // 경고: 이 함수는 멀티스레드 환경에서 안전하지 않을 수 있습니다 (non-thread safe). 단일 루프 환경에서 주로 사용됩니다.
  if (Serial.available()) { // 수신 버퍼에 읽을 데이터가 있는지 확인
    *c = Serial.read(); // 데이터를 읽어와 c가 가리키는 메모리에 저장
    return false; // 데이터 수신 성공
  } else {
    return true; // 수신할 데이터 없음
  }
}

// @brief 통신에 사용되는 각 데이터 패킷 타입에 대한 페이로드(실제 데이터)의 크기를 정의하는 배열.
// COMM_TYPE 열거형의 순서에 맞게 각 타입의 페이로드 크기를 명시합니다.
// 페달 인터페이스의 경우, 센서 데이터(FEEDBACK) 또는 로봇 제어 명령(CTRL) 등의 크기가 여기에 정의됩니다.
int comm_payload_size[] = {
  16, // COMM_TYPE_PING: 연결 확인용 핑 패킷 페이로드 크기
  16, // COMM_TYPE_PONG: 핑 응답용 퐁 패킷 페이로드 크기
  16, // COMM_TYPE_FEEDBACK: 페달 센서 데이터와 같은 로봇의 피드백 패킷 페이로드 크기 (예: 8개의 아날로그 센서 값)
};

// @brief 특정 데이터 패킷 타입을 수신 시 `comm_recv_poll_last` 함수에서 무시할지 여부를 나타내는 배열.
// 페달 인터페이스와 같이 실시간성이 중요한 시스템에서는 최신 데이터만 처리하기 위해 일부 패킷을 무시할 수 있습니다.
// true로 설정된 패킷 타입은 `comm_recv_poll_last` 호출 시 최신 데이터로 갱신되지 않고 즉시 반환됩니다.
bool comm_type_importance[] = {
  false, // COMM_TYPE_PING: 핑 패킷은 무시 가능 (주기적인 상태 확인 목적이 아니라면)
  false, // COMM_TYPE_PONG: 퐁 패킷은 무시 가능
  false, // COMM_TYPE_FEEDBACK: 페달 피드백 데이터는 무시 가능 (항상 최신 상태를 유지하는 것이 중요)
};

// @brief 데이터 패킷을 블로킹 방식으로 시리얼 포트로 전송하는 함수.
// 패킷 헤더(0x5A), 패킷 타입, 페이로드 데이터를 순차적으로 전송합니다.
// 페달 인터페이스에서는 주로 센서 데이터를 로봇 시스템으로 전송하는 데 사용될 수 있습니다.
// @param type 전송할 데이터 패킷의 타입입니다 (comm_type_t).
// @param payload 전송할 실제 데이터가 담긴 uint8_t 배열입니다. (예: 8개의 아날로그 센서 값)
// @return 전송 중 오류 발생 시 true, 정상 시 false를 반환합니다.
bool comm_send_blocking(comm_type_t type, const uint8_t payload[]) {
  bool ret;
  // 패킷 시작을 알리는 헤더 바이트 전송 (0x5A)
  ret = serial_send_blocking(0x5A);
  if (ret) return true; // 전송 오류 시 즉시 반환
  // 패킷 타입 바이트 전송 (예: COMM_TYPE_FEEDBACK)
  ret = serial_send_blocking((uint8_t)type);
  if (ret) return true; // 전송 오류 시 즉시 반환
  // 패킷 페이로드 데이터 전송 (예: 아날로그 센서 값)
  for (int i = 0; i < comm_payload_size[type]; ++i) {
    ret = serial_send_blocking(payload[i]);
    if (ret) return true; // 전송 오류 시 즉시 반환
  }
  return false; // 전송 성공
}

// 수신 버퍼: 패킷 헤더 (1 바이트) + 패킷 타입 (1 바이트) + 최대 페이로드 크기
// 외부 시스템(로봇)으로부터의 제어 명령 등을 임시 저장하는 버퍼입니다.
static uint8_t recv_buf[2 + COMM_PAYLOAD_SIZE_MAX];
// 수신 버퍼의 현재 채워진 위치를 나타내는 포인터
static int recv_buf_p;

// @brief 시리얼 포트에서 데이터 패킷을 수신하는 논블로킹 함수.
// 데이터가 완전히 수신될 때까지 내부적으로 폴링하며, 수신된 데이터가 없으면 즉시 반환됩니다.
// 유효하지 않은 데이터나 알 수 없는 타입의 패킷은 경고를 출력하고 무시합니다.
// 페달 인터페이스에서는 로봇 시스템으로부터의 상태 정보나 설정 값 등을 수신하는 데 사용될 수 있습니다.
// @param type 수신된 데이터 패킷의 타입을 저장할 comm_type_t 포인터입니다.
// @param payload 수신된 실제 데이터를 저장할 uint8_t 배열입니다.
// @return 데이터 패킷 수신 성공 시 false, 수신할 데이터가 없거나 오류 발생 시 true를 반환합니다.
bool comm_recv_poll(comm_type_t *type, uint8_t payload[]) {
  bool ret;
  while (true) {
    uint8_t buf;
    ret = serial_recv_poll(&buf);
    if (ret) return true; // 수신할 데이터가 없으면 true 반환

    // 수신 버퍼의 시작 (recv_buf_p == 0)에서 데이터가 패킷 헤더(0x5A)가 아니면 무시
    if (recv_buf_p == 0 && buf != 0x5A) {
      Serial.println("comm: warning: Received wrong byte!"); // 잘못된 시작 바이트 경고
      recv_buf_p = 0; // 버퍼 초기화
      continue; // 다음 바이트를 기다림
    }
    // 수신된 데이터를 버퍼에 저장하고 버퍼 포인터 증가
    recv_buf[recv_buf_p++] = buf;

    // 어설션: 수신 버퍼가 오버플로우되지 않도록 확인. 버퍼 크기가 충분한지 검사.
    assert(recv_buf_p <= sizeof recv_buf);

    // 패킷 타입 수신 후 (recv_buf_p == 2), 알 수 없는 타입인지 확인
    // 알 수 없는 타입이면 경고를 출력하고 버퍼를 리셋
    if (recv_buf_p == 2 && recv_buf[1] >= (sizeof comm_payload_size) / (sizeof comm_payload_size[0])) {
      Serial.println("comm: warning: Received wrong type!"); // 알 수 없는 패킷 타입 경고
      recv_buf_p = 0; // 수신 상태를 초기화
      continue; // 잘못된 타입 수신 시 다음 패킷을 기다림
    }

    // 완전한 데이터 패킷이 수신되었는지 확인
    // 수신 버퍼의 크기가 헤더(1) + 타입(1) + 해당 타입의 페이로드 크기와 같으면 완전한 패킷
    if (recv_buf_p >= 2 && recv_buf_p == 2 + comm_payload_size[recv_buf[1]]) {
      // 수신된 페이로드 데이터를 출력 버퍼로 복사
      *type = (comm_type_t)recv_buf[1]; // 수신된 패킷 타입 설정
      memcpy(payload, recv_buf + 2, comm_payload_size[recv_buf[1]]); // 페이로드 데이터 복사
      // 수신 버퍼 초기화
      recv_buf_p = 0;
      break; // 완전한 패킷 수신 완료
    }
    // TODO: 데이터 수신 중 타임아웃 메커니즘 추가 필요. 일정 시간 내에 패킷이 완료되지 않으면 오류 처리.
  }
  return false; // 데이터 패킷 수신 성공
}

// @brief 시리얼 수신 버퍼를 가능한 한 비우고 가장 마지막에 수신된 데이터 패킷을 반환하는 함수.
// 페달 인터페이스와 같이 항상 최신 제어/피드백 데이터가 중요한 경우, 중간 데이터를 버리고 최신 데이터만 처리하는 데 유용합니다.
// 여러 개의 패킷이 빠르게 수신될 경우, 중간 패킷들은 버려지고 가장 최신 패킷만 처리됩니다.
// 이는 시스템 간의 주기를 동기화하고 실시간성을 확보하는 데 도움이 될 수 있습니다.
// 중요도가 낮은 패킷은 무시될 수 있습니다.
// @param type 수신된 마지막 데이터 패킷의 타입을 저장할 comm_type_t 포인터입니다.
// @param payload 수신된 마지막 실제 데이터를 저장할 uint8_t 배열입니다.
// @return 데이터 패킷 수신 성공 시 false, 수신할 데이터가 없거나 오류 발생 시 true를 반환합니다.
bool comm_recv_poll_last(comm_type_t *type, uint8_t payload[]) {
  bool last_ret = true; // 마지막 수신 시도 결과를 저장 (기본값: 데이터 없음)

  // 최소한 한 번 comm_recv_poll을 호출하여 데이터 수신을 시도
  bool ret = comm_recv_poll(type, payload);
  // 데이터가 더 이상 없을 때까지 comm_recv_poll을 반복 호출하여 버퍼를 비움
  while (!ret) {
    // 현재 수신된 패킷의 중요도를 확인
    if (comm_type_importance[*type]) {
      // 중요 패킷인 경우, 버퍼를 비우지 않고 현재 패킷을 즉시 반환 (예: 긴급 정지 명령 등)
      return ret;
    }
    // 중요하지 않은 패킷인 경우, 현재 수신 결과를 저장하고 다음 패킷 수신 시도
    last_ret = ret;
    ret = comm_recv_poll(type, payload);
  }
  // 데이터가 더 이상 없을 때 (ret이 true가 될 때), 마지막으로 성공적으로 수신했던 결과 (last_ret)를 반환
  return ret = last_ret;
}

// @brief 통신 모듈을 초기화하는 함수.
// 시리얼 통신을 설정하고 수신 버퍼를 사용할 준비를 합니다.
// @return 초기화 중 오류 발생 시 true, 정상 시 false를 반환합니다.
bool comm_init(void) {
  bool ret = serial_init(); // 시리얼 통신 초기화
  if (ret) return true; // 초기화 오류 시 즉시 반환
  recv_buf_p = 0; // 수신 버퍼 포인터 초기화 (버퍼 비우기)
  return false; // 초기화 성공
}

#include <Arduino.h> // Arduino 플랫폼에서 아날로그 읽기 등을 사용하기 위한 헤더 파일
#include "main.h" // 메인 함수 관련 정의 (필요하다면)
#include "comm.h" // 커스텀 통신 헤더 파일: 시리얼 통신 프로토콜 정의
#include <lwip/sockets.h> // 네트워크 바이트 순서 변환 함수 (htons)를 위한 헤더 (ESP32 등에서 사용)

// @brief Arduino 보드의 초기 설정을 수행하는 함수.
// 아날로그 입력 해상도 설정, 통신 모듈 초기화 등을 수행합니다.
void setup() {
  // 아날로그-디지털 컨버터(ADC)의 해상도를 12비트로 설정합니다 (0-4095).
  // 이는 페달 센서와 같은 아날로그 입력의 정밀도를 결정합니다.
  analogReadResolution(12);

  // 로봇 시스템과의 시리얼 통신 모듈을 초기화합니다.
  // comm.cpp에 정의된 comm_init 함수를 호출합니다.
  comm_init();
}

// @brief 로봇 시스템의 메인 실행 루프 함수.
// 주기적으로 페달 센서 값을 읽고, 이를 통신 패킷으로 구성하여 로봇 시스템으로 전송합니다.
void loop() {
  // ESP32의 경우 ADC 2는 Wi-Fi 사용 시 제약이 있을 수 있습니다.
  // (참고: https://blog.mjyai.com/2022/10/28/esp32-arduino-ads1115-adc-benchmark/)
  // 여기서는 8개의 아날로그 센서 값을 저장할 배열을 선언합니다.
  // 이 값들은 페달의 각 축(전진, 좌우, 회전 등)에 해당할 수 있습니다.
  uint16_t analog[8];
  analog[0] = analogRead(36); // 아날로그 핀 36에서 값 읽기 (예: 전진/후진 페달)
  analog[1] = analogRead(39); // 아날로그 핀 39에서 값 읽기 (예: 좌우 조향 페달)
  analog[2] = analogRead(34); // 아날로그 핀 34에서 값 읽기
  analog[3] = analogRead(35); // 아날로그 핀 35에서 값 읽기
  analog[4] = analogRead(32); // 아날로그 핀 32에서 값 읽기
  analog[5] = analogRead(33); // 아날로그 핀 33에서 값 읽기
  analog[6] = analogRead(25); // 아날로그 핀 25에서 값 읽기
  analog[7] = analogRead(26); // 아날로그 핀 26에서 값 읽기

  // 시리얼 모니터로 아날로그 값들을 출력하여 디버깅할 때 사용할 수 있습니다.
  // Serial.printf("%hu %hu %hu %hu %hu %hu %hu %hu\n", analog[0], analog[1], analog[2], analog[3], analog[4], analog[5], analog[6], analog[7]);

  // 아날로그 값들은 16비트 정수이므로, 네트워크 전송을 위해 바이트 순서를 변환합니다.
  // htons: Host to Network Short (16비트 데이터 변환)
  for (int i = 0; i < 8; ++i) analog[i] = htons(analog[i]);

  // 읽은 아날로그 값들 (페달 상태)을 COMM_TYPE_FEEDBACK 패킷으로 구성하여 전송합니다.
  // 이 패킷은 로봇 시스템으로 전달되어 로봇 제어에 사용될 수 있습니다.
  comm_send_blocking(COMM_TYPE_FEEDBACK, (uint8_t *)analog);

  // 100ms 지연.
  // 이는 페달 상태를 로봇으로 전송하는 주기를 결정합니다.
  // 제어 시스템의 요구사항에 따라 조정될 수 있습니다.
  delay(100);

  // 참고: 이 코드는 페달 센서 값을 읽어 로봇으로 전송하는 부분만 구현되어 있습니다.
  // 로봇 시스템에서 이 피드백 데이터를 어떻게 해석하고 로봇 움직임으로 변환하는지는
  // 로봇 측의 소프트웨어에서 처리됩니다. 또한, 로봇으로부터의 응답 수신 로직은
  // comm_recv_poll 또는 comm_recv_poll_last 함수를 사용하여 추가 구현될 수 있습니다.
}