首页 › Arduino › 贡献我的四轴代码

camion 发表于 2013-8-20 17:17:30

贡献我的四轴代码

#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>

//=============mpu6050 declarations===========
MPU6050 accelgyro;
static double Gx_offset = -1.46;//soldered as LENGBEI
static double Gy_offset = -0.07;
static double Ax_offset = -0.21;
static double Ay_offset = -1.7;


int16_t ax,ay,az;
int16_t gx,gy,gz;//raw data from sensors
double aax,aay,aaz,ggx,ggy,ggz;
double Ax,Ay,Az;//单位 g(9.8m/s^2)
double Gx,Gy,Gz;//单位 °/s

//=================mesure declarations=============
double estima_X = 0.2;
double estima_Y = 0.2;
double es_GX;
double es_GY;
double fusion_angle_Y;
double fusion_angle_X;
double now_error_X, last_error_X, error_diff_X, error_int_X;
double now_error_Y, last_error_Y, error_diff_Y, error_int_Y;
double Angel_accX,Angel_accY,Angel_accZ;

//===============control & accuracy declarations========
double basic_power = 5;   // z acc balancing, need calibration each time
double rot_power_X, rot_power_Y;
double orientX=0;
double orientY=0;
long tic_1,tic_2,tic_3, duration;
int time_span_milli =20;
float time_span =0.02;
int moterPins;
int moterPowers;
   
void set_Motors(){
pinMode(11,OUTPUT);
pinMode(10,OUTPUT);
pinMode(9,OUTPUT);
pinMode(6,OUTPUT);
analogWrite(10, 0);
analogWrite(6, 0);
analogWrite(11,0);
analogWrite(9, 0);
delay(100);
}

void setup()
{
Wire.begin();
Serial.begin(9600);
set_Motors();
accelgyro.initialize();
delay(100);
autoCalibration();
delay(100);
}

void autoCalibration(){
Gx_offset=0.0;
Gy_offset=0.0;
double sum_1 =0;
double sum_2 =0;
double sum_3 =0;
double sum_4 =0;
for (int i=0; i<500; i++){
   accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
   ggx=gx/131.00;
   ggy=gy/131.00;
   Ax=ax/16384.00;
   Ay=ay/16384.00;
   Az=az/16384.00;
   Angel_accX =atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14 ;
   Angel_accY =-atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14;   
   sum_1 = sum_1 + ggx;
   sum_2 = sum_2 + ggy;
   sum_3 = sum_3 + Angel_accX;
   sum_4 = sum_4 + Angel_accY;
   delay(1);
}
Gx_offset = sum_1/500;
Gy_offset = sum_2/500;
Ax_offset = sum_3/500;
Ay_offset = sum_4/500;
//   Serial.print("Gx_offset= ");
//   Serial.print(sum_1/1000);
//   Serial.print("Gy_offset= ");
//   Serial.println(sum_2/1000);   
}



