KoreanEnglish ROBOONE Korea Official Site Homesitemapmailadmin
모임소개일정관리공지사항게시판자료실대회현황ROBO-ONE Japan
 
자료실 > 강좌자료
게시물 번호    11 - 1 작 성 일    2003-03-27  01:52:17
글 쓴 이    메가로보틱스 Homepage     http://www.megarobotics.com

조회 : 3622 AIMOTOR의 포지션리드 테스트 수정하기 삭제하기   
첨부된 이미지 : human3-2.gif


두개의 모터를 준비를 하시고 하나의 모터는 디폴트 아이디인 0번으로 그냥두고 하나의 모터는 18번으로 툴을 이용하여 세팅하시고 두모터를 케이블로 연결해 주면 이번 테스트의 준비는 다된 것입니다. 그리고 각모터의 축에 RC 혼을 연결해 주시면 됩니다.
이 예제는 두모터중 0번 모터를 손으로 움직이면 다른 모터가 손으로 움직인 모터의 변위만큼 따라서 움직이도록 하는것입니다. 그래서 손으로 움직이는 모터의 변화하는 위치값을 계속적으로 읽어서 다른 하나의 모터를 따라 움직이게 하는 실험입니다.

#include <mega32.h>
#include <delay.h>
#include <stdio.h>


#define RXB8 1
#define TXB8 0
#define OVR 3
#define FE 4
#define UDRE 5
#define RXC 7

#define RXCIE 7
#define TXCIE 6
#define UDRIE 5
#define TXEN 3
#define RXEN 4
#define CHR9 2


// Frequency = 7.3728MHz
/*

// 클럭 7.3728 MHz일때의 보레이트 설정 상수 (에러 = 0%)
#define BR9600 47
#define BR19200 23
#define BR38400 11
#define BR57600 7
#define BR115200 3
*/

// Frequency = 14.7456MHz
// 클럭 14.7456 MHz일때의 보레이트 설정 상수 (에러 = 0%)
#define BR9600 95
#define BR19200 47
#define BR38400 23
#define BR57600 11
#define BR115200 7

unsigned char G_ID=0; // 아이디 전역 변수

void Motor_Test3(void);

void main(void)
{
// Declare your local variables here



// 포트 설정 ==========================================================
// Input/Output Ports initialization
// Port A initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T
PORTA=0x00;
DDRA=0x00;

// Port B initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T
PORTB=0x00;
DDRB=0x00;

// Port C initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func0=In Func1=Out Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In
// State0=P State1=1 State2=P State3=P State4=T State5=T State6=T State7=T
PORTD=0x0C;
DDRD=0x00;


// 타이머 설정
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=FFh
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer 1 Stopped
// Mode: Normal top=FFFFh
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer 2 Stopped
// Mode: Normal top=FFh
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;


// 외부 인터럽트와 칩 설정
// External Interrupt(s) initialization
// INT0: off
// INT1: off
// INT2: Off
GICR=0x00;
MCUCR=0x0F;
MCUCSR=0x00;
GIFR=0x00;

// 타이머 인터럽트 설정
// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;


// 시리얼 포트 설정
// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud rate: 115200
UCSRA=0x00;
UCSRB=0x18;
UCSRC=0x86;
UBRRL=0x07;
UBRRH=0x00;


// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
// Analog Comparator Output: Off
ACSR=0x80;
SFIOR=0x00;


Motor_Test3();

}

// 시리얼 포트로 한 문자를 전송하기 위한 함수
void sciTxData(unsigned char td)
{
while(!(UCSRA&(1<<UDRE)));
// 이전의 전송이 완료될때까지 대기
UDR=td;
}

// 시리얼 포트로 한 문자를 받을때까지 대기하기 위한 함수
unsigned char sciRxReady(void)
{
while(!(UCSRA&(1<<RXC)) );
// 한 문자가 수신될때까지 대기
return UDR;
}

// 모터에 위치, 회전, 파워 절약 모드를 제어할때 모터의 프로토콜로 전송하는 함수
void Send2MotorCMD(unsigned char mode,unsigned char id,unsigned char data)
{
// id : 0 ~ 30
// 1.Position Control Mode : mode = ( 0 ~ 4 ) -> speed , data = ( 0 ~ 254 )
// 2.Position Read Mode : mode = ( 5 ) , data = ( x )
// 3.Act Down Mode : mode = ( 6 ) , data = ( 0x10 )
// 4.Power Down Mode : mode = ( 6 ) , data = ( 0x20 )
// 5.Wheel Mode : mode = ( 6 ) , data = ( 0x30 : CCW , 0x40 : CW ) | ( 0x00 ~ 0x0F : Speed )

unsigned char modid=0;
modid=((mode<<5)&0xe0) | id;
// 7 ~ 5 비트는 모드 , 4 ~ 0 비트는 아이디
sciTxData(0xff);
// 모터의 수신 초기화 값 전송
delay_us(5);
sciTxData(modid);
// 모드 | 아이디 전송
delay_us(5);
sciTxData(data);
// 위치 또는 제어값 전송
delay_us(5);
sciTxData( (modid^data)&0x7f ); // 체크섬 전송
delay_us(5);
}

