Transcript (RC servo motor & servo type DC motor) 실습 (not updated)

마이크로프로세서응용설계 - 8
RC servo motor
servo type DC motor
시스템및센서네트워크연구실
1
ATmega128
시스템및센서네트워크연구실
2
LED sink / switch pullup 회로
470
5V
PC0
+5V
PC1
…..
10K
PC7
PD2
104 (0.1uF)
시스템및센서네트워크연구실
3
LCD 회로
#1
+5V
VDD
#2
LCD 밝기 조절
V0
VSS
#3
시스템및센서네트워크연구실
+5
V
LEDA
LEDK
RS
R/W
E
PC0
PC1
PC2
DB4
DB5
DB6
DB7
PC4
PC5
PC6
PC7
4
Speaker 회로
-
OC1A(PB5)
+
+5V
시스템및센서네트워크연구실
5
시스템및센서네트워크연구실
6
LCD 함수
// LCD 초기화 함수
#include <delay.h>
void LCD_init(void)
#define LINE2
#define HOME
#define RSHIFT
#define LSHIFT
#define DISPON
#define DISPOFF
0xC0
0x02
0x1C
0x18
0x0c
0x08
// 2nd Line Move
// Cursor Home
// Display Right Shift
// Display Left Shift
// Display On
// Display Off
{
DDRC = 0xFF;
// 포트 C 출력 설정
PORTC &= 0xFB;
//E = 0;
// 충분한 지연시간을 통한 안정화 과정
delay_ms(15);
Command(0x20);
void LCD_init(void);
void LCD_String(char flash str[]);
void Busy(void);
void Command(unsigned char);
void Data(unsigned char);
// D5=1
delay_ms(5);
Command(0x20);
// D5=1
delay_us(100);
Command(0x20);
// D5=1
// 초기화 과정
Command(0x28);
// function set
Command(0x06);
// entry mode set
Command(0x01);
// all clear
Command(0x0c);
// display on
}
시스템및센서네트워크연구실
7
//데이터 쓰기 함수
void Data(unsigned char byte)
{
Busy();
// 인스트럭션 쓰기 함수
void Command(unsigned char byte)
{
Busy();
// 인스트럭션 상위 바이트
PORTC = (byte & 0xF0);
PORTC &= 0xFE;
PORTC &= 0xFD;
delay_us(1);
PORTC |= 0x04;
delay_us(1);
PORTC &= 0xFB;
// 인스트럭션 하위 바이트
PORTC = ((byte<<4) & 0xF0);
PORTC &= 0xFE;
PORTC &= 0xFD;
delay_us(1);
PORTC |= 0x04;
delay_us(1);
PORTC &= 0xFB;
// 데이터 상위 바이트
PORTC = (byte & 0xF0);
PORTC |= 0x01;
PORTC &= 0xFD;
delay_us(1);
PORTC |= 0x04;
delay_us(1);
PORTC &= 0xFB;
// 데이터
// RS = 0;
// RW = 0;
// E = 1;
// E = 0;
// 데이터 하위 바이트
PORTC = ((byte<<4) & 0xF0);
PORTC |= 0x01;
PORTC &= 0xFD;
delay_us(1);
PORTC |= 0x04;
delay_us(1);
PORTC &= 0xFB;
// 데이터
// RS = 0;
// RW = 0;
// E = 1;
// E = 0;
// 데이터
//RS = 1;
//RW = 0;
//E = 1;
//E = 0;
// 데이터
//RS = 1;
//RW = 0;
//E = 1;
//E = 0;
}
}
// 문자열 출력 함수
void LCD_String(char flash str[])
{
char flash *pStr=0;
// Busy Flag Check -> 일반적인 BF를 체크하는 것이 아니라
// 일정한 시간 지연을 이용한다.
void Busy(void)
{
delay_ms(2);
}
pStr = str;
while(*pStr) Data(*pStr++);
}
시스템및센서네트워크연구실
8
Motor
 모터
 전기에너지를 회전운동의 기계적 에너지로 변환시키는 전기부품
 모터의 종류
 브러시 타입 직류 모터 (brush-type DC motor)