void loop()
{
   tic_1=millis();
   int emer_stat = 0;
   //------------------------command tests-----------------------------------
   while (Serial.available()>=1){
      char datatype = Serial.read();
      if (datatype == 'S'){
             digitalWrite(13, HIGH);   
             delay(1);               
             digitalWrite(13, LOW);   
             delay(1);
      }else if(datatype == 'M'){
            orientX=0;
            orientY=0;
      }else if(datatype == 'z'){
            orientX=0;
      }else if(datatype == 'y'){
            orientY=0;
      }else if(datatype == 'L'){
         basic_power = basic_power+10;
      }else if(datatype == 'R'){
         basic_power = basic_power-10;
      }else if(datatype == 'P'){
         basic_power = basic_power+1;
      }else if(datatype == 'K'){
         basic_power = basic_power-1;
      }else if(datatype == 'Q'){
         if (orientX <=18){
             orientX=orientX+2;
         }         
      }else if(datatype == 'W'){
         if (orientX >= -18){
             orientX=orientX-2;               
             digitalWrite(13, HIGH);   
         }
      }else if(datatype == 'D'){
         if (orientY <= 18){
             orientY=orientY+2;
         }         
      }else if(datatype == 'G'){
         if (orientY >=-18){
             orientY=orientY-2;
         }         
      }else if (datatype == 'V'){
             int value = Serial.parseInt();
             if ((value<=255)&&(value>=0)){
               basic_power = value;
             }
      }else if (datatype == 'X'){

      }else if (datatype == 'Y'){
      
      }else if (datatype == 'j'){
          orientY = 10;
      }else if (datatype == 'k'){
          orientY = 5;
      }else if (datatype == 'l'){
          orientY = -5;
      }else if (datatype == 'm'){
          orientY = -10;
      }else if (datatype == 'n'){
          orientX = -5;
      }else if (datatype == 'o'){
          orientX = 5;
      }else if (datatype == 'p'){
          orientX = 10;
      }else if (datatype == 'y'){
          orientY = 0;
      }else if (datatype == 'z'){
          orientX = 0;
      }   
   }
   
//--------------------------do NOT touch-----------------------------------------
   
   
// obtaining and filtering Acc angles   
   accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
   Ax=ax/16384.00;
   Ay=ay/16384.00;
   Az=az/16384.00;
   Angel_accX =atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14 -Ax_offset; //offset
   Angel_accY =-atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14 -Ay_offset; //offset
   estima_X = estima_cal(estima_X, Angel_accX, 0.2);
   estima_Y = estima_cal(estima_Y, Angel_accY, 0.2);   
//   Serial.print("estima_X=");
//   Serial.print(estima_X);
//   Serial.print("estima_Y=");
//   Serial.println(estima_Y);      
   
// obtaining Gyro angles   without filter simply because I'm lasy
   ggx=gx/131.00;
   ggy=gy/131.00;
   ggz=gz/131.00;
   Gx=Gx+(ggx-Gx_offset)*time_span;
   Gy=Gy+(ggy-Gy_offset)*time_span;
   autoCorrection();
//   Serial.print("Gx=");
//   Serial.print(Gx);
//   Serial.print(" Gy=");
//   Serial.println(Gy);


//fusion angles by combining 40% last observe + 30% Acc + 30% Gyro

fusion_angle_X = update_Estimat(fusion_angle_X, estima_X, Gx );
fusion_angle_Y = update_Estimat(fusion_angle_Y, estima_Y, Gy );
//   Serial.print("fusion_X=");
//   Serial.print(fusion_angle_X);
//   Serial.print("fusion_Y=");
//   Serial.println(fusion_angle_Y);   
   
//   if (fusion_angle_Y>=80){
//             digitalWrite(13, HIGH);   
//             delay(1);               
//             digitalWrite(13, LOW);   
//             delay(1);
//   }
   
//---------------------------------PID's----------------------------------------
uodate_error(orientX, orientY ,fusion_angle_X, fusion_angle_Y);

double command_Y_raw = PID_control(now_error_Y, error_diff_Y, error_int_Y);
double command_X_raw = PID_control(now_error_X, error_diff_X, error_int_X);
int command_X = int (command_X_raw);
int command_Y = int (command_Y_raw);

moterPowers =basic_power + command_X;
moterPowers =basic_power - command_X;
moterPowers =basic_power + command_Y;
moterPowers =basic_power - command_Y;
   
emergency_STOP(emer_stat);

//---------------------------------fuzzy's----------------------------------------
//   int command_X = fussy_command (fusion_angle_X);
//   int command_Y = fussy_command (fusion_angle_Y);
//moterPowers =basic_power + command_X + int(orientX);
//moterPowers =basic_power - command_X - int(orientX);
//moterPowers =basic_power + command_Y + int(orientY);
//moterPowers =basic_power - command_Y - int(orientY);


//----------------------executing state for time_span_milli------------------------

   for (int i=0; i<=3; i++){
   if (moterPowers< 0){
       moterPowers = 0;
   }else if (moterPowers >255){
       moterPowers = 255;
   }
   }
   
//   Serial.print("po 10 = ");
//   Serial.print(moterPowers);
//   Serial.print("   po 6 = ");
//   Serial.print(moterPowers);   
//   Serial.print("   po 11 = ");
//   Serial.print(moterPowers);
//   Serial.print("   po 9 = ");
//   Serial.println(moterPowers);
   
   analogWrite(10, moterPowers);
   analogWrite(6, moterPowers);
   analogWrite(11, moterPowers);
   analogWrite(9, moterPowers);

   tic_2=millis();
   duration = tic_2 - tic_1;
   if(duration > time_span_milli){
   duration=0;
   }
   //Serial.println(time_span_milli - duration);
   delay (time_span_milli - duration);
   
}


