[40호]장애물 방지 드론
2016 ICT 융합 프로젝트 공모전 참가상
장애물 방지 드론
글 | 단국대학교 김건희, 김혜원
심사평
JK전자 드론을 이용한 무인택배, 사람을 쫒아다니면서 촬영해 주는 드론 카메라, 위험지역탐사, 화재 진화 등 최근에 드론을 이용한 여러가지 서비스에 대한 아이디어들이 봇물처럼 나오고 있다. 이러한 무인으로 동작하는 서비스가 가능하기 위해서 가장 기초적으로 필요한 부분이 장애물을 감지하고 회피하는 능력일 것이다. 본 작품에서는 기초적인 방법으로 적외선 센서를 이용해서 아주 근접한 물체에 대한 판별만 하고 있지만, 이 부분을 조금더 수정한다면 다양한 분야에 이용이 가능할 것이다. 또한 최신의 32비트 프로세서인 Cortex-M4를 이용하였고 직접 PID 제어 코드를 모두 구현한 것으로 판단하건데 본 작품 구현을 위해서 많은 노력을 기울인 것이 엿보인다.
뉴티씨 드론 관련 내용 중에서도 장애물을 회피하는 내용을 다루고 있는 데, 실질적으로 충돌을 막아내는 효과가 있는 작품이다. 실제 드론에 적용한다면 좋을 것 같다. 자체로는 상품이 되기는 어렵지만, 이런 아이템을 하나하나 쌓아 나간다면 좋은 작품이 될 것으로 생각한다. 실용성과 창의성 및 작품 완성면에서 좋은 점수를 받았다.
칩센 주제는 식상하다, 하지만 많은 학생들이 관심을 가지는 분야에 자세한 설명이 제공되는 부분에 높은 평가를 한다.
위드로봇 트라이콥터 형식의 기체를 자체적으로 제작한 점은 훌륭하나, 작품의 특성인 장애물을 회피하는 기동을 보이는데 해당 드론이 장점이 있다는 점이 잘 부각되지 않았습니다. 또한 장애물을 회피하기 위해 사용한 센서가 적외선인데 이게 실외에서 잘 동작할 수 있을지 의문입니다. 이러한 설명이 부족한 점이 아쉬웠습니다.
1. 작품개요
제작동기
요즘 무인항공기인 드론이 대세입니다. 하지만 사건, 사고가 많이 일어나고 있습니다. 이탈리아 밀나노의 명소인 두오모 성당에 한국인들이 조종하던 드론이 충돌하는 사고가 일어다는 뉴스도 접할 수 있었습니다.
드론은 사회에 급속히 보급되고 있지만 그만큼 드론과 관련한 충돌 사고도 자주 발생하고 있습니다. 그래서 충돌사고를 줄이는데 도움이 되고자 장애물 피하는 드론을 만들게 되었습니다.
제작과정
stm32f4 보드에 모터, 변속기, 센서 등을 모두 각각 구매하여 자체적으로 제작하였습니다. 처음 드론을 선정할 때 프로펠러 수가 적을수록 기동성이 빠르기 때문에 프로펠러가 3개인 트라이 콥터로 선정하고 제작하였습니다. 트라이콥터를 제작하여 호버링(헬리콥터가 공중에 정지해 있는 상태) 테스트 후 적외선 센서를 통하여 장애물과 가까워지면 드론이 자동적으로 피하게끔 제작하였습니다.
작품내용
트라이콥터를 설명드리면, 일반적으로 쿼드콥터와 달리 3개의 BLDC모터와 1개의 서보모터를 가지고 비행하는 것이 특징입니다. 3개의 BLDC모터로 IMU 센서값에서 얻는 Roll축 값과 Pitch축 값을 가지고 제어하고 나머지 서보모터로 Yaw축 값을 받아 Yaw축을 제어하게 됩니다.
하드웨어를 만들고 소스 구현을 한 다음 적외선 센서를 부착하기 전에 조종기로 드론이 호버링이 될 수 있도록 제작하였습니다. 조종기로 호버링이 된 후 적외선 센서를 9개를 달아 벽이나 장애물에 가까이 다가갔을 때 장애물 반대 방향으로 제어하게끔 소스를 구현하였습니다. 고도도 ms5611센서(기압센서)에서 추출한 값으로 일정 고도를 유지하게 제작하였습니다.
제작 결과
조종기를 이용하여 현재 호버링은 쉽게 되고 있습니다. 이후에 적외선 센서를 통하여 장애물이 가까이 갔을 때 센서 값을 확인하여 보았습니다. 50cm부터 센서값이 추출되어 50cm 이상 거리가 떨어지면 인식을 하지 못하지만 50cm 이내로 들어오면 드론이 장애물의 반대로 움직이려는 현상을 보게 되었습니다. 따라서 현재 시중에 판매할 정도로 완벽하지는 않지만 어느 정도 장애물에 가까이 갔을 때 장애물을 인식하여 반대로 움직이려는 현상을 확인하였고 제가 원하는 제어가 가능하다는 것을 확인하였습니다.
3. 작품 설명
3.1. 주요 동작 및 특징
트라이콥터에는 3개의 bldc 모터와 1개의 서보모터가 들어가 있습니다. 먼저 드론을 전체적으로 제어할 stm32f4 보드가 있고, 조종기와 통신하기 위한 블루투스 모듈, 고도제어를 위한 기압센서, 드론 제어를 위해 관성 측정장치인 imu가 들어가 있습니다, 또한 장애물을 피하기 위한 9개의 적외선 센서가 들어가 있습니다.
먼저 블루투스 통신기는 JMOD-BT-1 모듈을 이용하였습니다. 조종기와 통신하기 위하여 sci 통신을 하였고 tx와 rx를 연결하고 조종기를 움직이게 되면 움직임에 따라 값을 보내게 되고, 제어에 맞는 값이 블루투스 모듈을 통하여 stm32f4 보드에 값을 전달하며, 이 값에 따라 조종을 하게 됩니다.
둘째로 고도제어는 ms5611이라는 기압센서를 통하여 제어하게 됩니다. 다만 일정 고도 이상일 때 값이 일정하게 나오게 되어, 그때부터는 조종기를 통하여 제어하지 않고 드론 스스로 일정 고도를 유지하게끔 모터제어를 하였습니다.
또한 관성 측정장치인 imu(EBIMU-9DOFV2)를 이용하였습니다. imu에는 자이로 센서, 가속도 센서, 지자기 센서가 들어있어 roll, pitch, yaw축을 제어하게 됩니다. 공중에서 드론이 기울어지게 되면 기울어지는 반대방향으로 움직이게끔 모터제어를 하기위해 imu를 사용하게 되었습니다.
마지막으로 적외선 센서가 들어가 있습니다. 한 개의 프로펠러에 3개의 적외선 센서를 달았습니다. 그래서 총 9개의 적외선 센서가 들어있습니다. 처음에는 초음파 센서로 i2c 통신을 시도해 보았으나, 3개의 초음파 센서를 다니까 반응 속도가 느려지는 현상이 발생하게 되었습니다. 그래서 초음파 센서 대신 9개의 적외선 센서를 부착하였습니다. 테스트 해보니 적외선 센서는 장애물 50cm 이내에 들어왔을 때 센서값이 증가하게 됩니다. 이 증가하는 값을 이용하여 모터를 제어를 하게 됩니다. 이에 장애물을 피할 수 있게 소스를 구현하였습니다.
모터 제어는 bldc 제어를 하기 위해서 하비킹의 30A 변속기를 사용하였습니다. 변속기는 일반적으로 모터 드라이브라고 생각하면 쉽게 이해가 됩니다. 3개의 bldc모터에 각각 변속기가 붙어있어 모터를 제어하게 됩니다. 그리고 변속기에 pwm 파형을 주면 변속기를 통하여 모터를 제어하게 됩니다.
서보모터는 imu값에 따라 stm32f4에서 pwm파 형이 나오게 됩니다. 이 파형을 서보모터의 pwm핀에 물리면 서보모터는 파형에 따라 각도 회전을 하기 때문에 이를 이용하여 서보모터를 제어하게 됩니다.
bldc 모터와 서보모터를 제어하기 위해서 pid 제어를 하였습니다. pid 제어는 비례, 적분, 미분을 합쳐 목표값과의 차이를 이용하여 원하는 제어값을 얻는 제어법 중 하나입니다.
3.2. 전체 시스템 구성
3.3. 개발환경(개발 언어, tool, 사용시스템 등)
stm32f407 보드를 사용하기 위해서는 많은 프로그램이 있습니다. 저는 그중에서 IAR Embedded Workbench를 이용하였습니다. IAR은 전세계적으로 임베디드 시스템 개발자들이 많이 사용하는 C/C++ Compiler 및 Debugging Tool 입니다. 그리고 Tera Term이라는 모니터를 이용하여 PC상에서 m4 보드에 접근할 수 있었습니다. 또한 드론을 제작하면서 printf나 에러 처리 등을 확인할 수 있었습니다.
4. 단계별 제작 과정
4.1. 하드웨어 부품 조립
4.2. 소프트웨어 소스 구현
4.2.1. Tera Term과 stm32f4보드 터미널 연결
PB6▶tx, PB7▶rx핀을 연결후 시리얼 포트인 rx232와 연결을 하였습니다. 연결시 vcc, ground, tx, rx핀을 연결 후 sci 인터럽트를 통해 보드에서 printf();값이 나오는지 확인하였습니다.
4.2.2. timer interrupt
timer interrupt가 일정 시간마다 되는지 확인하였습니다. 왜냐하면 bldc 모터나 서보모터를 제어하기 위해서는 일정 시간만큼 계속 제어를 해주어야 하기 때문에 timer interrupt가 있어야 합니다.
4.2.3. pwm
모터 제어를 하기 위해서는 pwm 파형이 나오는지를 확인해야 됩니다. pwm이란 ‘펄스 폭 변조’, 펄스 폭을 원하는 값에 따라 조절할 수 있습니다. 이 조절된 펄스를 가지고 모터를 제어하게 됩니다. 서보모터는 펄스 폭에 따라 회전 각도가 정해져 있어 회전 각도에 따라 펄스 폭 조절을 해야 됩니다.
4.2.4. I2C를 이용하여 고도센서 통신
통신 방법에는 대표적으로 SCI, CAN, I2C, SPI 4가지 통신 방법이 있습니다. 그 중에 기압센서를 제어하기 위해서는 I2C통신이 필요합니다. 그래서 I2C통신이 되는지 확인하였습니다.
4.2.5. ADC
적외선 센서 값을 받기위해서는 ADC가 필요합니다. ADC는 아날로그 신호를 디지털 신호로 바꿔주는 변환기입니다. 적외선 센서 값에 따라 값이 나오는지 확인했습니다. ADC를 통하여 센서 값을 받아도 백열등이나 밖에서 다른 환경일 때 적외선 센서가 값을 인식하는 경우가 발생하게 됩니다. 그래서 하드웨어적으로는 HPF(HIGH PASS FILTER)로 땜을 하여 필터링을 하고 소프트웨어적으로 IIR필터를 설계하였습니다.
IIR필터란 디지털 필터로 적외선 센서인 경우 노이즈를 많이 타기 때문에 노이즈 제거 또는 감소를 시키기 위해 필터를 설계하였습니다.
사진은 센서값을 printf하여 확인한 값입니다.
밑의 사진은 iir필터 식입니다.
4.2.6. IMU 센서값 확인
imu 센서에서 sci 통신으로 값을 받게되면 상수 값으로 넘어오는게 아니라 아스키 코드값으로 문자열이 넘어오게 됩니다. 3개의 값이 *123, 456, 789\n처럼 값이 하나의 문자열로 넘어오기 때문에 하나의 문자열을 3개의 변수로 잡아 모터 제어에 필요하게 3개의 변수 값을 확인하였습니다. 밑의 사진은 3개의 변수 중 pitch값을 확인하였습니다.
4.2.7. IMU 센서에서 받은 값과 기압센서에서 받은 값을 이용하여 모터 pid 제어소스 구현을 했습니다.
4.3. 실제 기동 테스트
pid 제어소스를 구현하여 실제 기동 테스트를 해보았습니다.
4.3.1. 시소테스트
pid 제어 소스를 구현해도 pid의 상수값을 맞춰주어야 합니다. P값의 경우에는 한쪽으로 기울어 졌을 때 받아치려는 힘이 느껴질 때 그때의 P값으로 지정하고 D값을 맞추었습니다. D값을 맞추다보면 한쪽으로 기울었을 때 평형을 유지하려고 합니다. 그때가 시소테스트가 완벽하게 된 것입니다.
4.3.2. 실내 테스트
처음에는 서보모터를 제어하지 않고 프로펠러 3개를 이용하여 드론을 날려보니 시소테스트를 제대로 하면 프로펠러가 역방향 2개와 정방향 1개이기 때문에 역방향으로 회전을 하면서 올랐다가 내려오는 모습을 보게 됩니다. 이게 제대로 된 후 imu값의 회전축인 yaw축 값을 받아 서보모터를 제어하여 회전을 안하게 했습니다.
4.3.3. 실외 테스트
4.4. 장애물 인식후 제어 테스트
장애물을 인식하기 위해서 트라이콥터 프로펠러 부분에 3개의 적외선 센서를 달아 총 9개의 적외선 센서로 근처에 장애물이 있는지 감지할 수 있도록 하였습니다. 다음 사진은 한 개의 프로펠러에 3개의 적외선 센서를 달아놓은 상태입니다.
아래는 벽에 가까이 다가갈때의 모습입니다.
벽 반대편으로 움직이려 스스로 제어하려 하는 모습을 볼 수 있습니다.
5. 기타(회로도, 소스코드, 참고문헌등)
5.1. 적외선 센서 I2C 필터 소스
// SENSOR VARIABLE FILTERING
g_sen[ gUint16SensorSelect ].LPFOutDataYet = g_sen[ gUint16SensorSelect ].LPFOutData;
g_sen[ gUint16SensorSelect ].LPFOutData =
mpy( SENSOR_Kb, 30, g_sen[ gUint16SensorSelect ].LPFInData
+ QUP( g_sen[ gUint16SensorSelect ].Uint16Value, 17 ), 17 )
- mpy( SENSOR_Ka, 30, g_sen[ gUint16SensorSelect ].LPFOutData, 17 );
g_sen[ gUint16SensorSelect ].LPFInData = QUP( g_sen[ gUint16SensorSelect ].Uint16Value, 17 );
5.2. IMU값을 3개의 변수로 바꾸는 소스
void IMUchar(void)
{
if(sci_buf == ‘\n’)
{
g_int32_sciflag = OFF;
g_int32_strcnt = 0;
g_int32_IMUnum = 0;
g_f32_imu_flag = 1;
}
if(g_int32_sciflag == ON)
{
if(g_int32_IMUnum == 0)
{
if(sci_buf == ‘,’)
{
g_str_roll[g_int32_strcnt++] = ‘’;
g_int32_IMUnum++;
g_int32_strcnt = 0;
}
else
{
g_str_roll[g_int32_strcnt++] = sci_buf;
}
}
else if(g_int32_IMUnum == 1)
{
if(sci_buf == ‘,’)
{
g_str_pitch[g_int32_strcnt++] = ‘’;
g_int32_IMUnum++;
g_int32_strcnt = 0;
}
else
{
g_str_pitch[g_int32_strcnt++] = sci_buf;
}
}
else if(g_int32_IMUnum == 2)
{
if(sci_buf == 13)//imu 맨마지막 숫자는 13이다.
{
g_str_yaw[g_int32_strcnt++] = ‘’;
g_int32_strcnt = 0;
}
else
{
g_str_yaw[g_int32_strcnt++] = sci_buf;
}
}
}
if(sci_buf == ‘*’)
{
g_int32_sciflag = ON;
}
}
void IMUdecimal(void)
{
int16_t length_roll = 0;
int16_t length_pitch = 0;
int16_t length_yaw = 0;
length_roll = strlen((char *)g_str_roll);
length_pitch = strlen((char *)g_str_pitch);
length_yaw = strlen((char *)g_str_yaw);
//roll
if(length_roll == 4)
{
sci_buf2roll[0]=g_str_roll[0]-48;
sci_buf2roll[2]=(g_str_roll[2]-48)*0.1;
sci_buf2roll[3]=(g_str_roll[3]-48)*0.01;
g_fp32roll = sci_buf2roll[0]+sci_buf2roll[2]+sci_buf2roll[3];
}
else if(length_roll == 5)
{
if(g_str_roll[0]==’-’)
{
sci_buf2roll[1]=g_str_roll[1]-48;
sci_buf2roll[3]=(g_str_roll[3]-48)*0.1;
sci_buf2roll[4]=(g_str_roll[4]-48)*0.01;
g_fp32roll = (sci_buf2roll[1]+sci_buf2roll[3]+sci_buf2roll[4])*(-1);
}
else
{
sci_buf2roll[0]=(g_str_roll[0]-48)*10;
sci_buf2roll[1]=(g_str_roll[1]-48);
sci_buf2roll[3]=(g_str_roll[3]-48)*0.1;
sci_buf2roll[3]=(g_str_roll[4]-48)*0.01;
g_fp32roll = sci_buf2roll[0]+sci_buf2roll[1]+sci_buf2roll
[3]+sci_buf2roll[4];
}
}
else if(length_roll == 6)
{
if(g_str_roll[0]==’-’)
{
sci_buf2roll[1]=(g_str_roll[1]-48)*10;
sci_buf2roll[2]=(g_str_roll[2]-48);
sci_buf2roll[4]=(g_str_roll[4]-48)*0.1;
sci_buf2roll[5]=(g_str_roll[5]-48)*0.01;
g_fp32roll = (sci_buf2roll[1]+sci_buf2roll[2]+sci_buf2roll
[4]+sci_buf2roll[5])*(-1);
}
else
{
sci_buf2roll[0]=(g_str_roll[0]-48)*100;
sci_buf2roll[1]=(g_str_roll[1]-48)*10;
sci_buf2roll[2]=(g_str_roll[2]-48);
sci_buf2roll[4]=(g_str_roll[4]-48)*0.1;
sci_buf2roll[5]=(g_str_roll[5]-48)*0.01;
g_fp32roll = sci_buf2roll[0]+sci_buf2roll[1]+sci_buf2roll[2]+sci_
buf2roll[4]+sci_buf2roll[5];
}
}
else if(length_roll == 7)
{
sci_buf2roll[1]=(g_str_roll[1]-48)*100;
sci_buf2roll[2]=(g_str_roll[2]-48)*10;
sci_buf2roll[3]=(g_str_roll[3]-48);
sci_buf2roll[5]=(g_str_roll[5]-48)*0.1;
g_fp32roll=(sci_buf2roll[1]+sci_buf2roll[2]+sci_buf2roll[3]
+sci_buf2roll[5])*(-1);
}
//pitch
if(length_pitch == 4)
{
sci_buf2pitch[0]=g_str_pitch[0]-48;
sci_buf2pitch[2]=(g_str_pitch[2]-48)*0.1;
sci_buf2pitch[3]=(g_str_pitch[3]-48)*0.01;
g_fp32pitch = sci_buf2pitch[0]+sci_buf2pitch[2]+sci_buf2pitch[3];
}
else if(length_pitch == 5)
{
if(g_str_pitch[0]==’-’)
{
sci_buf2pitch[1]=g_str_pitch[1]-48;
sci_buf2pitch[3]=(g_str_pitch[3]-48)*0.1;
sci_buf2pitch[4]=(g_str_pitch[4]-48)*0.01;
g_fp32pitch = (sci_buf2pitch[1]+sci_buf2pitch[3]
+sci_buf2pitch[4])*(-1);
}
else
{
sci_buf2pitch[0]=(g_str_pitch[0]-48)*10;
sci_buf2pitch[1]=(g_str_pitch[1]-48);
sci_buf2pitch[3]=(g_str_pitch[3]-48)*0.1;
sci_buf2pitch[3]=(g_str_pitch[4]-48)*0.01;
g_fp32pitch = sci_buf2pitch[0]+sci_buf2pitch[1]+sci_buf2pitch
[3]+sci_buf2pitch[4];
}
}
else if(length_pitch == 6)
{
if(g_str_pitch[0]==’-’)
{
sci_buf2pitch[1]=(g_str_pitch[1]-48)*10;
sci_buf2pitch[2]=(g_str_pitch[2]-48);
sci_buf2pitch[4]=(g_str_pitch[4]-48)*0.1;
sci_buf2pitch[5]=(g_str_pitch[5]-48)*0.01;
g_fp32pitch = (sci_buf2pitch[1]+sci_buf2pitch[2]
+sci_buf2pitch[4]+sci_buf2pitch[5])*(-1);
}
else
{
sci_buf2pitch[0]=(g_str_pitch[0]-48)*100;
sci_buf2pitch[1]=(g_str_pitch[1]-48)*10;
sci_buf2pitch[2]=(g_str_pitch[2]-48);
sci_buf2pitch[4]=(g_str_pitch[4]-48)*0.1;
sci_buf2pitch[5]=(g_str_pitch[5]-48)*0.01;
g_fp32pitch = sci_buf2pitch[0]+sci_buf2pitch[1]+sci_buf2pitch[2]
+sci_buf2pitch[4]+sci_buf2pitch[5];
}
}
else if(length_pitch == 7)
{
sci_buf2pitch[1]=(g_str_pitch[1]-48)*100;
sci_buf2pitch[2]=(g_str_pitch[2]-48)*10;
sci_buf2pitch[3]=(g_str_pitch[3]-48);
sci_buf2pitch[5]=(g_str_pitch[5]-48)*0.1;
g_fp32pitch=(sci_buf2pitch[1]+sci_buf2pitch[2]+sci_buf2pitch[3]+
sci_buf2pitch[5])*(-1);
}
//yaw
if(length_yaw == 4)
{
sci_buf2yaw[0]=g_str_yaw[0]-48;
sci_buf2yaw[2]=(g_str_yaw[2]-48)*0.1;
sci_buf2yaw[3]=(g_str_yaw[3]-48)*0.01;
g_fp32yaw = sci_buf2yaw[0]+sci_buf2yaw[2]+sci_buf2yaw[3];
}
else if(length_yaw == 5)
{
if(g_str_yaw[0]==’-’)
{
sci_buf2yaw[1]=g_str_yaw[1]-48;
sci_buf2yaw[3]=(g_str_yaw[3]-48)*0.1;
sci_buf2yaw[4]=(g_str_yaw[4]-48)*0.01;
g_fp32yaw = (sci_buf2yaw[1]+sci_buf2yaw[3]+sci_buf2yaw[4])*(-1);
}
else
{
sci_buf2yaw[0]=(g_str_yaw[0]-48)*10;
sci_buf2yaw[1]=(g_str_yaw[1]-48);
sci_buf2yaw[3]=(g_str_yaw[3]-48)*0.1;
sci_buf2yaw[3]=(g_str_yaw[4]-48)*0.01;
g_fp32yaw = sci_buf2yaw[0]+sci_buf2yaw[1]+sci_buf2yaw[3]+sci_buf2yaw[4];
}
}
else if(length_yaw == 6)
{
if(g_str_yaw[0]==’-’)
{
sci_buf2yaw[1]=(g_str_yaw[1]-48)*10;
sci_buf2yaw[2]=(g_str_yaw[2]-48);
sci_buf2yaw[4]=(g_str_yaw[4]-48)*0.1;
sci_buf2yaw[5]=(g_str_yaw[5]-48)*0.01;
g_fp32yaw = (sci_buf2yaw[1]+sci_buf2yaw[2]+sci_buf2yaw[4]
+sci_buf2yaw[5])*(-1);
}
else
{
sci_buf2yaw[0]=(g_str_yaw[0]-48)*100;
sci_buf2yaw[1]=(g_str_yaw[1]-48)*10;
sci_buf2yaw[2]=(g_str_yaw[2]-48);
sci_buf2yaw[4]=(g_str_yaw[4]-48)*0.1;
sci_buf2yaw[5]=(g_str_yaw[5]-48)*0.01;
g_fp32yaw = sci_buf2yaw[0]+sci_buf2yaw[1]+sci_buf2yaw[2]
+sci_buf2yaw[4]+sci_buf2yaw[5];
}
}
else if(length_yaw == 7)
{
sci_buf2yaw[1]=(g_str_yaw[1]-48)*100;
sci_buf2yaw[2]=(g_str_yaw[2]-48)*10;
sci_buf2yaw[3]=(g_str_yaw[3]-48);
sci_buf2yaw[5]=(g_str_yaw[5]-48)*0.1;
g_fp32yaw=(sci_buf2yaw[1]+sci_buf2yaw[2]+sci_buf2yaw[3]
+sci_buf2yaw[5])*(-1);
}
}
5.3. roll축 pid 제어 소스
roll_motor.g_fp32err_vel[9] = roll_motor.g_fp32err_vel[8];
roll_motor.g_fp32err_vel[8] = roll_motor.g_fp32err_vel[7];
roll_motor.g_fp32err_vel[7] = roll_motor.g_fp32err_vel[6];
roll_motor.g_fp32err_vel[6] = roll_motor.g_fp32err_vel[5];
roll_motor.g_fp32err_vel[5] = roll_motor.g_fp32err_vel[4];
roll_motor.g_fp32err_vel[4] = roll_motor.g_fp32err_vel[3];
roll_motor.g_fp32err_vel[3] = roll_motor.g_fp32err_vel[2];
roll_motor.g_fp32err_vel[2] = roll_motor.g_fp32err_vel[1];
roll_motor.g_fp32err_vel[1] = roll_motor.g_fp32err_vel[0];
roll_motor.g_fp32err_vel[0] = g_fp32roll + g_fp32rollmoveval ;
roll_motor.g_fp32err_vel_sum += roll_motor.g_fp32err_vel[0]*0.001;
//백의 자리 까지 값 변함
if( roll_motor.g_fp32err_vel_sum > 100.0 )
roll_motor.g_fp32err_vel_sum = 100.0 ;
else if( roll_motor.g_fp32err_vel_sum < -100.0 )
roll_motor.g_fp32err_vel_sum = -100.0 ;
g_fp32_rolldiffvelyet =
((roll_motor.g_fp32err_vel[0] – roll_motor.g_fp32err_vel[1])
+(roll_motor.g_fp32err_vel[1] – roll_motor.g_fp32err_vel[2])
+(roll_motor.g_fp32err_vel[2] – roll_motor.g_fp32err_vel[3])+
(roll_motor.g_fp32err_vel[3] – roll_motor.g_fp32err_vel[4])
+(roll_motor.g_fp32err_vel[4] – roll_motor.g_fp32err_vel[5])
+(roll_motor.g_fp32err_vel[5] – roll_motor.g_fp32err_vel[6])+
(roll_motor.g_fp32err_vel[6] – roll_motor.g_fp32err_vel[7])) ;
if(g_fp32_rolldiffvelyet > 0.8) g_fp32_rolldiffvelyet = 0.8;
else if(g_fp32_rolldiffvelyet < -0.8) g_fp32_rolldiffvelyet = -0.8;
if(g_fp32_rolldiffvelyet)
{
g_fp32_rolldiffvelyet2 = g_fp32_rolldiffvelyet * g_fp32_rolldiffvelyet *
g_fp32_rolldiffvelyet * g_fp32_rolldiffvelyet * g_fp32_rolldiffvelyet ;
}
else if(g_fp32_rolldiffvelyet == 0)
{
g_fp32_rolldiffvelyet = g_fp32_rolldiffvelyet2 ;
}
roll_motor.g_fp32proportion_val = roll_motor.g_fp32kp *
(roll_motor.g_fp32err_vel[0] );
roll_motor.g_fp32differential_val = roll_motor.g_fp32kd *
(g_fp32_rolldiffvelyet * 13);
roll_motor.g_fp32integral_val = roll_motor.g_fp32ki *
roll_motor.g_fp32err_vel_sum ;
roll_motor.g_fp32PID_output = roll_motor.g_fp32proportion_val
+ roll_motor.g_fp32differential_val + roll_motor.g_fp32integral_val ;
if( roll_motor.g_fp32PID_output > 300 ) roll_motor.g_fp32PID_output = 300;
else if( roll_motor.g_fp32PID_output < -300 ) roll_motor.g_fp32PID_output = -300;
roll_motor.g_fp32L_Handle = 1 + (roll_motor.g_fp32PID_output /
g_fp32_rollhandleval);
roll_motor.g_fp32R_Handle = 1 – (roll_motor.g_fp32PID_output /
g_fp32_rollhandleval);
g_fp32_roll1motorval = g_fp32pwm_val * roll_motor.g_fp32L_Handle;
g_fp32_roll2motorval = g_fp32pwm_val * roll_motor.g_fp32R_Handle;
5.4. yaw축 서보모터 pid 제어 소스
g_fp32_yawerr[9] = g_fp32_yawerr[8];
g_fp32_yawerr[8] = g_fp32_yawerr[7];
g_fp32_yawerr[7] = g_fp32_yawerr[6];
g_fp32_yawerr[6] = g_fp32_yawerr[5];
g_fp32_yawerr[5] = g_fp32_yawerr[4];
g_fp32_yawerr[4] = g_fp32_yawerr[3];
g_fp32_yawerr[3] = g_fp32_yawerr[2];
g_fp32_yawerr[2] = g_fp32_yawerr[1];
g_fp32_yawerr[1] = g_fp32_yawerr[0];
g_fp32_yawerr[0] = yaw_motor.g_fp32err_vel[9];
yaw_motor.g_fp32err_vel[9] = yaw_motor.g_fp32err_vel[8];
yaw_motor.g_fp32err_vel[8] = yaw_motor.g_fp32err_vel[7];
yaw_motor.g_fp32err_vel[7] = yaw_motor.g_fp32err_vel[6];
yaw_motor.g_fp32err_vel[6] = yaw_motor.g_fp32err_vel[5];
yaw_motor.g_fp32err_vel[5] = yaw_motor.g_fp32err_vel[4];
yaw_motor.g_fp32err_vel[4] = yaw_motor.g_fp32err_vel[3];
yaw_motor.g_fp32err_vel[3] = yaw_motor.g_fp32err_vel[2];
yaw_motor.g_fp32err_vel[2] = yaw_motor.g_fp32err_vel[1];
yaw_motor.g_fp32err_vel[1] = yaw_motor.g_fp32err_vel[0];
yaw_motor.g_fp32err_vel[0] = g_fp32yaw – g_fp32_yawOffset
+ g_fp32yawmoveval ;
if( yaw_motor.g_fp32err_vel_sum > 150.0 )
yaw_motor.g_fp32err_vel_sum = 150.0 ;
if( yaw_motor.g_fp32err_vel_sum < -150.0 )
yaw_motor.g_fp32err_vel_sum = -150.0 ;
g_fp32_yawdiffvelyet = (yaw_motor.g_fp32err_vel[0] -
g_fp32_yawerr[9]) * 1.6;/
if(g_fp32_yawdiffvelyet > 0)
{
g_int32_yawdiffint = g_fp32_yawdiffvelyet ;
}
else if(g_fp32_yawdiffvelyet < -0)
{
g_int32_yawdiffint = g_fp32_yawdiffvelyet ;
}
else
{
g_int32_yawdiffint = 0;
g_fp32_yawdiffvelyet = 0;
}
g_fp32_yawaverage[4] = g_fp32_yawaverage[3];
g_fp32_yawaverage[3] = g_fp32_yawaverage[2];
g_fp32_yawaverage[2] = g_fp32_yawaverage[1];
g_fp32_yawaverage[1] = g_fp32_yawaverage[0];
g_fp32_yawaverage[0] = g_int32_yawdiffint;
g_int32_yawdiffvelyet3 =
(g_fp32_yawaverage[0]+g_fp32_yawaverage[1]+g_fp32_yawaverage[2]+
g_fp32_yawaverage[3]+g_fp32_yawaverage[4])*0.25 ;
yaw_motor.g_fp32proportion_val = yaw_motor.g_fp32kp *
yaw_motor.g_fp32err_vel[0] ;
yaw_motor.g_fp32differential_val = yaw_motor.g_fp32kd *
g_int32_yawdiffvelyet3;
yaw_motor.g_fp32PID_output =
yaw_motor.g_fp32proportion_val + yaw_motor.g_fp32differential_val ;
if( yaw_motor.g_fp32PID_output > 100 ) yaw_motor.g_fp32PID_output = 100;
else if( yaw_motor.g_fp32PID_output < -100 ) yaw_motor.g_fp32PID_
output = -100;
g_int32_yawint = (yaw_motor.g_fp32PID_output / g_fp32_yawhandleval );
g_fp32_yawHandle = g_int32_yawint * 0.5 ;
g_fp32_yawmotorval = 27.5 – g_fp32_yawHandle ;
if((g_fp32_yawmotorval > 17)&&(g_fp32_yawmotorval < 37))
{
TIM3->CCR1 = g_fp32_yawmotorval;
}
6. 참고문헌
· Cortex-M3/M4 완벽가이드(높이깊이) – 임종수
· ARM Cortex-M3 시스템 프로그래밍 완전정복1(D&W WAVE) – 박선호, 오영환
· 최신제어시스템 제 11판(PEARSON) – Richard C. Dorf