目次
前
次
アクチュエータ制御
アクチュエータを制御するために、ATtiny2313を利用します。
ATtiny2313は、左右のDUTY比をシリアルで入力し
対応したPWM波形を、フォトカプラに出力します。
タイマー0を利用し、2つのPWM波形を生成します。
レジスタOCR0A、OCR0Bに与える数値でDUTY比を確定します。
タイマー0の内部カウンタは8ビットなので、0〜255が
0〜100%に対応します。従って、シリアルで転送された
値を、変換します。
変換は、(100/255)をかけるだけでよいのですが
動作を高速化するために、64倍した値を25で割る
処理にしています。64倍をシフトで実現します。
通信プロトコルは、以下です。
- data rate -> 9600bps
- data length -> 8bits
- stop bit -> 1bit
- parity check -> none
- flow control -> none
左右のモータ回転パルスを、外部割込みで入力し
内部カウンタを更新し、直進性を向上させるよう
PID制御をかけます。
ソースコードは、以下です。
#include <avr/io.h>
#include <avr/iotn2313.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#define FOSC 4000000
#define BAUD 9600
#define MYUBRR (FOSC/16/BAUD)-1
#define OFF 0
#define ON OFF+1
typedef unsigned char UBYTE ;
typedef unsigned short UWORD ;
typedef signed char SBYTE ;
typedef signed short SWORD ;
typedef union {
struct {
unsigned char B0:1;
unsigned char B1:1;
unsigned char B2:1;
unsigned char B3:1;
unsigned char B4:1;
unsigned char B5:1;
unsigned char B6:1;
unsigned char B7:1;
} BIT ;
unsigned char DR ;
} FLAGSP ;
volatile FLAGSP x_flags ;
#define UFLAG x_flags.BIT.B0
#define TFLAG x_flags.BIT.B1
volatile UBYTE sbuf[8] ;
volatile UBYTE sindex ;
volatile UBYTE lduty ;
volatile UBYTE rduty ;
volatile UWORD lcount ;
volatile UWORD rcount ;
volatile UBYTE dleft ;
volatile UBYTE dright ;
#define NO 0
#define YES NO+1
#define MASKFF 0xff
#define MASK0F 0x0f
#define MASKF0 0xf0
#define INTERVAL 49999
const prog_char msg_h[] PROGMEM = "H help" ;
const prog_char msg_d[] PROGMEM = "D set" ;
const prog_char msg_sd[] PROGMEM = "d show" ;
/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void user_initialize(void);
void usart_putchar(UBYTE x);
void scmd_interpret(void);
void crlf(void);
void usart_puts(UBYTE *x);
UBYTE get_hex(UBYTE x);
UWORD get_left_velocity(void);
UWORD get_right_velocity(void);
void pid(void);
/*------*/
/* main */
/*------*/
int main(void)
{
/* initialize port and variables */
user_initialize();
/* enable interrupt */
sei();
/* endless loop */
while ( ON ) {
/* usart receive handling */
if ( UFLAG ) {
UFLAG = OFF ;
/* serial communication process */
scmd_interpret();
}
/* 100ms handling */
if ( TFLAG ) {
TFLAG = OFF ;
pid();
}
}
/* dummy */
return 0 ;
}
/*-----------------------*/
/* Insert user functions */
/*-----------------------*/
void user_initialize(void)
{
UWORD tmp ;
/* PORT B */
PORTB = 0b00000000 ; /* 00000000 */
DDRB = 0b11111111 ; /* oooooooo */
/* PORT D */
PORTD = 0b00001100 ; /* 00001100 */
DDRD = 0b11110010 ; /* ooooiioi */
/* initialize flags */
x_flags.DR = 0 ;
/* initialize timer/counter0 */
{
TCCR0A = (1 << COM0B1) | (1 << COM0A1) | (3 << WGM00);
TCCR0B = (7 << CS00) ;
TCNT0 = 0 ;
OCR0A = 128 ;
OCR0B = 128 ;
dleft = 0 ;
dright = 0 ;
}
/* initialize timer/counter1 */
{
/* compare match (1/8) -> 500kHz */
TCCR1B = (1 << WGM12) | (1 << CS10) ;
TCNT1 = 0 ;
OCR1A = INTERVAL ;
OCR1B = 60000 ;
}
/* initialize serial */
{
sindex = 0 ;
/* set Baud Rate Registers */
tmp = MYUBRR ;
UBRRH = (UBYTE)(tmp >> 8) ;
UBRRL = (UBYTE)(tmp & MASKFF) ;
/* Enable receive interrupt , receive module and transmit module */
UCSRB = (1 << TXEN) | (1 << RXEN) | (1 << RXCIE) ;
/* 8 bits , 1 stop , non parity */
UCSRC = (3 << UCSZ0);
}
/* set INT0 and INT1 */
MCUCR = (1 << ISC11) | (1 << ISC01) ;
/* set TIMER1 interrupt */
TIMSK = (1 << OCIE1A) ;
/* enable interrupt (global) */
GIMSK = (1 << INT1) | (1 << INT0) ;
lcount = 0 ;
rcount = 0 ;
/* set duty ratio */
lduty = 0 ;
rduty = 0 ;
}
/* command interpriter */
void scmd_interpret(void)
{
UWORD xtmp ;
UBYTE cmd ;
char msg[20];
/* get command */
cmd = *(sbuf+0) ;
/* judge */
if ( cmd == 'D' ) {
/* left duty */
dleft = get_hex( *(sbuf+1) ) ;
dleft = dleft * 10 + get_hex( *(sbuf+2) ) ;
xtmp = (dleft << 6) / 25 ;
lduty = xtmp & MASKFF ;
/* right duty */
dright = get_hex( *(sbuf+3) ) ;
dright = dright * 10 + get_hex( *(sbuf+4) ) ;
xtmp = (dright << 6) / 25 ;
rduty = xtmp & MASKFF ;
/* store duty ratio */
OCR0A = rduty ;
OCR0B = lduty ;
}
/* judge */
if ( cmd == 'd' ) {
/* store delimiter */
*(msg+4) = '\0' ;
/* separate */
*(msg+0) = dleft / 10 + '0' ; *(msg+1) = dleft % 10 + '0' ;
*(msg+2) = dright / 10 + '0' ; *(msg+3) = dright % 10 + '0' ;
/* show */
usart_puts((UBYTE *)msg);
}
/* help */
if ( cmd == 'H' ) {
strcpy_P(msg,msg_h) ; usart_puts((UBYTE *)msg);
strcpy_P(msg,msg_d) ; usart_puts((UBYTE *)msg);
strcpy_P(msg,msg_sd); usart_puts((UBYTE *)msg);
}
/* new line */
crlf();
}
void usart_putchar(UBYTE x)
{
while ( !(UCSRA & (1 << UDRE)) ) ;
UDR = x ;
}
void crlf(void)
{
usart_putchar('\r');
usart_putchar('\n');
}
void usart_puts(UBYTE *x)
{
/* send charactor one by one */
while ( *x != '\0' ) {
usart_putchar( *x ) ;
/* pointer increment */
x++ ;
}
crlf();
}
UBYTE get_hex(UBYTE x)
{
UBYTE result ;
/* default */
result = 0 ;
/* judge */
if ( '0' <= x && x <= '9' ) { result = x - '0' ; }
if ( 'A' <= x && x <= 'F' ) { result = x - 'A' + 10 ; }
if ( 'a' <= x && x <= 'f' ) { result = x - 'a' + 10 ; }
return result ;
}
/* EXTERNAL INT0 */
ISR(INT0_vect)
{
rcount++ ;
}
/* EXTERNAL INT1 */
ISR(INT1_vect)
{
lcount++ ;
}
/* timer1 interrupt */
ISR(TIMER1_COMPA_vect)
{
TFLAG = ON ;
}
/* UART receive interrupt */
ISR(USART_RX_vect)
{
volatile UBYTE ch ;
/* get 1 charactoer */
ch = UDR ;
/* store */
*(sbuf+sindex) = ch ;
sindex++ ;
/* judge */
if ( ch == '\r' ) {
UFLAG = ON ;
sindex = 0 ;
}
}
UWORD get_left_velocity(void)
{
UWORD result ;
/* disable INT1 interrupt */
GIMSK &= ~(1 << INT1) ;
/* get counter */
result = lcount ;
/* clear counter */
lcount = 0 ;
/* enable INT1 interrupt */
GIMSK |= (1 << INT1) ;
return result ;
}
UWORD get_right_velocity(void)
{
UWORD result ;
/* disable INT0 interrupt */
GIMSK &= ~(1 << INT0) ;
/* get counter */
result = rcount ;
/* clear counter */
rcount = 0 ;
/* enable INT0 interrupt */
GIMSK |= (1 << INT0) ;
return result ;
}
void pid(void)
{
UWORD cr ;
UWORD cl ;
UBYTE dd ;
/* get velocity */
cr = get_right_velocity() ;
cl = get_left_velocity() ;
/* equal */
if ( rduty == lduty ) {
dd = OCR0B ;
/* increment */
if ( cr > cl ) { OCR0B = dd + (cr - cl) / 10 ; }
/* decremtn */
if ( cr < cl ) { OCR0B = dd - (cl - cr) / 10 ; }
}
}
目次
前
次