本人用arduino做单轴旋翼系统程序不太懂跪求大神详细解释！

xmz198907

//http://scolton.blogspot.com/2010/07/arduino-interrupt-driven-servo-routine.html

#include <MsTimer2.h>
#include <Servo.h>

//MPU6050-----------------
#include "Wire.h"
#include "I2Cdev.h"
#include"MPU6050_6Axis_MotionApps20.h"
//--------------------

int ESC_U0=1000, ESC_DeltaU=0;     //������? ������, ����, 1000-2000us
const int ESC_PIN=9;   //
Servo ESC; // create servo object to control a servo

const int T=10;  //ms
int SET_POINT=0;                     //����ֵ
//const int KP_PIN=A0, TD_PIN=A2;
float ERROR;
float KP=0, TI=0, TD=0;      //PID����
int P_ITERM, D_ITERM, I_ITERM=0;      //ƫ��,�Լ������PID�������
unsigned long SAMPLES=0;               //�ڼ����������ڣ�����ʱ�䡢����
struct _STATE{
       floattheta;
       floatomiga;
}STATE={0,0};
float OMIGA_ZERO=1.4;
boolean RUN=false;

String inputString = "";         // a string to hold incoming data
boolean newLine = false;  // whether the string is complete
int status=0;

void setup()
{
       Serial.begin(115200);    // opens serial port, sets data rate to 9600bps

       //ʹ���ⲿ�ж�0
       mpu_setup();
       delay(20);      

       //cli();          // disable global interrupts
       ESC.attach(ESC_PIN);  // attaches the servo on pin 9 to the servoobject
       ESC.writeMicroseconds(ESC_U0);

       pinMode(9,OUTPUT);

       MsTimer2::set(T,pid); // ms, period
       MsTimer2::start();
       //sei();          // enable global interrupts
}

void loop()
{
       staticboolean led=false;

       struct_STATE state=mpu_get_theta();
       state.omiga-=OMIGA_ZERO;
       STATE=state;

       //   float k;
       //   k = analogRead(KP_PIN);
       //   KP=k*100/1024;
       //   k = analogRead(KD_PIN);
       //   KD=k*10/1024;

       staticunsigned int lastSamples=0;
       charstr[100];
       if(SAMPLES!=lastSamples){
              led=!led;
              if(led)
                     digitalWrite(9,HIGH);   // turn the LED on (HIGH is thevoltage level)
              else
                     digitalWrite(9,LOW);   // turn the LED on (HIGH is thevoltage level)

              lastSamples=SAMPLES;
              sprintf(str,"%ld,%d,%d,%d,%d,%d,%d,0",
                     SAMPLES,int(STATE.theta*10),int(ERROR*10),P_ITERM,D_ITERM,I_ITERM,ESC_DeltaU);
              Serial.print(str);
              Serial.write(0x0a);
       }
}

void pid()
{
       staticfloat lastERROR=0;
       staticunsigned long int lastTime=0;
       staticfloat interval;
       intoutput;
       longint now;

       sei();
       now= micros();
       interval= (now - lastTime)/1000000;
       //if(interval> 0.001){
              SAMPLES++;
              lastTime= now;

              ERROR= SET_POINT - STATE.theta;

              I_ITERM+=KP*(I_ITERM + interval * ERROR);
              //I_ITERM=constrain(I_ITERM,0,250);

              P_ITERM=KP* ERROR;
              D_ITERM=KP* TD * (ERROR - lastERROR)/interval;
              ESC_DeltaU= P_ITERM + D_ITERM+I_ITERM;
              output= ESC_DeltaU + ESC_U0;
              output= constrain(output,1000,2000);

              /*Remembersome variables for next time*/
              lastERROR= ERROR;
       //}
       if(RUN)
              ESC.writeMicroseconds(output);
       else
              ESC.writeMicroseconds(1000);
}

void serialEvent()
{
       charstr[100];

       if(Serial.available()){
              //get the new byte:
              charinChar = (char)Serial.read();
              //add it to the inputString:
              //if the incoming character is a newline, set a flag
              if((byte)inChar == 0x0a) {
                     inputString.toCharArray(str,100);
                     newLine= true;
              }else{
                     inputString+= inChar;
              }
       }
       if(newLine){
              newLine= false;
              switch(status){
              case0:
                     if(0==strcmp(str,"start")){
                            status=0;
                            RUN=true;
                     }elseif(0==strcmp(str,"stop")){
                            status=0;
                            RUN=false;
                     }elseif(0==strcmp(str,"u0")){
                            status=1;
                     }elseif(0==strcmp(str,"kp")){
                            status=2;
                     }elseif(0==strcmp(str,"td")){
                            status=3;
                     }elseif(0==strcmp(str,"ti")){
                            status=4;
                     }elseif(0==strcmp(str,"setpoint")){
                            status=5;
                     }
                     break;
              case1:
                     ESC_U0=atof(str);
                     status=0;
                     break;
              case2:
                     KP=atof(str);
                     status=0;
                     break;
              case3:
                     TD=atof(str);
                     status=0;
                     break;
              case4:
                     TI=atof(str);
                     status=0;
                     break;
              case5:
                     SET_POINT=atof(str);
                     status=0;
                     break;
              default:
                     break;
              }
              inputString="";
       }

}
我在做单轴旋翼控制系统但c语言程序不太懂，希望大神详细解释各个语句都什么意思？
所以器材如下：
控制器：arduino
电机：朗宇V2216 KV900无刷电机，好盈Flyfun 40A电调
传感器：MPU6050三轴加速度、陀螺传感器。内置DMP处理器，直接输出角度。

wenziheni

程序在哪里弄来的？