// 모터의 세팅 모드를 제어할때 모터의 프로토콜로 전송하는 함수
void Send2MotorSetCMD(unsigned char mode,unsigned char id,unsigned char setmode,unsigned char setdata1,unsigned char setdata2)
{
// id : 0 ~ 30
// mode : 7 ( all Mode )
// x is any value
// 1.Baud Rate Setting Mode : setmode = ( 8 ) , setdata1 = ( Baud Rate Constant ) , setdata = ( x )
// 2.Gain Setting Mode : setmode = ( 9 ) , setdata1 = ( pGain Constant ) , setdata = ( dGain Constant )
// 3.ID Setting Mode : setmode = ( 10 ) , setdata1 = ( ID Constant ) , setdata = ( x )
// 4.Auto Gain Setting Mode : setmode = ( 11 ) , setdata1 = ( x ) , setdata = ( x )
// 5.Gain Read Mode : setdata1 = setmode = ( 12 ) , setdata1 = ( x ) , setdata = ( x )
// 6.Resolution Setting Mode : setmode = ( 13 ) , setdata1 = ( Resolution Constant : 0 or 1 ) , setdata = ( x )
// 7.Resolution Read Mode : setmode = ( 14 ) , setdata1 = ( x ) , setdata = ( x )
// 8.Over Current Setting Mode : setmode = ( 15 ) , setdata1 = ( Current Constant ) , setdata = ( x )
// 9.Over Current Read Mode : setmode = ( 16 ) , setdata1 = ( x ) , setdata = ( x )

unsigned char modid=0;
modid=((mode<<5)&0xe0) | id;
// 7 ~ 5 비트는 모드 , 4 ~ 0 비트는 아이디
sciTxData(0xff);
// 모터의 수신 초기화 값 전송
delay_us(5);
sciTxData(modid);
// 모드 | 아이디 전송
delay_us(5);
sciTxData(setmode);
// 세팅 모드 값 전송
delay_us(5);
sciTxData(setdata1);
// 첫번째 세팅 값 전송
delay_us(5);
sciTxData(setdata2);
// 두번째 세팅 값 전송
delay_us(5);
sciTxData( (modid^setmode^setdata1^setdata2)&0x7f );
// 체크섬 전송
delay_us(5);
}

// 위치 제어 함수
unsigned char Position_Control(unsigned char id,unsigned char position,unsigned char spd)
{
// 위치 제어 모드일때의 모터의 회전 속도 상수
#define MOT_SPEED0 0
#define MOT_SPEED1 1
#define MOT_SPEED2 2
#define MOT_SPEED3 3
#define MOT_SPEED4 4
unsigned char torque;
Send2MotorCMD( spd, id, position );
torque=sciRxReady();
sciRxReady();
return torque;


}

// 위치 확인 함수
unsigned char Position_Read(unsigned char id)
{
unsigned char pos;
Send2MotorCMD( 0x05, id, 0 );
sciRxReady();
pos=sciRxReady();;
return pos;

//sciRxReady();
// 첫번째 피드백 토크값 스킵
//return sciRxReady();
// 두번째 피드백 위치값 반환

}

// 모터의 전류 흐름을 차단하는 함수
void ActDown_Mode(unsigned char id)
{
#define ACT_DOWN 0x10
Send2MotorCMD( 0x06, id, ACT_DOWN );
}

// 파워 절약 모드 함수
void PwrDown_Mode(unsigned char id)
{
#define PWR_DOWN 0x20
Send2MotorCMD( 0x06, id, PWR_DOWN );
}

// 회전 모드 제어 함수
void Wheel_Mode(unsigned char id, unsigned char dir, unsigned char spd)
{
// id : 모터의 아이디
// dir : 모터의 방향 ( CW or CCW )
// spd : 모터의 회전 속도 ( 0 ~ 15 )

#define WHEEL_CW 0x40
#define WHEEL_CCW 0x30

#define WHEEL_STOP 0x00
// 방향 | 속도
Send2MotorCMD( 0x06, id , dir | spd );
}

