#include <avr/io.h>   
#include <avr/interrupt.h>   
//-------------------------------------------------------------------
//Ŷ
int ADpin=0;
int IDGpin=1;


int pinI1=2;
int pinI2=4;
int pinI3=5;
int pinI4=7;
int speedpinEA=3;
int speedpinEB=6;

//-------------------------------------------------------------------
//
/*kalman*/
static const double C_0 = 1;
static const double Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.005;//ע⣺dtȡֵΪkalman˲ʱ
double P[2][2] = {{ 1, 0 },
                  { 0, 1 }};
double Pdot[4] ={ 0,0,0,0};
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
double angle, angle_dot;

int pwm;              //ռձ
int angle_0;          //ɼĻ
float a;              //ȻĽǶ  
float angle_dot_0;    //ɼĽٶ

int ADval;//ģŶȡֵ
int IDGval;
//-------------------------------------------------------------------
/* 
 * ʱжΪ1ms 
 */  
void init_time()  
{  
    TCCR2A |= (1 <<WGM21) | (1 << WGM20);  
    TCCR2B |= (1 << CS22 );  //by clk/64   
    TCCR2B &= ~((1 <<CS21) | (1 <<CS20));  //by clk/64   
    TCCR2B &= ~((1 << WGM21 ) | (1 << WGM20));  
    ASSR |= ( 1 << AS2 );  
    TIMSK2 |= ( 1 << TOIE2 ) | ( 0 << OCIE2B );  
    TCNT2 = 6;  
    sei();  
}  
//-------------------------------------------------------------------

void AD_calculate(void)
{
    
      ADval=analogRead(ADpin);   
      IDGval=analogRead(IDGpin);
        
        a=asin(0.028*ADval-8.596);          //򻯺ĻȻ
        angle_0=a*57.3-4.3;                 //ȻΪǶ
        angle_dot_0=739.2-2.4*IDGval;       //ٶȻ
        Kalman_Filter(angle_0,angle_dot_0); //˲

        //Serial.println(angle);  
        //Serial.println(angle_dot);   
 }
//-------------------------------------------------------------------
//˲

void Kalman_Filter(double angle_m,double gyro_m)   
{
  angle+=(gyro_m-q_bias) * dt;
  angle_err = angle_m - angle;
  Pdot[0]=Q_angle - P[0][1] - P[1][0];
  Pdot[1]=- P[1][1];
  Pdot[2]=- P[1][1];
  Pdot[3]=Q_gyro;
  P[0][0] += Pdot[0] * dt;
  P[0][1] += Pdot[1] * dt;
  P[1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  E = R_angle + C_0 * PCt_0;
  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;
  t_0 = PCt_0;
  t_1 = C_0 * P[0][1];
  P[0][0] -= K_0 * t_0;
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
  angle	+= K_0 * angle_err;  //ŽǶ
  q_bias += K_1 * angle_err;
  angle_dot = gyro_m-q_bias; //Žٶ
}
//-------------------------------------------------------------------
void pwm_calculate()
{
  pwm=angle*6+angle_dot*0.6516+pwm*1.066;   //PID
  if(pwm>255)                               //ֵСֵ
  pwm=255;
  if(pwm<-255)
  pwm=-255;
}
//-------------------------------------------------------------------
void set_motor()
{
  pwm_calculate();
  if (pwm>0)
  Advance(pwm+10);
  else if(pwm<0)
  {
  Back(pwm*(-1)+10);}
}

//-------------------------------------------------------------------
void setup()
{
  pinMode(pinI1,OUTPUT);
  pinMode(pinI2,OUTPUT);
  pinMode(pinI3,OUTPUT);
  pinMode(pinI4,OUTPUT);
  pinMode(speedpinEA,OUTPUT);
  pinMode(speedpinEB,OUTPUT);
  
  //Serial.begin(115200);  
    init_time();  //ʼʱ
  
}
//-------------------------------------------------------------------
void Advance(int speed)
{
  digitalWrite(pinI1,HIGH);
  digitalWrite(pinI3,HIGH);
  digitalWrite(pinI2,LOW);
  digitalWrite(pinI4,LOW);
  analogWrite(speedpinEA,speed);
  analogWrite(speedpinEB,speed);
  
}
//-------------------------------------------------------------------
void Back(int speed)
{
  digitalWrite(pinI1,LOW);
  digitalWrite(pinI3,LOW);
  digitalWrite(pinI2,HIGH);
  digitalWrite(pinI4,HIGH);
  analogWrite(speedpinEA,speed);
  analogWrite(speedpinEB,speed);
  
}

//-------------------------------------------------------------------
int count = 0;  
SIGNAL(SIG_OVERFLOW2)  
{  
    TCNT2 = 6;  
      
    ++count;  
    if( count == 10 )  //2msһжϷ޸Ĵ˴ýжʱ
    {   //жϷ
        
        AD_calculate();
        pwm_calculate();
        //set_motor();
       
       
       if (pwm>0)
       Advance(pwm);
       else if(pwm<0)
        {
          Back(pwm*(-1));
        }
          count=0; 
    } 
    
}  
//-------------------------------------------------------------------
void loop()
{
  //Serial.println("start ... ...");  
  while( 1 )  
    {   
    }  
 
  }
  