//--------------------------accesory functions----------------------------------------
double estima_cal (double estima, double metron, double KG){
double result1 = estima + KG*(metron - estima);
return result1;
}
double PID_control(double error, double error_diff, double error_int ){
double P=0.7;
double D=0.2;
double I=0;
double command = P*error + I*error_int + D*error_diff;
return command;
}
void uodate_error(double consigne_X, double consigne_Y, double mesure_X, double mesure_Y){
now_error_X = consigne_X - mesure_X;
now_error_Y = consigne_Y - mesure_Y;
error_diff_X = (now_error_X - last_error_X)/time_span;
error_diff_Y = (now_error_Y - last_error_Y)/time_span;
error_int_X = error_int_X + now_error_X*time_span;
error_int_Y = error_int_Y + now_error_Y*time_span;
last_error_X =now_error_X;
last_error_Y =now_error_Y;
}
double update_Estimat(double estimate, double metron_1, double metron_2){
double temp = 0.4* estimate + 0.3*metron_1 + 0.3*metron_2;
return temp;
}
void emergency_STOP(int choice){
if (choice == 0){
    //do nothing
}else if(choice == 1){
   //brutal descending
    basic_power =5;
//    moterPowers =5;
//    moterPowers =5;
//    moterPowers =5;
}else if(choice == 2){
    //douce descending
    moterPowers = moterPowers-10;
    moterPowers = moterPowers-10;
    moterPowers = moterPowers-10;
    moterPowers = moterPowers-10;
}
}

double PorportionDet(double limA, double limB, double outA, double outB, double x){
double a = (x - limB) / (limA-limB);
double result = a*outA + (1-a)*outB;
return result;
}

int fussy_command(double input){
   double PWM_command;
   if (input>=45 ){
   PWM_command = 30;
   }else if(input<45 & input>=25 ){
   PWM_command = PorportionDet(45, 25, 30, 10, input );
   }else if(input<25 & input>=5){
   PWM_command = PorportionDet(25, 5, 10, 3, input );   
   }else if(input<5 & input>=-5){
   PWM_command = PorportionDet(5, -5, 3, -3, input );         
   }else if(input<-5 &input>=-25){
   PWM_command = PorportionDet(-5, -25, -3, -10, input );            
   }else if(input<-25 & input>=-45){
   PWM_command = PorportionDet(-25, -45, -10, -30, input );         
   }else if(input<-45){
   PWM_command =-30;
   }
   int regu_pwm =int(PWM_command);
   return regu_pwm;
}
void autoCorrection (){
if ( Angel_accX > -1.3 & Angel_accX < 1.3 ){
    Gx=Angel_accX /2;
}
if ( Angel_accY > -1.3 & Angel_accY < 1.3 ){
    Gy=Angel_accY /2;
}
}



除了PID不太稳定，其他还好

camion 发表于 2013-8-20 17:19:43

/Users/samuel/Desktop/1.jpeg /Users/samuel/Desktop/2.jpg

FIGHT 发表于 2013-8-20 17:22:10

期待。谢谢分享。设备马上到齐了，调试下看看

学慧放弃 发表于 2013-8-20 20:15:22

谢谢楼主无私的共享，飞得平稳度怎样？？？

camion 发表于 2013-8-20 21:01:31

不好，容易晃，还需要细调PID参数

wing 发表于 2013-8-21 09:00:33

{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}{:soso_e179:}
我怎么看不见下面那两张图

tanbocandy 发表于 2013-12-4 20:50:45

这个要必须顶，有没有视频呢

18192908029 发表于 2014-9-24 20:21:35

感谢楼主！！能否附上说明呢，新手不会接线。。

努力微笑 发表于 2014-10-2 01:43:19

能飞不？，如果行，详细教程可以有

Super169 发表于 2014-10-2 12:19:06

感謝樓主無私分享.
可否提供一點硬件的資料, 以及接線方法, 好讓大家學習學習.

peterllloo 发表于 2015-2-8 12:22:27

感谢楼主分享的代码啊，我用LED灯测试成功了。不过连接电调似乎还要加上空占比发生程序，还有这个代码里面还没有滤波吧，出来的曲线跳动较大。

axjl_yd 发表于 2015-2-11 16:58:16

感谢分享，关注中

东尼菌 发表于 2015-7-30 10:50:24

赞一个，第一个帖子给你了

查看完整版本: 贡献我的四轴代码