// 보레이트 설정 함수
void BaudSet_Mode(unsigned char id, unsigned char Baud)
{
// Baud : 보레이트 상수
// Frequency = 14.7456MHz
#define BR9600 95
#define BR19200 47
#define BR38400 23
#define BR57600 11
#define BR115200 7

#define BAUD_SET 0x08
Send2MotorSetCMD( 0x07, id , BAUD_SET, Baud, Baud );

}

// 게인 설정 함수
void GainSet_Mode(unsigned char id, unsigned char pgain, unsigned char dgain)
{
#define GAIN_SET 0x09
Send2MotorSetCMD( 0x07, id , GAIN_SET, pgain, dgain );

}

// 아이디 설정 함수
void IDSet_Mode(unsigned char id, unsigned char newid)
{
// newid : 0 ~ 30

#define ID_SET 0x0A
Send2MotorSetCMD( 0x07, id , ID_SET, newid, newid );
}

void AutoGainSet_Mode(unsigned char id, unsigned char pgain, unsigned char dgain)
{
#define AGAIN_SET 0x0B
Send2MotorSetCMD( 0x07, id , AGAIN_SET, 0, 0 );

}

// 현재의 게인값 확인 함수
void GainRead_Mode(unsigned char id, unsigned char *pgain, unsigned char *dgain)
{
#define GAIN_READ 0x0C
Send2MotorSetCMD( 0x07, id , GAIN_READ, 0, 0 );
*pgain=sciRxReady();
*dgain=sciRxReady();
}

// 분해능 설정 함수
void ResolutionSet_Mode(unsigned char id, unsigned char resol)
{
// resol : 0 or 1

#define RESOL_SET 0x0D

#define RESOL_LOW 0
#define RESOL_HIGH 1

Send2MotorSetCMD( 0x07, id , RESOL_SET, resol, resol );
}

// 현재의 분해능 확인 함수
unsigned char ResolutionRead_Mode(unsigned char id)
{
#define RESOL_READ 0x0E
Send2MotorSetCMD( 0x07, id , RESOL_READ, 0, 0 );
return sciRxReady();


}

// 과전류 차단 설정 함수
void OverCurrentSet_Mode(unsigned char id, unsigned char current)
{

// current : 전류 상수 값

#define OVERCUR_SET 0x0F

// 전류 설정 상수
#define OC_400MA 20 // 400mA
#define OC_450MA 23
#define OC_500MA 25
#define OC_550MA 28
#define OC_600MA 30
#define OC_650MA 33
#define OC_700MA 35
#define OC_750MA 38
#define OC_800MA 40
#define OC_850MA 43
#define OC_900MA 46
#define OC_950MA 48
#define OC_1000MA 51 // 1000mA

Send2MotorSetCMD( 0x07,id,OVERCUR_SET,current,current );
}

// 현재 차단 전류값 확인 함수
unsigned char OverCurrentRead_Mode(unsigned char id)
{
#define OVERCUR_READ 0x10

Send2MotorSetCMD( 0x07, id , OVERCUR_READ, 0 , 0 );
return sciRxReady();

}

void Motor_Test3(void)
{
unsigned char position, pos_temp;
while(1){

position=Position_Read( 0 );
Position_Control( 18 , position , MOT_SPEED0);
delay_ms(20);

}

}

 

답변하기 목록보기 이전글 다음글

[ 현재 페이지 전체 목록 ]
    제                  목 글 쓴 이 등 록 일 조회 받음
20 파일받기:pyro.dwg(98K) 제 로봇 쇠돌이(Pyro)의 설계도입니다. 김낙현 2003-07-30 5560 159
19   R/C 서보 모터와 AI Motor 비교 메가로보틱스 2003-07-02 4873
18   AI Motor관련 매니아와 일문일답 메가로보틱스 2003-06-25 4344
17   AI Motor의 가격비밀 메가로보틱스 2003-06-25 5323
16   [자료] AIMOTOR로 만든 2 족 로봇 사진 3-2 메가로보틱스 2003-04-28 4901
15   [TIP4] AIMOTOR 에 메탈기어를 사용하... 메가로보틱스 2003-04-21 3966
14   [Tip3] AIMOTOR 로 이족 로봇을 만들때... 메가로보틱스 2003-04-14 5139
13 파일받기:1슈퍼짱 로봇 VER1.0.pdf(223K) TMS320F240 DSP 김진범 2003-04-10 3516 414
12   다수의 AIMOTOR를 싱크로 제어(인터럽트 이용) 메가로보틱스 2003-03-27 3472
11   AIMOTOR의 포지션리드 테스트 메가로보틱스 2003-03-27 3622
 
Copyright ⓒ Since 2005 www.robo-one.or.kr All rights reservd. Designed by SaehanDataSystem
이메일주소 무단수집 거부