วันเสาร์ที่ 5 พฤศจิกายน พ.ศ. 2554

ทดสอบ Accerelometer MMA7331








วิดีโอสาธิตการใช้งานครับ




สำหรับการใช้งานก็จะต่อกับ MCU Atmega168 ที่ 3.3v เลยเพื่อให้ง่านต่อการใช้งานครับ



#include <stdio.h>
#include <avr/io.h>                  //define port
#include <compat/deprecated.h>       //call function sbi(port,bit),cbi(port,bit) = Set,Clear bit
#include <avr/pgmspace.h>            //for progrm data to Area Flash
#include <stdlib.h>

#define F_CPU 20000000UL             //CPU Clock Frequency (Hz)
#include <util/delay.h>              //call function delay


int Xdata,Ydata,Zdata,AngleAcc;





/***************************************************************
 **                 Function use For Printf(UART0)            **
 ***************************************************************/


//--------------------- Prototype function ---------------------

static int uart_putchar(char c,FILE *stream);
static FILE uart_str = FDEV_SETUP_STREAM(uart_putchar,NULL,_FDEV_SETUP_WRITE);
void Init_Serial(unsigned int baudrate);

#define BAUD 56700
#define UBRR F_CPU/16/BAUD-1

//----------------------- UART0 Putchar -------------------------

static int uart_putchar(char c,FILE *stream)
 {
   if(c=='\a')
     {
   fputs("*ring*\n",stderr);
   return 0;
     }
   
   if(c=='\n')
     uart_putchar('\r',stream);

   loop_until_bit_is_set(UCSR0A,UDRE0);

   UDR0 = c;

   return 0;
 }

//------------------- Initial UART0 -----------------------

static void UART0_Init(void)
{

 UCSR0B = 0x00;     //disable while setting baud rate
 UBRR0L = UBRR;     //Set Baud Rate

 UCSR0C = 0x06;     //Set Frame format : 8data,NoneParity,1stop bit
 UCSR0B = 0x98;     //Uart0 Control Enable Reciver and transmitter

 stdout = &uart_str;     //Set address uart_str to stdout

}


void InitPWM()
{

 TCCR0A = (1<<COM0A1) |
    (0<<COM0A0) |
    (1<<COM0B1) |
    (0<<COM0B0) |
    (1<<WGM01)  |
    (1<<WGM00);
 TCCR0B = (0<<WGM02)  |
    (0<<CS02)   |
    (1<<CS01)  |
    (1<<CS00);

 OCR0A = 0x00;
 OCR0B = 0x00;

 PORTD &= ~(1<<PD5)&~(1<<PD6);
 PORTB &= ~(1<<PB3)&~(1<<PB4);
 
 DDRD |= (1 << PORTD5) | (1 << PORTD6);
    DDRB |= (1 << PORTB3) | (1 << PORTB4);
}

void UpdatePWM(unsigned char LeftPwmVal,unsigned char LeftDirection,unsigned char RightPwmVal,unsigned char RightDirection)
{    
 if(LeftPwmVal>100)
 {
  LeftPwmVal=255;
 }
 else
 {
  LeftPwmVal=LeftPwmVal*(255/100);
 }
 if(RightPwmVal>100)
 {
  RightPwmVal=255;
 }
 else
 {
  RightPwmVal=RightPwmVal*(255/100);
 }

 if(LeftDirection>0)
 {
  OCR0B = 255-LeftPwmVal;
  PORTB |= (1<<PB3);
 }
 else
 {
  OCR0B = LeftPwmVal;   
  PORTB &= ~(1<<PB3);
 }

 if(RightDirection>0)
 {
  OCR0A = 255-RightPwmVal;
  PORTB |= (1<<PB4);   
 }
 else
 {
  OCR0A = RightPwmVal;
  PORTB &= ~(1<<PB4);
 }
}


/************************************************************************
 *      Function Read XOUT,YOUT,ZOUT Rang OF Acceleration #MMA7331L     *
 ************************************************************************/

//---------------------------------------------------------- 
//---------------- READ Value XOUT ADC1(PF1)-------------
//---------------------------------------------------------- 
 
