Á¶È¸ : 3747 |
´Ù¼öÀÇ AIMOTOR¸¦ ½ÌÅ©·Î Á¦¾î(ÀÎÅÍ·´Æ® ÀÌ¿ë)
|
|
|
÷ºÎµÈ À̹ÌÁö : 1human3-2.jpg
Á¦¸ñÀÇ ¸»Àº ´Ù¼öÀÇ ¸ðÅ͸¦ Á¦¾îÇÒ ¶§ °¢ ¸ðÅÍÀÇ À§Ä¡º¯À§°¡ ´Ù¸£°Ô µÇÁö¸¸ °°Àº ½Ã°£¿¡ °¢±â ´Ù¸¥ º¯À§¸¦ ¿òÁ÷ÀÌ°Ô µÇ¸é ½ÃÀÛ°ú ³¡(½Ã°£)ÀÌ °°°Ô ¿òÁ÷ÀÏ ¼ö ÀÖ´Ù. À̶§ °¢¸ðÅÍÀÇ º¯À§°ª°ú ¼Óµµ°ªÀº ¹Ýºñ·ÊÇÏ°Ô µÉ °ÍÀÌ´Ù.
´ÙÀ½ÀÇ ÇÁ·Î±×·¥Àº ŸÀÓ»ó¼ö¸¸ÅÀÇ ½Ã°£µ¿¾È¿¡ °¢±â ´Ù¸¥ ¸ñÇ¥º¯À§¸¦ °®´Â µÎ¸ðÅ͸¦ ½ÌÅ©·Î(µ¿±âÈ)½ÃÅ°´Â °ÍÀ¸·Î ŸÀÌ¸Ó ÀÎÅÍ·´Æ®¸¦ ÀÌ¿ëÇßÀ¸´Ï Äڵ带 Àß ºÐ¼®ÇÏ¿© ½ÇÁ¦ ·Îº¿À» Á¦¾îÇϴµ¥ ÀÌ¿ëÇØ º¸±â ¹Ù¶õ´Ù.
#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
/*
#define BR9600 47
// Ŭ·° 7.3728 MHzÀ϶§ÀÇ º¸·¹ÀÌÆ® ¼³Á¤ »ó¼ö (¿¡·¯ = 0%)
#define BR19200 23
#define BR38400 11
#define BR57600 7
#define BR115200 3
*/
// Frequency = 14.7456MHz
#define BR9600 95
// Ŭ·° 14.7456 MHzÀ϶§ÀÇ º¸·¹ÀÌÆ® ¼³Á¤ »ó¼ö (¿¡·¯ = 0%)
#define BR19200 47
#define BR38400 23
#define BR57600 11
#define BR115200 7
unsigned char G_ID=0; // ¾ÆÀ̵ð Àü¿ª º¯¼ö
void Motor_Test1(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;
delay_ms(1000);
Motor_Test1();
}
// ½Ã¸®¾ó Æ÷Æ®·Î ÇÑ ¹®ÀÚ¸¦ Àü¼ÛÇϱâ À§ÇÑ ÇÔ¼ö
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();
}
unsigned int Interval;
unsigned char Src_Pos_0,Src_Pos_1;
unsigned char Dest_Pos_0,Dest_Pos_1;
unsigned char Sample_Num,Count;
interrupt [TIM1_OVF] void Timer1_Int(void)
{
float D0,D1;
D0=((Dest_Pos_0-Src_Pos_0)/Sample_Num)*Count;
D1=((Dest_Pos_1-Src_Pos_1)/Sample_Num)*Count;
Position_Control( 0 , Src_Pos_0+(unsigned char)D0 , MOT_SPEED0 );
Position_Control( 18 , Src_Pos_1+(unsigned char)D1 , MOT_SPEED0 );
TCNT1=65535-Interval*10;
if( Count++ >= Sample_Num )TCCR1B=0x00; // TIMER1 STOP
}
void Motor_Test1(void)
{
TIMSK=0x04; // TIMER1 INTERRUPT ENABLE
TCCR1B=0x00; // TIMER1 STOP
Src_Pos_0=80;
Src_Pos_1=80;
Dest_Pos_0=180;
Dest_Pos_1=240;
Sample_Num=10;
Interval=100;
Count=1;
Position_Control( 0 , Src_Pos_0 , MOT_SPEED0 );
Position_Control( 18 , Src_Pos_1 , MOT_SPEED0 );
#asm("sei"); //gloval interupt enable
TCCR1B=0x05; // Prescale CLK/1024 & Timer1 Start
while(1){};
}
|
|