OpenRB-150 초음파센서 제어 도와주세요 ㅠㅠ

OpenRB-150에 초음파센서를 연결했는데 초음파 센서 인식이 안 되는 것 같습니다…

#include <Dynamixel2Arduino.h>
#include <Scheduler.h>

const int trigPin = 4;    //Trig 핀 할당
const int echoPin = 5;    //Echo 핀 할당
int h = 12;
int i,turn,current_d =0;
int g=100;
int b=-100;
uint32_t l;
uint32_t r;
uint32_t R;
uint32_t L;

// Please modify it to suit your hardware.
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield
  #include <SoftwareSerial.h>
  SoftwareSerial soft_serial(13, 14); // DYNAMIXELShield UART RX/TX
  #define DXL_SERIAL   Serial
  #define DEBUG_SERIAL soft_serial
  const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield
  #define DXL_SERIAL   Serial
  #define DEBUG_SERIAL SerialUSB
  const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield
  #define DXL_SERIAL   Serial1
  #define DEBUG_SERIAL SerialUSB
  const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit.
  #define DXL_SERIAL   Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
  #define DEBUG_SERIAL Serial
  const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board)
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
  // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
  // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
  #define DXL_SERIAL   Serial3
  #define DEBUG_SERIAL Serial
  const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
#elif defined(ARDUINO_OpenRB)  // When using OpenRB-150
  //OpenRB does not require the DIR control pin.
  #define DXL_SERIAL Serial1
  #define DEBUG_SERIAL Serial
  const int DXL_DIR_PIN = -1;
#else // Other boards when using DynamixelShield
  #define DXL_SERIAL   Serial1
  #define DEBUG_SERIAL Serial
  const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#endif

//Please see eManual Control Table section of your DYNAMIXEL.
//This example is written for DYNAMIXEL X series(excluding XL-320)
#define OPERATING_MODE_ADDR         11
#define OPERATING_MODE_ADDR_LEN     1
#define TORQUE_ENABLE_ADDR          64
#define TORQUE_ENABLE_ADDR_LEN      1
#define LED_ADDR                    65
#define LED_ADDR_LEN                1
#define GOAL_POSITION_ADDR          116
#define GOAL_POSITION_ADDR_LEN      4
#define PRESENT_POSITION_ADDR       132
#define PRESENT_POSITION_ADDR_LEN   4
#define POSITION_CONTROL_MODE       3
#define TIMEOUT 10    //default communication timeout 10ms

uint8_t turn_on = 1;
uint8_t turn_off = 0;

const uint8_t DXL_ID[] = {0,1};
const float DXL_PROTOCOL_VERSION = 2.0;

//uint8_t operatingMode = POSITION_CONTROL_MODE;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

void setup() {
   DEBUG_SERIAL.begin(115200);
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);
  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  // Get DYNAMIXEL information
  dxl.ping(DXL_ID[0]);
  dxl.ping(DXL_ID[1]);
  dxl.torqueOff(DXL_ID[0]);
  dxl.torqueOff(DXL_ID[1]);
  dxl.setOperatingMode(DXL_ID[0], OP_EXTENDED_POSITION);
  dxl.setOperatingMode(DXL_ID[1], OP_EXTENDED_POSITION);
  dxl.torqueOn(DXL_ID[0]);
  dxl.torqueOn(DXL_ID[1]);
  dxl.setGoalPosition(DXL_ID[0], 0);
  dxl.setGoalPosition(DXL_ID[1], 0);
  
  Scheduler.startLoop(loop2);
}

