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();
}