目次

アクチュエータ制御

 アクチュエータを制御するために、ATtiny2313を利用します。



 ATtiny2313は、左右のDUTY比をシリアルで入力し
 対応したPWM波形を、フォトカプラに出力します。



 タイマー0を利用し、2つのPWM波形を生成します。
 レジスタOCR0A、OCR0Bに与える数値でDUTY比を確定します。

 タイマー0の内部カウンタは8ビットなので、0〜255が
 0〜100%に対応します。従って、シリアルで転送された
 値を、変換します。

 変換は、(100/255)をかけるだけでよいのですが
 動作を高速化するために、64倍した値を25で割る
 処理にしています。64倍をシフトで実現します。

 通信プロトコルは、以下です。

 左右のモータ回転パルスを、外部割込みで入力し
 内部カウンタを更新し、直進性を向上させるよう
 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 ; }
  }
}



目次

inserted by FC2 system