Á¶È¸ : 4000 |
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);
}
}
|
|