API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Apr 12th, 2011
156
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 15.29 KB | None | 0 0
raw download clone embed print report
  1. /*****************************************************
  2. This program was produced by the
  3. CodeWizardAVR V1.25.9 Professional
  4. Automatic Program Generator
  5. © Copyright 1998-2008 Pavel Haiduc, HP InfoTech s.r.l.
  6. http://www.hpinfotech.com
  7.  
  8. Project :
  9. Version :
  10. Date    : 4/8/2011
  11. Author  : F4CG                            
  12. Company : F4CG                            
  13. Comments:
  14.  
  15.  
  16. Chip type           : ATmega16L
  17. Program type        : Application
  18. Clock frequency     : 1.000000 MHz
  19. Memory model        : Small
  20. External SRAM size  : 0
  21. Data Stack size     : 256
  22. *****************************************************/
  23.  
  24. #include <mega16.h>
  25. #include <stdlib.h>
  26. #include <delay.h>
  27. #include <stdio.h>
  28. #include <math.h>
  29.  
  30. // I2C Bus functions
  31. #asm
  32.    .equ __i2c_port=0x15 ;PORTC
  33.    .equ __sda_bit=1
  34.    .equ __scl_bit=0
  35. #endasm
  36. #include <i2c.h>
  37.  
  38. // Alphanumeric LCD Module functions
  39. #asm
  40.    .equ __lcd_port=0x18 ;PORTB
  41. #endasm
  42. #include <lcd.h>
  43.  
  44.  
  45. // Declare your global variables here
  46. char timer_0_over=0;
  47. int sensor_x_initial_value;
  48. int sensor_y_initial_value;
  49. int sensor_z_initial_value;
  50. float x_axis_degree_per_second;
  51. float y_axis_degree_per_second;
  52. float z_axis_degree_per_second;
  53. char buffer[6];  
  54. float x_angle_gyro;
  55. float y_angle_gyro;
  56. float z_angle_gyro;
  57. unsigned long int cycle;
  58. int x_value_ADC_value_from_accelerometer;
  59. int y_value_ADC_value_from_accelerometer;
  60. int z_value_ADC_value_from_accelerometer;
  61. float angle_x_from_accelerometer;
  62. float angle_y_from_accelerometer;
  63. float angle_z_from_accelerometer;
  64.  
  65.  
  66.  
  67.  
  68. // Timer 0 overflow interrupt service routine
  69. interrupt [TIM0_OVF] void timer0_ovf_isr(void)
  70. {
  71. timer_0_over++;
  72.  
  73. }
  74.  
  75.  
  76.  
  77.  
  78. void getting_initails_from_gyro(void){
  79.  
  80. int i2c_reading=0;
  81. unsigned char counter;
  82. int sensor_x_digitalvalue=0;
  83. int sensor_y_digitalvalue=0;
  84. int sensor_z_digitalvalue=0;
  85. float average_x;
  86. float average_y;
  87. float average_z;
  88.  
  89.  
  90.  
  91. for(counter=0;counter<=100;counter++){
  92.       //finding X initial
  93.       ///////////////////////////////////////////////////////////////////////////////
  94.       i2c_start();
  95.       i2c_write(0xD0);
  96.       i2c_write(0x1D);
  97.       i2c_start();
  98.       i2c_write(0xD1);
  99.       i2c_reading=i2c_read(0);
  100.       i2c_stop();
  101.       sensor_x_digitalvalue=i2c_reading<<8;
  102.       i2c_start();
  103.       i2c_write(0xD0);
  104.       i2c_write(0x1E);
  105.       i2c_start();
  106.       i2c_write(0xD1);
  107.       i2c_reading=i2c_read(0);
  108.       i2c_stop();
  109.       sensor_x_digitalvalue = sensor_x_digitalvalue | i2c_reading;
  110.       average_x = ((average_x * counter) + sensor_x_digitalvalue)/(counter+1);
  111.  
  112.       ///////////////////////////////////////////////////////////////////////////////
  113.      
  114.      
  115.       //finding Y initial
  116.       ///////////////////////////////////////////////////////////////////////////////
  117.       i2c_start();
  118.       i2c_write(0xD0);
  119.       i2c_write(0x1F);
  120.       i2c_start();
  121.       i2c_write(0xD1);
  122.       i2c_reading=i2c_read(0);
  123.       i2c_stop();
  124.       sensor_y_digitalvalue=i2c_reading<<8;
  125.       i2c_start();
  126.       i2c_write(0xD0);
  127.       i2c_write(0x20);
  128.       i2c_start();
  129.       i2c_write(0xD1);
  130.       i2c_reading=i2c_read(0);
  131.       i2c_stop();
  132.       sensor_y_digitalvalue = sensor_y_digitalvalue | i2c_reading;
  133.       average_y =((average_y * counter) + sensor_y_digitalvalue)/(counter+1);
  134.       ///////////////////////////////////////////////////////////////////////////////
  135.      
  136.      
  137.       //finding Z initial
  138.       ///////////////////////////////////////////////////////////////////////////////
  139.       i2c_start();
  140.       i2c_write(0xD0);
  141.       i2c_write(0x21);
  142.       i2c_start();
  143.       i2c_write(0xD1);
  144.       i2c_reading=i2c_read(0);
  145.       i2c_stop();
  146.       sensor_z_digitalvalue=i2c_reading<<8;
  147.       i2c_start();
  148.       i2c_write(0xD0);
  149.       i2c_write(0x22);
  150.       i2c_start();
  151.       i2c_write(0xD1);
  152.       i2c_reading=i2c_read(0);
  153.       i2c_stop();
  154.       sensor_z_digitalvalue = sensor_z_digitalvalue | i2c_reading;
  155.       average_z = ((average_z * counter) + sensor_z_digitalvalue)/(counter+1);
  156.       ///////////////////////////////////////////////////////////////////////////////
  157.       }  
  158.      
  159.       if(average_x - floor(average_x)>=0.5){
  160.       sensor_x_initial_value= ceil(average_x);
  161.       }
  162.       else{
  163.       sensor_x_initial_value= floor(average_x);
  164.       }
  165.      
  166.       if(average_y - floor(average_y)>=0.5){
  167.       sensor_y_initial_value= ceil(average_y);
  168.       }
  169.       else{
  170.       sensor_y_initial_value= floor(average_y);
  171.       }
  172.  
  173.       if(average_z - floor(average_z)>=0.5){
  174.       sensor_z_initial_value= ceil(average_z);
  175.       }
  176.       else{
  177.       sensor_z_initial_value= floor(average_z);
  178.       }
  179.      
  180.      
  181.          
  182.       }
  183.        
  184. void reading_data_from_gyro(void){
  185.  
  186. int i2c_reading=0;
  187. int sensor_x_digitalvalue=0;
  188. int sensor_y_digitalvalue=0;
  189. int sensor_z_digitalvalue=0;
  190. float scale_factor=14.375;
  191.      
  192.       TCCR0=0x01;
  193.       //X axis reading
  194.       ////////////////////////////////////////////////////////////    
  195.       i2c_start();
  196.       i2c_write(0xD0);
  197.       i2c_write(0x1D);
  198.       i2c_start();
  199.       i2c_write(0xD1);
  200.       i2c_reading=i2c_read(0);
  201.       i2c_stop();
  202.       sensor_x_digitalvalue=i2c_reading<<8;
  203.       i2c_start();
  204.       i2c_write(0xD0);
  205.       i2c_write(0x1E);
  206.       i2c_start();
  207.       i2c_write(0xD1);
  208.       i2c_reading=i2c_read(0);
  209.       i2c_stop();
  210.       sensor_x_digitalvalue = sensor_x_digitalvalue + i2c_reading;
  211.       x_axis_degree_per_second = (sensor_x_digitalvalue - sensor_x_initial_value)/scale_factor;
  212.       ////////////////////////////////////////////////////////////
  213.      
  214.      
  215.       //Y axis reading
  216.       ////////////////////////////////////////////////////////////
  217.       i2c_start();
  218.       i2c_write(0xD0);
  219.       i2c_write(0x1F);
  220.       i2c_start();
  221.       i2c_write(0xD1);
  222.       i2c_reading=i2c_read(0);
  223.       i2c_stop();
  224.       sensor_y_digitalvalue=i2c_reading<<8;
  225.       i2c_start();
  226.       i2c_write(0xD0);
  227.       i2c_write(0x20);
  228.       i2c_start();
  229.       i2c_write(0xD1);
  230.       i2c_reading=i2c_read(0);
  231.       i2c_stop();
  232.       sensor_y_digitalvalue = sensor_y_digitalvalue + i2c_reading;
  233.       y_axis_degree_per_second = (sensor_y_digitalvalue - sensor_y_initial_value)/scale_factor;
  234.       ////////////////////////////////////////////////////////////
  235.      
  236.      
  237.       //Z axis reading
  238.       ////////////////////////////////////////////////////////////
  239.       i2c_start();
  240.       i2c_write(0xD0);
  241.       i2c_write(0x21);
  242.       i2c_start();
  243.       i2c_write(0xD1);
  244.       i2c_reading=i2c_read(0);
  245.       i2c_stop();
  246.       sensor_z_digitalvalue=i2c_reading<<8;
  247.       i2c_start();
  248.       i2c_write(0xD0);
  249.       i2c_write(0x22);
  250.       i2c_start();
  251.       i2c_write(0xD1);
  252.       i2c_reading=i2c_read(0);
  253.       i2c_stop();
  254.       sensor_z_digitalvalue = sensor_z_digitalvalue + i2c_reading;
  255.       z_axis_degree_per_second = (sensor_z_digitalvalue - sensor_z_initial_value)/scale_factor;
  256.       ////////////////////////////////////////////////////////////
  257.      
  258.      
  259. }
  260.  
  261.  
  262. void gyro_angle_calculation(void){
  263.      
  264.       cycle= TCNT0 + timer_0_over*256;
  265.       timer_0_over=0;
  266.       TCNT0=0;
  267.       x_angle_gyro=x_angle_gyro + x_axis_degree_per_second*cycle/1000000;
  268.       y_angle_gyro=y_angle_gyro + y_axis_degree_per_second*cycle/1000000;
  269.       z_angle_gyro=z_angle_gyro + z_axis_degree_per_second*cycle/1000000;
  270.      
  271.      
  272. }
  273.  
  274.  
  275.  
  276.  
  277.  
  278. void start_with_accelerometer(void){
  279.       i2c_start();
  280.       i2c_write(0xA6);
  281.       i2c_write(0x2D);
  282.       i2c_write(0x00);
  283.       i2c_stop();
  284.       i2c_start();
  285.       i2c_write(0xA6);
  286.       i2c_write(0x31);
  287.       i2c_write(0x01);
  288.       i2c_stop();
  289.       i2c_start();
  290.       i2c_write(0xA6);
  291.       i2c_write(0x2D);
  292.       i2c_write(0x08);
  293.       i2c_stop();
  294. }
  295.  
  296.  
  297. void measuring_acceleration(void)
  298. {    
  299.       signed char i;
  300.       int digital_values_from_sensor[6];
  301.       signed char x_axis_0g_point=1;
  302.       signed char y_axis_0g_point=1;
  303.       signed char z_axis_0g_point = -8;
  304.      
  305.       //i2c reading
  306.       i2c_start();
  307.       i2c_write(0xA6);
  308.       i2c_write(0x32);
  309.       i2c_start();
  310.       i2c_write(0xA7);
  311.       for (i=0;i<5;i++){
  312.       digital_values_from_sensor[i]=i2c_read(1);
  313.       }
  314.       digital_values_from_sensor[i]=i2c_read(0);
  315.       i2c_stop();
  316.       //i2c reading is done
  317.  
  318.         //comining LSB and HSB
  319.       x_value_ADC_value_from_accelerometer = (digital_values_from_sensor[1]<<8) |  digital_values_from_sensor[0];
  320.       y_value_ADC_value_from_accelerometer = (digital_values_from_sensor[3]<<8) |  digital_values_from_sensor[2];
  321.       z_value_ADC_value_from_accelerometer = (digital_values_from_sensor[5]<<8) |  digital_values_from_sensor[4];
  322.  
  323.      
  324.       //giving initial values
  325.       x_value_ADC_value_from_accelerometer -= x_axis_0g_point;
  326.       y_value_ADC_value_from_accelerometer -= y_axis_0g_point;
  327.       z_value_ADC_value_from_accelerometer = z_value_ADC_value_from_accelerometer - z_axis_0g_point;
  328.        
  329.  
  330.      
  331. }
  332.  
  333. void process_on_accelerometer_date(void){
  334.         int x_value_signed;
  335.         int y_value_signed;
  336.         int z_value_signed;
  337.         float x_scale=7.6;
  338.         float y_scale=7.58;
  339.         float z_scale=7.93;
  340.         signed char LSb_for_2complement;
  341.         int the_value_for_finding_2complement=1024;
  342.         int magnetude_of_acceleration_resulten;
  343.         float sensor_reading_in_g_for_x;
  344.         float sensor_reading_in_g_for_y;
  345.         float sensor_reading_in_g_for_z;
  346.         float angle_reading_in_radian;
  347.        
  348.          
  349.         ////////////////////////////////////////////////////////
  350.         LSb_for_2complement = x_value_ADC_value_from_accelerometer>>9;
  351.         if(LSb_for_2complement==1){
  352.         x_value_signed = (x_value_ADC_value_from_accelerometer - the_value_for_finding_2complement)*x_scale;
  353.         }
  354.         else
  355.         {
  356.         x_value_signed = x_value_ADC_value_from_accelerometer * x_scale;
  357.         }
  358.         ///////////////////////////////////////////////////////
  359.         LSb_for_2complement = y_value_ADC_value_from_accelerometer>>9;
  360.         if(LSb_for_2complement==1){
  361.         y_value_signed = (y_value_ADC_value_from_accelerometer - the_value_for_finding_2complement)*y_scale;
  362.         }
  363.         else
  364.         {
  365.         y_value_signed = y_value_ADC_value_from_accelerometer * y_scale;
  366.         }
  367.         ///////////////////////////////////////////////////////
  368.         LSb_for_2complement = z_value_ADC_value_from_accelerometer>>9;
  369.         if(LSb_for_2complement==1){
  370.         z_value_signed = (z_value_ADC_value_from_accelerometer - the_value_for_finding_2complement)*z_scale;
  371.         }
  372.         else
  373.         {
  374.         z_value_signed = z_value_ADC_value_from_accelerometer * z_scale;
  375.         }
  376.         /////////////////////////////////////////////////////////
  377.         sensor_reading_in_g_for_x = (float)x_value_signed/1000;   //conversion for x,y,z to g
  378.         sensor_reading_in_g_for_y = (float)y_value_signed/1000;
  379.         sensor_reading_in_g_for_z=(float)z_value_signed/1000;
  380.        
  381.         magnetude_of_acceleration_resulten = 1000*sqrt((sensor_reading_in_g_for_x * sensor_reading_in_g_for_x) + (sensor_reading_in_g_for_y * sensor_reading_in_g_for_y) + (sensor_reading_in_g_for_z * sensor_reading_in_g_for_z)); //unclear why multiply by 400 ???
  382.        
  383.        
  384.         //finding angle for xy plane
  385.         angle_reading_in_radian = acos( (float)x_value_signed / magnetude_of_acceleration_resulten );
  386.         angle_x_from_accelerometer =  (angle_reading_in_radian*180/PI);
  387.         //finding angle for xz plane
  388.         angle_reading_in_radian = acos( (float)y_value_signed / magnetude_of_acceleration_resulten );
  389.         angle_y_from_accelerometer =  (angle_reading_in_radian*180/PI);
  390.         //finding angle for yz plane
  391.         angle_reading_in_radian = acos( (float)z_value_signed / magnetude_of_acceleration_resulten );
  392.         angle_z_from_accelerometer =  (angle_reading_in_radian*180/PI);
  393.         }
  394.  
  395.  
  396. void main(void)
  397. {
  398. // Declare your local variables here
  399.  
  400. // Input/Output Ports initialization
  401. // Port A initialization
  402. // Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
  403. // State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
  404. PORTA=0x00;
  405. DDRA=0x00;
  406.  
  407. // Port B initialization
  408. // Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
  409. // State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
  410. PORTB=0x00;
  411. DDRB=0x00;
  412.  
  413. // Port C initialization
  414. // Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
  415. // State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
  416. PORTC=0x00;
  417. DDRC=0x00;
  418.  
  419. // Port D initialization
  420. // Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
  421. // State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
  422. PORTD=0x00;
  423. DDRD=0x00;
  424.  
  425. // Timer/Counter 0 initialization
  426. // Clock source: System Clock
  427. // Clock value: 1000.000 kHz
  428. // Mode: Normal top=FFh
  429. // OC0 output: Disconnected
  430. TCCR0=0x00;
  431. TCNT0=0x00;
  432. OCR0=0x00;
  433.  
  434.  
  435. // Timer/Counter 1 initialization
  436. // Clock source: System Clock
  437. // Clock value: Timer 1 Stopped
  438. // Mode: Normal top=FFFFh
  439. // OC1A output: Discon.
  440. // OC1B output: Discon.
  441. // Noise Canceler: Off
  442. // Input Capture on Falling Edge
  443. // Timer 1 Overflow Interrupt: Off
  444. // Input Capture Interrupt: Off
  445. // Compare A Match Interrupt: Off
  446. // Compare B Match Interrupt: Off
  447. TCCR1A=0x00;
  448. TCCR1B=0x00;
  449. TCNT1H=0x00;
  450. TCNT1L=0x00;
  451. ICR1H=0x00;
  452. ICR1L=0x00;
  453. OCR1AH=0x00;
  454. OCR1AL=0x00;
  455. OCR1BH=0x00;
  456. OCR1BL=0x00;
  457.  
  458. // Timer/Counter 2 initialization
  459. // Clock source: System Clock
  460. // Clock value: Timer 2 Stopped
  461. // Mode: Normal top=FFh
  462. // OC2 output: Disconnected
  463. ASSR=0x00;
  464. TCCR2=0x00;
  465. TCNT2=0x00;
  466. OCR2=0x00;
  467.  
  468. // External Interrupt(s) initialization
  469. // INT0: Off
  470. // INT1: Off
  471. // INT2: Off
  472. MCUCR=0x00;
  473. MCUCSR=0x00;
  474.  
  475. // Timer(s)/Counter(s) Interrupt(s) initialization
  476. TIMSK=0x01;
  477.  
  478. // Analog Comparator initialization
  479. // Analog Comparator: Off
  480. // Analog Comparator Input Capture by Timer/Counter 1: Off
  481. ACSR=0x80;
  482. SFIOR=0x00;
  483.  
  484. // I2C Bus initialization
  485. i2c_init();
  486.  
  487. // LCD module initialization
  488. lcd_init(16);
  489.  
  490. // Global enable interrupts
  491. #asm("sei")
  492.  
  493. while (1)
  494.       {
  495.       getting_initails_from_gyro();
  496.       start_with_accelerometer();
  497.      
  498.       while(1){
  499.       measuring_acceleration();
  500.       process_on_accelerometer_date();
  501.       reading_data_from_gyro();
  502.       gyro_angle_calculation();      
  503.        
  504.        
  505.        
  506.        
  507.        
  508.        _lcd_ready();
  509.       lcd_clear();
  510.       //show X  
  511.       lcd_gotoxy(0,0);
  512.       lcd_putsf("X");                  
  513.       itoa(angle_x_from_accelerometer,buffer);
  514.       lcd_puts(buffer);
  515.       lcd_gotoxy(0,1);
  516.       lcd_putsf("X");                  
  517.       itoa(x_angle_gyro,buffer);
  518.       lcd_puts(buffer);
  519.    
  520.      
  521.       //show Y
  522.       lcd_gotoxy(5,0);
  523.       lcd_putsf("Y");
  524.       itoa(angle_y_from_accelerometer,buffer);
  525.       lcd_puts(buffer);
  526.       lcd_gotoxy(5,1);
  527.       lcd_putsf("Y");
  528.       itoa(y_angle_gyro,buffer);
  529.       lcd_puts(buffer);
  530.      
  531.      
  532.       //show Z
  533.       lcd_gotoxy(10,0);
  534.       lcd_putsf("Z");
  535.       itoa(angle_z_from_accelerometer,buffer);
  536.       lcd_puts(buffer);
  537.       lcd_gotoxy(10,1);
  538.       lcd_putsf("Z");
  539.       itoa(z_angle_gyro,buffer);
  540.       lcd_puts(buffer);
  541.      
  542.       }
  543.                
  544.       };
  545. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!