int Read_XOUT(void)
{
 int xout ;


 ADMUX   = 0xC5;  // [REFS1..0]:11 = Set Internal Ref. 1.1V , [Mux4..0]:00001 =Select Input Channel ADC1  
 ADCSRA  = 0x83;  // [ADEN]:1= ADC Enable ,[ADATE]:0= Auto triger Diable , [ADPS2..0]:011= Xtal/8


 ADCSRA |= 0x40;  // [ADSC]:1= ADC Start Conversion

 while(!(ADCSRA & 0x10)){;}    // [ADIF]:0 = Wait Conversion Completes[ADIF=1]
 
 xout = ADCW ;  // Read ADC 10-bit 

 return(xout);
}


//--------------------------------------------------------
//---------------- READ Value YOUT ADC2(PF2)-------------
//-------------------------------------------------------- 
 
 int Read_YOUT(void)
 {
 int yout ;

  
 ADMUX   = 0xC4             ;  // [REFS1..0]:11 = Set Internal Ref. 1.1V , [Mux4..0]:00010 =Select Input Channel ADC2  
 ADCSRA  = 0x83             ;  // [ADEN]:1= ADC Enable ,[ADATE]:0= Auto triger Diable , [ADPS2..0]:011= Xtal/8


 ADCSRA |= 0x40             ;  // [ADSC]:1= ADC Start Conversion

 while(!(ADCSRA & 0x10)){;}    // [ADIF]:0 = Wait Conversion Completes[ADIF=1]
     
 yout = ADCW                ;  // Read ADC 10-bit 


 return(yout)             ;
 }

 

//----------------------------------------------------------
//---------------- READ Value ZOUT ADC3(PF3) ---------------
//---------------------------------------------------------- 
 

int Read_ZOUT(void)
{
 int zout ;

 ADMUX   = 0xC3             ;  // [REFS1..0]:11 = Set Internal Ref. 1.1V , [Mux4..0]:00011 =Select Input Channel ADC3  
 ADCSRA  = 0x83             ;  // [ADEN]:1= ADC Enable ,[ADATE]:0= Auto triger Diable , [ADPS2..0]:011= Xtal/8


 ADCSRA |= 0x40             ;  // [ADSC]:1= ADC Start Conversion

 while(!(ADCSRA & 0x10)){;}    // [ADIF]:0 = Wait Conversion Completes[ADIF=1]

 zout = ADCW                ;  // Read ADC 10-bit 

 return(zout)               ;
}


int arctan2(int y, int x) {  // http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
   int coeff_1 = 180/4;         // angle in Quids (1024 Quids=360?)
   int coeff_2 = 3*coeff_1;
   float abs_y = abs(y)+1e-10; // kludge to prevent 0/0 condition
   float r, angle;
  
   if (x >= 0) {
     r = (x - abs_y) / (x + abs_y);
     angle = coeff_1 - coeff_1 * r;
   }  else {
     r = (x + abs_y) / (abs_y - x);
     angle = coeff_2 - coeff_1 * r;
   }
   if (y < 0)   
    return (int)(-angle);
   else            
    return (int)(angle);
} 

int getAccAngle(int z, int x) {                      
  return arctan2(-z, -x);    // in Quid: 1024/(2*PI))
}

/* ################################################################
   ##                                                                                                         ##
   ##                        Main Program                                                             ##
   ##                                                                                                         ##
   ################################################################ */
 
int main(void)
{  
   
    UART0_Init();
 InitPWM();
 UpdatePWM(50,1,0,1);
 _delay_ms(200);

 //----------------- Read and Print Value Gx,Gy,Gz 10-bit  Rang 4G -----------------

 printf("\n\r******* MMA7331L 3-Axis Accelerometer 10-bit ADC 4-G *********");

 while(1)
 {    

  Xdata = Read_XOUT()                      ; //  Read Value Gx
  //Ydata = Read_YOUT()                      ; //  Read Value Gy
  Zdata = Read_ZOUT()                      ; //  Read Value Gz


  //printf("G_x = %d  ",Xdata-512);   //  Print Value Gx  
  //printf("G_y = %d  ",Ydata-512);   // Print Value Gy
  //printf("G_z = %d  ",Zdata-512);   //  Print Value Gz

  AngleAcc = getAccAngle(Zdata-512, Xdata-512); //512, ((3.3v/2)/3.3v*1024) offset Accelerometer @3.3v
  printf("Deg = %d  ", AngleAcc);


  printf("\n\r");        //  Line Feed

  _delay_ms(150);

 } 


    return 0 ;
}


เขียนโดย Unknown ที่ 01:10 0 ความคิดเห็น
สมัครสมาชิก: ความคิดเห็น (Atom)