Embedded Files
DARwIn-OP의 main.cpp의 위치
기본적인
DARwIn-OP/Linux/project/demo/main.cpp
를 찾을 수 있도록 한다.
- main.cpp 에서의 header file 선언
main.cpp 를 실행 하기 위해 필요한 header file들을 불러오는 과정
main.cpp 기본적으로 필요한 함수 선언을 위해 header file을 불러온다.
header file 선언은
#include <XXX.h> : 기존의 라이브러리 헤더파일을 불러오기
#include "XXX.h" : 같은 폴더내의 헤더파일을 불러오기
두가지 방법이 있다.
그 외 필요한 변수 정의와 void() 함수 정의
main.cpp
/
*
* main.cpp
*
* Created on: 2011. 1. 4.
* Author: robotis
*/
//기존의 라이브러리 헤더파일을 불러오기
#include <stdio.h>
#include <unistd.h>
#include <limits.h>
#include <string.h>
#include <libgen.h>
#include <signal.h>
//같은 폴더내에 헤더파일을 불러오기
#include "mjpg_streamer.h"
#include "LinuxDARwIn.h"
#include "StatusCheck.h"
#include "VisionMode.h"
#ifdef MX28_1024 //#ifdef 정의 되었다면 하고 안되어있으면 넘어가기
#define MOTION_FILE_PATH "../../../Data/motion_1024.bin"
#else
#define MOTION_FILE_PATH "../../../Data/motion_4096.bin"
#endif
#define INI_FILE_PATH "../../../Data/config.ini"
#define SCRIPT_FILE_PATH "script.asc"
#define U2D_DEV_NAME0 "/dev/ttyUSB0" //cm730 장치 드라이버
#define U2D_DEV_NAME1 "/dev/ttyUSB1"
LinuxCM730 linux_cm730(U2D_DEV_NAME0);
CM730 cm730(&linux_cm730);
void change_current_dir()
{
char exepath[1024] = {0};
if(readlink("/proc/self/exe", exepath, sizeof(exepath)) != -1)
{
if(chdir(dirname(exepath)))
fprintf(stderr, "chdir error!! \n");
}
}
void sighandler(int sig)
{
exit(0);
}
// main.cpp의 main 함수
int main(void)
{
//장치비젼처리시작
signal(SIGABRT, &sighandler);
signal(SIGTERM, &sighandler);
signal(SIGQUIT, &sighandler);
signal(SIGINT, &sighandler);
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
Image* rgb_output = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE);
LinuxCamera::GetInstance()->Initialize(0);
LinuxCamera::GetInstance()->SetCameraSettings(CameraSettings()); // set default
LinuxCamera::GetInstance()->LoadINISettings(ini); // load from ini
mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT);
ColorFinder* ball_finder = new ColorFinder();
ball_finder->LoadINISettings(ini);
httpd::ball_finder = ball_finder;
BallTracker tracker = BallTracker(); // BallTracker tracker 가 BallTracker함수이다.
BallFollower follower = BallFollower(); // BallFollower follower = BallFollower함수이다.
ColorFinder* red_finder = new ColorFinder(0, 15, 45, 0, 0.3, 50.0);
red_finder->LoadINISettings(ini, "RED");
httpd::red_finder = red_finder;
ColorFinder* yellow_finder = new ColorFinder(60, 15, 45, 0, 0.3, 50.0);
yellow_finder->LoadINISettings(ini, "YELLOW");
httpd::yellow_finder = yellow_finder;
ColorFinder* blue_finder = new ColorFinder(225, 15, 45, 0, 0.3, 50.0);
blue_finder->LoadINISettings(ini, "BLUE");
httpd::blue_finder = blue_finder;
httpd::ini = ini;
//////////////////// Framework Initialize ////////////////////////////
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
linux_cm730.SetPortName(U2D_DEV_NAME1);
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Fail to initialize Motion Manager!\n");
return 0;
}
}
Walking::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->AddModule((MotionModule*)Action::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
MotionManager::GetInstance()->AddModule((MotionModule*)Walking::GetInstance());
LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
motion_timer->Start();
/////////////////////////////////////////////////////////////////////
MotionManager::GetInstance()->LoadINISettings(ini);
int firm_ver = 0;
if(cm730.ReadByte(JointData::ID_HEAD_PAN, MX28::P_VERSION, &firm_ver, 0) != CM730::SUCCESS)
{
fprintf(stderr, "Can't read firmware version from Dynamixel ID %d!! \n\n", JointData::ID_HEAD_PAN);
exit(0);
}
if(0 < firm_ver && firm_ver < 27)
{
#ifdef MX28_1024
Action::GetInstance()->LoadFile(MOTION_FILE_PATH);
#else
fprintf(stderr, "MX-28's firmware is not support 4096 resolution!! \n");
fprintf(stderr, "Upgrade MX-28's firmware to version 27(0x1B) or higher.\n\n");
exit(0);
#endif
}
else if(27 <= firm_ver)
{
#ifdef MX28_1024
fprintf(stderr, "MX-28's firmware is not support 1024 resolution!! \n");
fprintf(stderr, "Remove '#define MX28_1024' from 'MX28.h' file and rebuild.\n\n");
exit(0);
#else
Action::GetInstance()->LoadFile((char*)MOTION_FILE_PATH);
#endif
}
else
exit(0); //비젼 처리끝
Action::GetInstance()->m_Joint.SetEnableBody(true, true);
MotionManager::GetInstance()->SetEnable(true);
cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x02|0x04, NULL);
LinuxActionScript::PlayMP3("../../../Data/mp3/Demonstration ready mode.mp3");
Action::GetInstance()->Start(15); //액션에디터의 15번 페이지에 있는 액션을 실행. sitdown 실행
while(Action::GetInstance()->IsRunning()) usleep(8*1000);
// 로봇이 축구를 하는 while 문
while(1) //for(;;) for문의 무한푸프
{
StatusCheck::Check(cm730); //StatusCheck.cpp를 체크
Point2D ball_pos, red_pos, yellow_pos, blue_pos; //point2D x,y : 좌표 변수를 만든다
LinuxCamera::GetInstance()->CaptureFrame();
memcpy(rgb_output->m_ImageData, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageData, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageSize);
// 모드가 레디모드 혹은 비젼
if(StatusCheck::m_cur_mode == READY || StatusCheck::m_cur_mode == VISION)
{
ball_pos = ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
red_pos = red_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
yellow_pos = yellow_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
blue_pos = blue_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame);
// 위에 XXX_pos는 영상처리로 검출 된 좌표를 XXX_pos로 넣는다.
unsigned char r, g, b;
for(int i = 0; i < rgb_output->m_NumberOfPixels; i++)
{
r = 0; g = 0; b = 0;
if(ball_finder->m_result->m_ImageData[i] == 1)
{
r = 255;
g = 128;
b = 0;
}
if(red_finder->m_result->m_ImageData[i] == 1)
{
if(ball_finder->m_result->m_ImageData[i] == 1)
{
r = 0;
g = 255;
b = 0;
}
else
{
r = 255;
g = 0;
b = 0;
}
}
if(yellow_finder->m_result->m_ImageData[i] == 1)
{
if(ball_finder->m_result->m_ImageData[i] == 1)
{
r = 0;
g = 255;
b = 0;
}
else
{
r = 255;
g = 255;
b = 0;
}
}
if(blue_finder->m_result->m_ImageData[i] == 1)
{
if(ball_finder->m_result->m_ImageData[i] == 1)
{
r = 0;
g = 255;
b = 0;
}
else
{
r = 0;
g = 0;
b = 255;
}
}
if(r > 0 || g > 0 || b > 0)
{
rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 0] = r;
rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 1] = g;
rgb_output->m_ImageData[i * rgb_output->m_PixelSize + 2] = b;
}
}
}
// 모드가 사커모드일 때
else if(StatusCheck::m_cur_mode == SOCCER)
{
tracker.Process(ball_finder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_HSVFrame));
// Ball tracker에 관한 내용이 나옴
for(int i = 0; i < rgb_output->m_NumberOfPixels; i++)
{
if(ball_finder->m_result->m_ImageData[i] == 1)
{
rgb_output->m_ImageData[i*rgb_output->m_PixelSize + 0] = 255;
rgb_output->m_ImageData[i*rgb_output->m_PixelSize + 1] = 128;
rgb_output->m_ImageData[i*rgb_output->m_PixelSize + 2] = 0;
}
}
}
streamer->send_image(rgb_output);
if(StatusCheck::m_is_started == 0)
continue;
switch(StatusCheck::m_cur_mode) //switch문 if문 과 달리 정해진 여러가지 경우에 사용 하기 유용하다.
{
case READY:
break;
case SOCCER:
if(Action::GetInstance()->IsRunning() == 0)
{
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
follower.Process(tracker.ball_position); //Ball follwer에 대한 내용
if(follower.KickBall != 0)
{
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Action::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true);
if(follower.KickBall == -1)
{
Action::GetInstance()->Start(12); // RIGHT KICK
fprintf(stderr, "RightKick! \n");
}
else if(follower.KickBall == 1)
{
Action::GetInstance()->Start(13); // LEFT KICK
fprintf(stderr, "LeftKick! \n");
}
}
}
break;
case MOTION:
if(LinuxActionScript::m_is_running == 0)
LinuxActionScript::ScriptStart(SCRIPT_FILE_PATH);
break;
case VISION:
int detected_color = 0;
detected_color |= (red_pos.X == -1)? 0 : VisionMode::RED;
detected_color |= (yellow_pos.X == -1)? 0 : VisionMode::YELLOW;
detected_color |= (blue_pos.X == -1)? 0 : VisionMode::BLUE;
if(Action::GetInstance()->IsRunning() == 0)
VisionMode::Play(detected_color);
break;
}
}
return 0;
}
main()
로봇의 구동을 위해 기본적인 설정을 확인하고 셋팅한다.
main의 로봇 구동 기본셋팅이 끝나고 로봇의 구동이 이루어지는
while(1) 반복문에 왔다.
여기서 반복문은 while()과 for()문이 있다.
while문은 조건동안 실행을 반복적으로 계속한다. 특히 조건이 1일경우 무한히
한다. 왜냐하면 1은 항상 프로그래밍에서 참이기 때문이다.
첫 줄에 StatusCheck::check(cm730) 은 이름 그대로 하드웨어 서브 컨트롤러를 체크하는 것이다.
StatusCheck.cpp
/*
* StatusCheck.cpp
*
* Created on: 2011. 1. 21.
* Author: zerom
*/
// 기존의 라이브러리의 헤더파일 불러오기
#include <stdio.h>
#include <unistd.h>
// 같은 폴더내의 헤어파일 불러오기
#include "StatusCheck.h"
#include "Head.h"
#include "Action.h"
#include "Walking.h"
#include "MotionStatus.h"
#include "MotionManager.h"
#include "LinuxActionScript.h"
using namespace Robot;
int StatusCheck::m_cur_mode = READY; //StatusCheck::m_cur_mode라는 함수는 READY다.
int StatusCheck::m_old_btn = 0; //StatusCheck::btn라는 함수는 0 이다.
int StatusCheck::m_is_started = 0; //StatusCheck::m_is_started는 함수는 0이다.
//statusCheck::Check함수
void StatusCheck::Check(CM730 &cm730)
{
if(MotionStatus::FALLEN != STANDUP && m_cur_mode == SOCCER && m_is_started == 1)
{
Walking::GetInstance()->Stop(); //걸음을 멈춘다
while(Walking::GetInstance()->IsRunning() == 1) usleep(8000); //걸음이 멈출때까지 기다린다.
Action::GetInstance()->m_Joint.SetEnableBody(true, true) ; //액션하기 위해서 다이나믹셀 초기화
if(MotionStatus::FALLEN == FORWARD) // 만약 넘어진 것이 앞으로 넘어지면
Action::GetInstance()->Start(10); // FORWARD GETUP // 액션 10 (엎드려 있는 상태에서 일어난다.)
else if(MotionStatus::FALLEN == BACKWARD) //만약 넘어진 것이 뒤로 넘어지면
Action::GetInstance()->Start(11); // BACKWARD GETUP // 액션 11( 누워있는 상태에서 일어난다.)
while(Action::GetInstance()->IsRunning() == 1) usleep(8000); // 액션이 끝날때 까지 기다린다.
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); //머리에 있는 다이나믹셀을 사용할 것이다.
Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true); // 걷기위해서 머리를 제외 다이나믹셀 사용
}
if(m_old_btn == MotionStatus::BUTTON)
return;
m_old_btn = MotionStatus::BUTTON;
if(m_old_btn & BTN_MODE) //m_old btn과 BTN MODE의 연산(&,|) 예를 들어 &은 and 즉 110과 101은 백의 자리의 수만 같기 때문에 연산하면 100이다. | 은 또는 이기때문에 110과 101은 111이다.
{
fprintf(stderr, "Mode button pressed.. \n"); //fprintf는 printf와 달리 모니터(검은창 콘솔)에 출력하는 것이 아니라
파일에 출력하는 것이다. 예를 들어 메모장으로 지정하면 메모장에 출력하는 것이다.
if(m_is_started == 1)
{
m_is_started = 0;
m_cur_mode = READY; //레디모드
LinuxActionScript::m_stop = 1;
Walking::GetInstance()->Stop(); //걸음을 멈추고
Action::GetInstance()->m_Joint.SetEnableBody(true, true); //액션을 위해 초기화 다이나믹셀
while(Action::GetInstance()->Start(15) == false) usleep(8000); //액션 15번이 실행이 안되면 정지
while(Action::GetInstance()->IsRunning() == true) usleep(8000); //실행이라면 정지
}
else
{
m_cur_mode++;
if(m_cur_mode >= MAX_MODE) m_cur_mode = READY;
//m_cur mode가 5보다 크거나 같으면 레디로 바꾼다.
}
MotionManager::GetInstance()->SetEnable(false);
usleep(10000);
if(m_cur_mode == READY) //m_cur_mode 가 레디
{
cm730.WriteByte(CM730::P_LED_PANNEL, 0x01|0x02|0x04, NULL); //인터페이스보드 LED1.2.4을 킨다.
LinuxActionScript::PlayMP3("../../../Data/mp3/Demonstration ready mode.mp3"); // 지정된 MP3 파일을 실행
}
else if(m_cur_mode == SOCCER) //m_cur_mode 가 사커
{
cm730.WriteByte(CM730::P_LED_PANNEL, 0x01, NULL); //인터페이스보드 LED1을 킨다.
LinuxActionScript::PlayMP3("../../../Data/mp3/Autonomous soccer mode.mp3"); // 지정된 MP3 파일을 실행
}
else if(m_cur_mode == MOTION) //m_cur_mode 가 모션
{
cm730.WriteByte(CM730::P_LED_PANNEL, 0x02, NULL); //인터페이스보드 LED2을 킨다
LinuxActionScript::PlayMP3("../../../Data/mp3/Interactive motion mode.mp3"); // 지정된 MP3 파일을 실행
}
else if(m_cur_mode == VISION) //m_cur_mode 가 비젼
{
cm730.WriteByte(CM730::P_LED_PANNEL, 0x04, NULL); //인터페이스보드 LED4을 킨다
LinuxActionScript::PlayMP3("../../../Data/mp3/Vision processing mode.mp3"); // 지정된 MP3 파일을 실행
}
}
if(m_old_btn & BTN_START) //m_old_btn 와 BTN_START &연산
{
if(m_is_started == 0)
{
fprintf(stderr, "Start button pressed.. & started is false.. \n");
if(m_cur_mode == SOCCER)
{
MotionManager::GetInstance()->Reinitialize();
MotionManager::GetInstance()->SetEnable(true);
m_is_started = 1;
//모드를 선택하고 실행했을때 실행은 하지만 m_is_started=1 그래서 다시 모드 설정 가능
LinuxActionScript::PlayMP3("../../../Data/mp3/Start soccer demonstration.mp3");
Action::GetInstance()->m_Joint.SetEnableBody(true, true); 액션을 위해 다이나믹셀 초기화
Action::GetInstance()->Start(9); 액션 9번 실행
while(Action::GetInstance()->IsRunning() == true) usleep(8000); 액션이 하는 동안 소스 진행 정지
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true); //머리와 관련된 다이나믹셀 초기화
Walking::GetInstance()->m_Joint.SetEnableBodyWithoutHead(true, true); //머리 외 관련된 다이나믹셀 초기화
MotionManager::GetInstance()->ResetGyroCalibration();
// 걷는데 필요한 제어를 위하여 자이로 센서 초기화
while(1)
{
if(MotionManager::GetInstance()->GetCalibrationStatus() == 1)
{
LinuxActionScript::PlayMP3("../../../Data/mp3/Sensor calibration complete.mp3");
break; //자이로선세 초기값이 참이라면 while문 탈출
}
else if(MotionManager::GetInstance()->GetCalibrationStatus() == -1)
{
LinuxActionScript::PlayMP3Wait("../../../Data/mp3/Sensor calibration fail.mp3");
MotionManager::GetInstance()->ResetGyroCalibration(); //자이로센서 초기값이 거짓이라면 무한루프
}
usleep(8000);
}
}
else if(m_cur_mode == MOTION)
{
MotionManager::GetInstance()->Reinitialize();
MotionManager::GetInstance()->SetEnable(true);
m_is_started = 1;
//모드를 선택하고 실행했을때 실행은 하지만 m_is_started=1 그래서 다시 모드 설정 가능
LinuxActionScript::PlayMP3("../../../Data/mp3/Start motion demonstration.mp3");
// Joint Enable..
Action::GetInstance()->m_Joint.SetEnableBody(true, true);
Action::GetInstance()->Start(1);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
}
else if(m_cur_mode == VISION)
{
MotionManager::GetInstance()->Reinitialize();
MotionManager::GetInstance()->SetEnable(true);
m_is_started = 1;
//모드를 선택하고 실행했을때 실행은 하지만 m_is_started=1 그래서 다시 모드 설정 가능
LinuxActionScript::PlayMP3("../../../Data/mp3/Start vision processing demonstration.mp3");
// Joint Enable...
Action::GetInstance()->m_Joint.SetEnableBody(true, true);
Action::GetInstance()->Start(1);
while(Action::GetInstance()->IsRunning() == true) usleep(8000);
}
}
else
{
fprintf(stderr, "Start button pressed.. & started is true.. \n");
}
}
}
BallTracker.cpp
*
* Author: ROBOTIS
*
*/
#include <math.h>
#include "Head.h"
#include "Camera.h"
#include "ImgProcess.h"
#include "BallTracker.h"
using namespace Robot;
BallTracker::BallTracker() :
ball_position(Point2D(-1.0, -1.0))
{
NoBallCount = 0;
}
BallTracker::~BallTracker()
{
}
void BallTracker::Process(Point2D pos)
{
if(pos.X < 0 || pos.Y < 0)
{
ball_position.X = -1;
ball_position.Y = -1;
if(NoBallCount < NoBallMaxCount)
{
Head::GetInstance()->MoveTracking();
NoBallCount++;
}
else
Head::GetInstance()->InitTracking();
}
else
{
NoBallCount = 0;
Point2D center = Point2D(Camera::WIDTH/2, Camera::HEIGHT/2);
Point2D offset = pos - center;
offset *= -1; // Inverse X-axis, Y-axis
offset.X *= (Camera::VIEW_H_ANGLE / (double)Camera::WIDTH); // pixel per angle
offset.Y *= (Camera::VIEW_V_ANGLE / (double)Camera::HEIGHT); // pixel per angle
ball_position = offset;
Head::GetInstance()->MoveTracking(ball_position);
}
}
BallFollower.cpp
*
* Author: ROBOTIS
*
*/
#include <stdio.h>
#include "ImgProcess.h"
#include "MX28.h"
#include "Head.h"
#include "Action.h"
#include "Walking.h"
#include "BallFollower.h"
#include "MotionStatus.h"
using namespace Robot;
BallFollower::BallFollower()
{
m_NoBallMaxCount = 10; //볼이 없는 경우 최대 확인 하는 카운트값
m_NoBallCount = m_NoBallMaxCount; //볼이 없는 경우 증하는 카운트값
m_KickBallMaxCount = 10; //공을 찰 수 있는 경우 최대 확인하는 카운트 값
m_KickBallCount = 0; //공을 찰 수 있는 경우 증가하는 카운트 값
m_KickTopAngle = -5.0; //공을 찰 수 있는 최대 각도 값
m_KickRightAngle = -30.0; // 공을 찰 수 있는 오른쪽 최대 각도 값
m_KickLeftAngle = 30.0; //공을 찰 수 있는 왼쪽 최대 각도 값
m_FollowMaxFBStep = 30.0; // 앞뒤로 걸을 수 있는 최대 보폭
m_FollowMinFBStep = 5.0; // 앞뒤로 걸을 수 있는 최소 보폭
m_FollowMaxRLTurn = 35.0; // 좌우로 회전할 수 있는 최대 보폭
m_FitFBStep = 3.0; //공이 가까워 진 상태에서 조금씪 움직이도록 하는 보폭값
m_FitMaxRLTurn = 35.0; //공이 가까어진 상태에서 조금씩 돌 도록 하는 보폭값
m_UnitFBStep = 0.3; //앞뒤로 걸을 때 증감하는 단위 보폭
m_UnitRLTurn = 1.0; //좌우로 볼 때 증감하는 단위 보폭
m_GoalFBStep = 0; // 목표하는 앞뒤 목표
m_GoalRLTurn = 0; // 목표하는 좌우 목표
m_FBStep = 0; //현재 앞뒤 보폭
m_RLTurn = 0; //현재 좌우 보폭
DEBUG_PRINT = false;
KickBall = 0;
}
BallFollower::~BallFollower()
{
}
void BallFollower::Process(Point2D ball_pos)
{
if(DEBUG_PRINT == true)
fprintf(stderr, "\r \r");
if(ball_pos.X == -1.0 || ball_pos.Y == -1.0)
{
KickBall = 0;
if(m_NoBallCount > m_NoBallMaxCount)
{
// can not find a ball
m_GoalFBStep = 0;
m_GoalRLTurn = 0;
Head::GetInstance()->MoveToHome();
if(DEBUG_PRINT == true)
fprintf(stderr, "[NO BALL]");
}
else
{
m_NoBallCount++;
if(DEBUG_PRINT == true)
fprintf(stderr, "[NO BALL COUNTING(%d/%d)]", m_NoBallCount, m_NoBallMaxCount);
}
}
else
{
m_NoBallCount = 0;
double pan = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_PAN);
double pan_range = Head::GetInstance()->GetLeftLimitAngle();
double pan_percent = pan / pan_range;
double tilt = MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_TILT);
double tilt_min = Head::GetInstance()->GetBottomLimitAngle();
double tilt_range = Head::GetInstance()->GetTopLimitAngle() - tilt_min;
double tilt_percent = (tilt - tilt_min) / tilt_range;
if(tilt_percent < 0)
tilt_percent = -tilt_percent;
// 공을 발견하고 왼발과 오른발로 공을 찰 수 있는 전방 범위에 있을 경우
if(pan > m_KickRightAngle && pan < m_KickLeftAngle)
{
if(tilt <= (tilt_min + MX28::RATIO_VALUE2ANGLE))
{
if(ball_pos.Y < m_KickTopAngle)
{
m_GoalFBStep = 0;
m_GoalRLTurn = 0;
if(m_KickBallCount >= m_KickBallMaxCount)
{
m_FBStep = 0;
m_RLTurn = 0;
if(DEBUG_PRINT == true)
fprintf(stderr, "[KICK]");
if(pan > 0)
{
KickBall = 1; // Left
if(DEBUG_PRINT == true)
fprintf(stderr, " Left");
}
else
{
KickBall = -1; // Right
if(DEBUG_PRINT == true)
fprintf(stderr, " Right");
}
}
else
{
KickBall = 0;
if(DEBUG_PRINT == true)
fprintf(stderr, "[KICK COUNTING(%d/%d)]", m_KickBallCount, m_KickBallMaxCount);
}
}
//로봇의 발이 찰 경우 공에 닿지 않는 거리 상에 위치 할 때
else
{
m_KickBallCount = 0;
KickBall = 0;
m_GoalFBStep = m_FitFBStep;
m_GoalRLTurn = m_FitMaxRLTurn * pan_percent //자전을 통해 공을 찰 수 있는 거리와 범위에 들게 한다.
if(DEBUG_PRINT == true)
fprintf(stderr, "[FIT(P:%.2f T:%.2f>%.2f)]", pan, ball_pos.Y, m_KickTopAngle);
}
}
else
{
m_KickBallCount = 0;
KickBall = 0;
m_GoalFBStep = m_FollowMaxFBStep * tilt_percent;
if(m_GoalFBStep < m_FollowMinFBStep)
m_GoalFBStep = m_FollowMinFBStep;
m_GoalRLTurn = m_FollowMaxRLTurn * pan_percent;
if(DEBUG_PRINT == true)
fprintf(stderr, "[FOLLOW(P:%.2f T:%.2f>%.2f]", pan, tilt, tilt_min);
}
}
else
{
m_KickBallCount = 0;
KickBall = 0;
m_GoalFBStep = 0;
m_GoalRLTurn = m_FollowMaxRLTurn * pan_percent;
if(DEBUG_PRINT == true)
fprintf(stderr, "[FOLLOW(P:%.2f T:%.2f>%.2f]", pan, tilt, tilt_min);
}
}
if(m_GoalFBStep == 0 && m_GoalRLTurn == 0 && m_FBStep == 0 && m_RLTurn == 0)
{
if(Walking::GetInstance()->IsRunning() == true)
Walking::GetInstance()->Stop();
else
{
if(m_KickBallCount < m_KickBallMaxCount)
m_KickBallCount++;
}
if(DEBUG_PRINT == true)
fprintf(stderr, " STOP");
}
else
{
if(DEBUG_PRINT == true)
fprintf(stderr, " START");
if(Walking::GetInstance()->IsRunning() == false)
{
m_FBStep = 0;
m_RLTurn = 0;
m_KickBallCount = 0;
KickBall = 0;
Walking::GetInstance()->X_MOVE_AMPLITUDE = m_FBStep;
Walking::GetInstance()->A_MOVE_AMPLITUDE = m_RLTurn;
Walking::GetInstance()->Start();
}
else
{
if(m_FBStep < m_GoalFBStep)
m_FBStep += m_UnitFBStep;
else if(m_FBStep > m_GoalFBStep)
m_FBStep = m_GoalFBStep;
Walking::GetInstance()->X_MOVE_AMPLITUDE = m_FBStep;
if(m_RLTurn < m_GoalRLTurn)
m_RLTurn += m_UnitRLTurn;
else if(m_RLTurn > m_GoalRLTurn)
m_RLTurn -= m_UnitRLTurn;
Walking::GetInstance()->A_MOVE_AMPLITUDE = m_RLTurn;
if(DEBUG_PRINT == true)
fprintf(stderr, " (FB:%.1f RL:%.1f)", m_FBStep, m_RLTurn);
}
}
}
Report abuse