void loop() {
  // put your main code here, to run repeatedly:
    
  // 초음파를 보낸다. 다 보내면 echo가 HIGH 상태로 대기하게 된다.
    digitalWrite(trigPin, LOW);
    digitalWrite(echoPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

  
  // echoPin 이 HIGH를 유지한 시간을 저장 한다.
    unsigned long duration = pulseIn(echoPin, HIGH); 
  // HIGH 였을 때 시간(초음파가 보냈다가 다시 들어온 시간)을 가지고 거리를 계산 한다.
  float distance = ((float)(340 * duration) / 10000) / 2;
    
    r=3750;
    l=3750;
    R=340;
    L=340;
    
    /*while(i<1){

    delay(5000);
    RT();
    delay(800);
    
    if(distance>=2.75)
    {
      S();
      delay(1000);
      B();
      delay(1000);
      S();
      delay(500);
      LT();
      delay(800);
      i++;
    }
    
    if(distance<2.75){
    S();
    delay(1000);
    i++;
    }
  } //방향 정하기*/

      if(distance>=2.75){
              
      S();
      delay(1000);
      turn=turn%2;
      
     if(turn%2==0)
     {
      B();
      delay(500);
      S();
      delay(500);
      LT();
      delay(1000);
      S();
      delay(500);
      Go();
      delay(250);
      S();
      delay(500);
      LT();
      delay(1000);
      S();
      delay(500);
      turn+=1;
     }

     else
      B();
      delay(500);
      S();
      delay(500);
      RT();
      delay(1000);
      S();
      delay(500);
      Go();
      delay(250);
      S();
      delay(500);
      RT();
      delay(1000);
      S();
      delay(500);
      turn+=1;
      
      }

      Go();
      delay(500);
}
      



//0번: 왼 1번: 오른쪽
void Go(){
  dxl.torqueOff(DXL_ID[0]);
  dxl.torqueOff(DXL_ID[1]);
  dxl.setOperatingMode(DXL_ID[0], OP_VELOCITY);
  dxl.setOperatingMode(DXL_ID[1], OP_VELOCITY);
  dxl.torqueOn(DXL_ID[0]);
  dxl.torqueOn(DXL_ID[1]);
  dxl.setGoalVelocity(DXL_ID[0], g);
  dxl.setGoalVelocity(DXL_ID[1], g); 
}

void LT(){
  dxl.setGoalVelocity(DXL_ID[0], 0);
  dxl.torqueOff(DXL_ID[1]);
  dxl.setOperatingMode(DXL_ID[1], OP_EXTENDED_POSITION);
  dxl.torqueOn(DXL_ID[1]);
  dxl.setGoalPosition(DXL_ID[1], l);
  l+=3750;
}

void RT(){
  dxl.setGoalVelocity(DXL_ID[1], 0);
  dxl.torqueOff(DXL_ID[0]);
  dxl.setOperatingMode(DXL_ID[0], OP_EXTENDED_POSITION);
  dxl.torqueOn(DXL_ID[0]);
  dxl.setGoalPosition(DXL_ID[0], r);
  r+=3750;
}

void LB(){
  dxl.torqueOff(DXL_ID[0]);
  dxl.setOperatingMode(DXL_ID[0], OP_EXTENDED_POSITION);
  dxl.torqueOn(DXL_ID[0]);
  dxl.setGoalPosition(DXL_ID[0], L);
  L+=340;
}

void RB(){
  dxl.torqueOff(DXL_ID[1]);
  dxl.setOperatingMode(DXL_ID[1], OP_EXTENDED_POSITION);
  dxl.torqueOn(DXL_ID[1]);
  dxl.setGoalPosition(DXL_ID[1], R);
  R+=340;
}
void B(){
  dxl.setGoalVelocity(DXL_ID[0], b);
  dxl.setGoalVelocity(DXL_ID[1], b); // 후진
}

void S(){
  
  dxl.torqueOff(DXL_ID[0]);
  dxl.torqueOff(DXL_ID[1]);
  dxl.setOperatingMode(DXL_ID[0], OP_VELOCITY);
  dxl.setOperatingMode(DXL_ID[1], OP_VELOCITY);
  dxl.torqueOn(DXL_ID[0]);
  dxl.torqueOn(DXL_ID[1]);
  dxl.setGoalVelocity(DXL_ID[1], 0);
  dxl.setGoalVelocity(DXL_ID[0], 0); // 정지
}

void loop2(){
    digitalWrite(h,HIGH);
    delay(500);
    digitalWrite(h,LOW);
    delay(100);
    yield();
}

안녕하세요,

사용하려는 digital pin의 용도를 명확히 하기 위해 setup 함수 내에 pinMode를 아래와 같이 지정해주시는 것을 추천드립니다.

pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);

echoPin을 초음파 센서의 입력 단자로 설정하신 것 같은데 GPIO의 출력핀에 사용하는 digitalWrite를 쓰는 것이 맞는지 확인해보시기 바랍니다.

위 설정으로 100ms 간격으로 High Low 반복시켰을 때 4번핀 정상동작되는것으로 나타납니다.