本人用arduino做单轴旋翼系统程序不太懂跪求大神详细解释！
本帖最后由 xmz198907 于 2013-5-29 20:36 编辑//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;
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;
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处理器，直接输出角度。
程序在哪里弄来的？
页:
[1]