Facebook
From Fiery Iguana, 6 Years ago, written in C.
Embed
Download Paste or View Raw
Hits: 276
  1. int main()
  2. {
  3.         char buffer[20], float_[10];
  4.         float Xa,Ya,Za,t;
  5.         float Xg=0,Yg=0,Zg=0, kalPitch, kalRoll, kalYaw;
  6.         I2C_Init();                                                                                     /* Initialize I2C */
  7.         Gyro_Init();                                                                            /* Initialize Gyro */
  8.         USART_Init(9600);                                                                       /* Initialize USART with 9600 baud rate */
  9.         calman_init_roll();
  10.         calman_init_pitch();
  11.         timer_init();
  12.         servo_angle(90);
  13.         servo_angle_2(90);
  14.        
  15.         DDRD = 0xff;
  16.        
  17.         while(1)
  18.         {
  19.                 Read_RawValue();
  20.  
  21.                 Xa = Acc_x/16384.0;                                                             /* Divide raw value by sensitivity scale factor to get real values */
  22.                 Ya = Acc_y/16384.0;
  23.                 Za = Acc_z/16384.0;
  24.                
  25.                 Xg = Gyro_x/16.4;
  26.                 Yg = Gyro_y/16.4;
  27.                 Zg = Gyro_z/16.4;
  28.  
  29.                 t = (Temperature/340.00)+36.53;                                 /* Convert temperature in °/c using formula */
  30.  
  31.                 arx = (180/3.141592) * atan(Xa / sqrt(square(Ya) + square(Za)));  //pitch
  32.                 ary = (180/3.141592) * atan(Ya / sqrt(square(Xa) + square(Za)));        //roll
  33.                 arz = (180/3.141592) * atan(sqrt(square(Ya) + square(Xa)) / Za);    //yaw
  34.                  
  35.                 grx = grx + (0.047 * Xg);
  36.                 gry = gry + (0.047 * Yg);
  37.                 grz = grz + (0.047 * Zg);
  38.                
  39.                 kalPitch = calman_filtr_pitch(arx, gry);
  40.                 kalRoll = calman_filtr_roll(ary, grx);
  41.                
  42.                
  43.                 if(kalRoll < 3 && kalRoll > -3) {
  44.                         servo_angle(90);
  45.                         servo_angle_2(-90);
  46.                 } else if(kalRoll > 3) {
  47.                         servo_angle(90-kalRoll);
  48.                         servo_angle_2(-90+kalRoll);
  49.                 } else if(kalRoll < -3) {
  50.                         servo_angle(90-(kalRoll));
  51.                         servo_angle_2(-90+(kalRoll));
  52.                 }
  53.                
  54.                
  55.                 /*dtostrf( arx, 3, 2, float_ );
  56.                 sprintf(buffer," AP = %s o\t",float_);
  57.                 USART_SendString(buffer);
  58.  
  59.                 dtostrf( ary, 3, 2, float_ );
  60.                 sprintf(buffer," AR = %s o\t",float_);
  61.                 USART_SendString(buffer);
  62.                
  63.                 dtostrf( grx, 3, 2, float_ );
  64.                 sprintf(buffer," Gx = %s o\t",float_);
  65.                 USART_SendString(buffer);
  66.  
  67.                 dtostrf( gry, 3, 2, float_ );
  68.                 sprintf(buffer," Gy = %s o\t",float_,0xF8);
  69.                 USART_SendString(buffer);
  70.  
  71.                 dtostrf( kalPitch, 3, 2, float_ );
  72.                 sprintf(buffer," kP = %s o\t",float_,0xF8);
  73.                 USART_SendString(buffer);
  74.  
  75.                 dtostrf( kalRoll, 3, 2, float_ );
  76.                 sprintf(buffer," kR = %s 0\t\n",float_,0xF8);
  77.                 USART_SendString(buffer); */
  78.  
  79.                 /*dtostrf( Xa, 3, 2, float_ );                                 
  80.                 sprintf(buffer," Ax = %s g\t",float_);
  81.                 USART_SendString(buffer);
  82.  
  83.                 dtostrf( Ya, 3, 2, float_ );
  84.                 sprintf(buffer," Ay = %s g\t",float_);
  85.                 USART_SendString(buffer);
  86.                
  87.                 dtostrf( Za, 3, 2, float_ );
  88.                 sprintf(buffer," Az = %s g\t",float_);
  89.                 USART_SendString(buffer);
  90.  
  91.                 dtostrf( t, 3, 2, float_ );
  92.                 sprintf(buffer," T = %s%cC\t",float_,0xF8);          
  93.                 USART_SendString(buffer);
  94.  
  95.                 dtostrf( Xg, 3, 2, float_ );
  96.                 sprintf(buffer," Gx = %s%c/s\t",float_,0xF8);
  97.                 USART_SendString(buffer);
  98.  
  99.                 dtostrf( Yg, 3, 2, float_ );
  100.                 sprintf(buffer," Gy = %s%c/s\t",float_,0xF8);
  101.                 USART_SendString(buffer);
  102.                
  103.                 dtostrf( Zg, 3, 2, float_ );
  104.                 sprintf(buffer," Gz = %s%c/s\r\n",float_,0xF8);
  105.                 USART_SendString(buffer); */
  106.                
  107.                 /*dtostrf( grx, 3, 2, float_ );
  108.                 sprintf(buffer," Grx = %s%c/s\t",float_,0xF8);
  109.                 USART_SendString(buffer);
  110.  
  111.                 dtostrf( gry, 3, 2, float_ );
  112.                 sprintf(buffer," Gry = %s%c/s\t",float_,0xF8);
  113.                 USART_SendString(buffer);
  114.                
  115.                 dtostrf( grz, 3, 2, float_ );
  116.                 sprintf(buffer," Grz = %s%c/s\r",float_,0xF8);
  117.                 USART_SendString(buffer);
  118.                
  119.                 dtostrf( arx, 3, 2, float_ );
  120.                 sprintf(buffer," Arx = %s%c/s\t",float_,0xF8);
  121.                 USART_SendString(buffer);
  122.  
  123.                 dtostrf( ary, 3, 2, float_ );
  124.                 sprintf(buffer," Ary = %s%c/s\t",float_,0xF8);
  125.                 USART_SendString(buffer);
  126.                
  127.                 dtostrf( arz, 3, 2, float_ );
  128.                 sprintf(buffer," Arz = %s%c/s\r\n",float_,0xF8);
  129.                 USART_SendString(buffer); */
  130.         }
  131. }