• 회전자(rotor) 권선(coil)에서 다른 권선으로 전류를 스위칭하기 위하여 정류
자(commutator)와 접촉되어 있는 브러시(brush)를 가지는 DC 모터
• 회전자는 코일 권선을 갖고 있으며, 고정자는 영구 자석이나 전자석으로 구
성
시스템및센서네트워크연구실
9
• DC 모터의 제어 회로 (H-회로)
• 속도 피드백 제어
시스템및센서네트워크연구실
10
 BLDC motor (brushless DC motor)
• 회전자가 영구 자석이고 고정자가 코일 권선을 갖는 형태
시스템및센서네트워크연구실
11
 AC 모터
• 교류 전압으로 작동하는 전기 모터
• 동기모터(synchronous motor)와 유도모터(induction motor)로 구분
 스테핑 모터
• 일정한 각도만큼 움직이는 펄스 신호를 보내서 작동시키는 모터
• 입력 펄스 수에 따라 회전 각도가 변위되며, 펄스 주파수에 비례하여 회전 속
도도 변화함
시스템및센서네트워크연구실
12
 서보 모터
 주어진 신호에 의해 일정한 각을 유지하거나 일정한 위치를 유지하는 모터
 토크 제어, 속도 제어, 위치 제어 서보 모터로 분류할 수 있음
 DC or AC 서보 모터
• DC or AC 모터에 엔코더를 설치하여 폐루프 제어 시스템을 구성
 RC 서보 모터
• 소형 DC 모터, 감속기어장치, 회전각도센서, 제어를 위한 전자 회로 등이 구
성되어 있어, 가장 간단하게 서보 제어되는 모터
• 장난감 등 무선 제어에 많이 사용되어 왔기에, RC(radio controlled) 서보 모터
라고 부름
시스템및센서네트워크연구실
13
RC servo motor
 RC servo motor
 주어진 전압 레벨에 따라 일정한 각을 유지하거나 일정한 위치를 유지하는 모터
 용도
• 무선 모형 비행기의 방향타, 무선 자동차의 조향 장치, 로봇 팔 등
 동작
• 10-20ms 주기로 PWM 파형 입력
• PWM 듀티비에 따라 일정한 각도 유지
시스템및센서네트워크연구실
14
실습 1
 RC servo motor : HS-311
 stall torque : 3kgf·cm
 operating speed : 0.19sec/60o at no load
 running current : 160mA/60o at no load
 stall current : 700mA
 operating voltage : 4.8 – 6V
RC servo motor
PWM Vcc GND
black
red
yellow
시스템및센서네트워크연구실
15
// 확인
// 1. RC 서보 모터의 위치를 정중앙으로 조정하려면, OCR1A의 값은?
// 2. RC 서보 모터의 위치를 시계방향 90도 회전시키려면, OCR1A의 값은?
// 3. RC 서보 모터의 위치를 반시계방향 90도 회전시키려면, OCR1A의 값은?
// fast PWM, ICRn TOP 실험
// 64분주, ICR1=0x09C3로 하여, 주기를 10msec로 조정
void main(void)
{
DDRB = 0b00100000;
// PB5(OC1A) 출력 설정
TCCR1A = 0b10000010;
TCCR1B = 0b00011011;
ICR1H = 0x09;
ICR1L = 0xC3;
// mode 14(ICRn top fast PWM) : 1110, 1,0(WGM11,10)-10, 7,6(COM1A1,COM1A0)-10
// 4,3(WGM13,12)-11, 2-0(CS)-011 (64분주)
// ICR1 = 2499 (0x09C3) -> 주기 : 10msec
TCNT1 = 0x0000;
// 타이머1 초기화
OCR1A = 0x0000;
delay_ms(50);
while(1){
OCR1A = 375;
}
// while(1) : 무한 루프
// 160(-90)-375(0)-590(+90) - 430칸, resolution - 1칸당 0.4186도
}
시스템및센서네트워크연구실
16
실습 1-1
// PWM 모드를 10bit TOP으로 했을 경우
// 1. RC 서보 모터의 위치를 정중앙으로 조정하려면, OCR1A의 값은?
// 2. RC 서보 모터의 위치를 시계방향 90도 회전시키려면, OCR1A의 값은?
// 3. RC 서보 모터의 위치를 반시계방향 90도 회전시키려면, OCR1A의 값은?
void main(void)
{
DDRB = 0b00100000;
// PB5(OC1A) 출력 설정
TCCR1A = 0b10000011;
// mode 7 (10bit TOP fast PWM) : 0111 1,0(WGM11,WGM10)-11, 7,6(COM1A1,COM1A0)-10
TCCR1B = 0b00001010;
// 4,3(WGM13,WGM12)-01, 2-0(CS)-010
TCNT1 = 0x0000;
// 타이머1 초기화
OCR1A = 0x0000;
delay_ms(50);
while(1){
OCR1A = 0x01FF;
// while(1) : 무한 루프
// 0%:0x0000, 25%:0x00FF, 50%:0x01FF, 75%:0x02FF, 100%:0x03FF (10bit TOP)
// ?(-90) - ?(0) - ?(+90) – 180도에 총 xx 칸, resolution - 1칸당 xx 도
}
}
시스템및센서네트워크연구실
17
실습 2
// while(1) : 무한 루프
while(1){
// mode 14, ICRn을 TOP으로 설정
// 64분주, ICR1=0x09C3로 하여, 주기를 10msec로 조정
// 0, +45, +90, 0, -45, -90을 1s 간격으로 반복
if(rot_dir == 0) {
rot_value += rot_delta;
if(rot_value >= rot_max) {
OCR1A = rot_max;
int rot_base = 375, rot_min = 160, rot_max = 590;
int rot_delta = 215;
// 45도씩 변경
int rot_value=0, rot_dir=0;
// rot_value : 현재 각도, rot_dir : 방향 0=CW, 1=CCW
rot_dir = 1;
}
else
void main(void)
{
DDRD = 0xFF;
OCR1A = rot_value;
}
else {
rot_value -= rot_delta;
DDRB = 0b01100000;
// PB5(OC1A), PB6(OC1B) 출력 설정
TCCR1A = 0b10100010; // mode 14(ICRn top fast PWM) : 1110,
// 1,0(WGM11,10)-10, 7,6(COM1A1,COM1A0)-10
TCCR1B = 0b00011011; // 4,3(WGM13,12)-11, 2-0(CS)-011 (64분주)
ICR1H = 0x09;
ICR1L = 0xC3;
rot_value = rot_base;
if(rot_value <= rot_min) {
OCR1A = rot_min;
rot_dir = 0;
}
else
OCR1A = rot_value;
// 정중앙에 위치 (0도)
}
delay_ms(1000);
OCR1A = rot_value;
}
}
시스템및센서네트워크연구실
18
LCD_init();
degree_display();
실습 3
// mode 14, ICRn을 TOP으로 설정
// 64분주, ICR1=0x09C3로 하여, 주기를 10msec로 조정
// SW1 누를 때 점차 증가, SW2 누를 때 점차 감소
// LCD에 각도 출력
while(1);
}
// 외부 인터럽트0 서비스 루틴
interrupt [EXT_INT0] void external_int0(void)
int rot_base = 375, rot_min = 160, rot_max = 590;
int rot_delta = 12;
// rot_delta : 90/215°(0.419° at 1 step) * 12 = 5.028°
int rot_value=0, rot_dir=0;
// rot_value : 현재 각도, rot_dir : 방향 0=CW, 1=CCW
{
if(rot_value >= rot_max)
rot_value = rot_max;
else
void main(void)
{
DDRB = 0b01100000;
// PB5(OC1A), PB6(OC1B) 출력 설정
TCCR1A = 0b10100010; // mode 14(ICRn top fast PWM) : 1110, 1,0(WGM11,10)-10
// 7,6(COM1A1,COM1A0)-10 ,5,4(COM1B1,COM1B0)-10
TCCR1B = 0b00011011; // 4,3(WGM13,12)-11, 2-0(CS)-011 (64분주)
ICR1H = 0x09;
// ICR1 = 2499 (0x09C3) -> 주기 : 10msec
ICR1L = 0xC3;
DDRD = 0b00000000;
EIMSK = 0b00000011;
EICRA = 0b00001010;
// PD0, PD1 입력 설정 (SW1, SW2)
// 외부 인터럽트0, 1 인에이블
// 외부 인터럽트0, 1 하강 에지
SREG = 0b10000000;
// 전역 인터럽트 인에이블 비트 셋
rot_value += rot_delta;
OCR1A = rot_value;
degree_display();
}
// 외부 인터럽트1 서비스 루틴
interrupt [EXT_INT1] void external_int1(void)
{
if(rot_value < =rot_min)
rot_value = rot_min;
else
rot_value -= rot_delta;
OCR1A = rot_value;
rot_value = rot_base;
OCR1A = rot_value;
시스템및센서네트워크연구실
degree_display();
}
19
void degree_display(void)
{
float degree_value=0;
int temp0=0, temp1=0, temp2=0, temp=0;
temp2 = temp / 100;
temp = temp % 100;
temp1 = temp / 10;
temp0 = temp % 10;
Data(0x30 + temp2);
Data(0x30 + temp1);
LCD_String(“.”);
Data(0x30 + temp0);
Command(HOME);
LCD_String(“Degree:”);
if(rot_value > rot_base) {
degree_value = (float)(rot_value - rot_base) * 90 / 215;
LCD_String(“+”);
}
else {
degree_value = (float)(rot_base - rot_value) * 90 / 215;
LCD_String(“-”);
}
}
temp = (int)(degree_value*10);
시스템및센서네트워크연구실
20
실습 4
rot_value = rot_base;
OCR1A = rot_value;
// 타이머/카운터1를 이용한 fast PWM , ICRn TOP 실험
// 타이머/카운터3를 이용하여 0.2초마다 OC1A(PB5)에
// 접속된 모터의 각도 증가 감소를 반복하는 프로그램
// 0.2s 계산 : 1/16us * 256분주 * 12500 = 200ms
int rot_base = 375, rot_min = 160, rot_max = 590;
}
int rot_delta = 12; // rot_delta : 90/215°(0.419° at 1 step) * 12 = 5.028°
{
int rot_value=0, rot_dir=0;
while(1);
interrupt [TIM3_OVF] void timer_int3(void)
// rot_value : 현재 각도, rot_dir : 방향 0=CW, 1=CCW
TCNT3H = 0xCF;
TCNT3L = 0x2C;
void main(void)
{
ETIMSK = 0b00000100;
TCCR3A = 0b00000000;
TCCR3B = 0b00000100;
TCNT3H = 0xCF;
TCNT3L = 0x2C;
DDRB = 0b01100000;
TCCR1A = 0b10100010;
TCCR1B = 0b00011011;
ICR1H = 0x09;
ICR1L = 0xC3;
if(dir == 0) {
rot_value += rot_delta;
if(rot_value >= rot_max) {
rot_value = rot_max;
dir = 1;
}
}
else {
rot_value -= rot_delta ;
if(rot_value <= rot_min) {
rot_value = rot_min;
dir = 0;
}
}
// TOIE3 = 1;
// 일반모드 (WGM11, WGM10)
// 일반모드 (WGM13, WGM12), 프리스케일 = 256
//53036;
// PB5(OC1A), PB6(OC1B) 출력 설정
// mode 14(ICRn top fast PWM) : 1110,
// 1,0(WGM11,10)-10, 7,6(COM1A1,COM1A0)-10
// 4,3(WGM13,12)-11, 2-0(CS)-011 (64분주)
// ICR1 = 2499 (0x09C3) -> 주기 : 10msec
SREG = 0b10000000;
LCD_init();
degree_display();
시스템및센서네트워크연구실
OCR1A = rot_value;
degree_display();
}
21
실습 5
// 타이머/카운터1를 이용한 fast PWM , ICRn TOP 실험
// 타이머/카운터3를 이용하여 0.2초마다 OC1A(PB5)에 접속된 RC 서보모터가 좌우로 왔다갔다 반복하는 프로그램
// 0.2s 계산 : 1/16us * 256분주 * 12500 = 200ms
// SW1을 누르면 RC 서보모터가 좌우로 왔다갔다를 반복, SW2를 누르면 RC 서보모터 정지
int rot_base = 375, rot_min = 160, rot_max = 590;
int rot_delta = 12;
// rot_delta : 90/215°(0.419° at 1 step) * 10 = 5.028°
int rot_value=0, rot_dir=0;
// rot_value : 현재 각도, rot_dir : 방향 0=CW, 1=CCW
void main(void)
{
DDRD = 0b00000000; // PD0, PD1 입력 설정 (SW1, SW2)
EIMSK = 0b00000011; // 외부 인터럽트0, 1 인에이블
EICRA = 0b00001010; // 외부 인터럽트0, 1 하강 에지
SREG = 0b10000000;
LCD_init();
degree_display();
rot_value = rot_base;
OCR1A = rot_value;
while(1);
}
시스템및센서네트워크연구실
22
// 외부 인터럽트0 서비스 루틴
interrupt [EXT_INT0] void external_int0(void)
interrupt [TIM3_OVF] void timer_int3(void)
{
{
TCNT3H = 0xCF;
ETIMSK = 0b00000100; // TOIE3 = 1;
TCCR3A = 0b00000000; // 일반모드 (WGM11, WGM10)
TCCR3B = 0b00000100; // 일반모드 (WGM13, WGM12), 프리스케일 = 256
TCNT3H = 0xCF;
TCNT3L = 0x2C;
//53036;
DDRB = 0b01100000;
TCCR1A = 0b10100010;
TCCR1B = 0b00011011;
ICR1H = 0x09;
ICR1L = 0xC3;
OCR1A = rot_value;
TCNT3L = 0x2C;
if(dir == 0) {
rot_value += rot_delta;
if(rot_value >= rot_max) {
rot_value = rot_max;
dir = 1;
}
}
else {
rot_value -= rot_delta ;
if(rot_value <= rot_min) {
rot_value = rot_min;
dir = 0;
}
}
// PB5(OC1A), PB6(OC1B) 출력 설정
// mode 14(ICRn top fast PWM) : 1110,
// 1,0(WGM11,10)-10, 7,6(COM1A1,COM1A0)-10
// 4,3(WGM13,12)-11, 2-0(CS)-011 (64분주)
// ICR1 = 2499 (0x09C3) -> 주기 : 10msec
}
// 외부 인터럽트1 서비스 루틴
interrupt [EXT_INT1] void external_int1(void)
{
TCCR3A = 0x00;
TCCR3B = 0x00;
TCCR1A = 0x00;
TCCR1B = 0x00;
}
시스템및센서네트워크연구실
//53036;
OCR1A = rot_value;
degree_display();
}
23
실습 6
motor+
motor-
red
black
 Servo type DC motor : SE-DS350
DC servo motor
 control system : voltage phase control
 torque : 4.2 / 5.2 kg cm at 4.8 / 6.0 V
 operating speed : 100 / 126 rpm at 4.8 / 6.0 V
 running current : 120mA
 DC motor driver
 maximum allowable current : 100mA
 소형 DC 모터 2개 구동
 정방향, 역방향으로 구동 가능
시스템및센서네트워크연구실
24
// 확인
// 1. DC 모터 정회전
// 2. DC 모터 역회전
void main(void)
{
DDRB = 0b00000011;
// PB0, PB1 출력 설정
// PB0 – 좌측 motor1_A상 연결, PB1 – 좌측 motor1_B상 연결
while(1){
// while(1) : 무한 루프
PORTB = 0b00000001; // CCW – A상 1, B상 0
delay_ms(1000);
PORTB = 0b00000000; // 정지
delay_ms(1000);
PORTB = 0b00000010; // CW – A상 0, B상 1
delay_ms(1000);
PORTB = 0b00000000; // 정지
delay_ms(1000);
}
}
시스템및센서네트워크연구실
25
실습 7
// 확인
// 1. OCR1A의 듀티비를 줄이게 되면 DC 모터의 속도 변화
// fast PWM, ICRn TOP 실험
// 64분주, ICR1=0x09C3로 하여, 주기를 10msec로 조정
void main(void)
{
DDRB = 0b00100000;
// PB5(OC1A) 출력 설정
// PB5 – 좌측 motor1_A상 연결, PB1 – GND 연결
TCCR1A = 0b10000010;
TCCR1B = 0b00011011;
ICR1H = 0x09;
ICR1L = 0xC3;
// mode 14(ICRn top fast PWM) : 1110, 1,0(WGM11,10)-10, 7,6(COM1A1,COM1A0)-10
// 4,3(WGM13,12)-11, 2-0(CS)-011 (64분주)
// ICR1 = 2499 (0x09C3) -> 주기 : 10msec
TCNT1 = 0x0000;
// 타이머1 초기화
OCR1A = 0x0000;
delay_ms(50);
while(1){
OCR1A = 2499;
// while(1) : 무한 루프
// 1이 출력되면 CCW 동작
// duty ratio 100% => 이걸 조금씩 줄여 나가보자. 속도가 변화하는 거 같은가?
}
}
시스템및센서네트워크연구실
26