四周上电后 电机可以转动，四轴进行各个角度转动时，四个电机能进行反应。会做出调整。但是如果四轴往一个方向转动过大，有一个电机就不转了。 这时测pwm波，输出的是正常的波。 大神们是不是电调坏了。参考程序如下；oid CONTROL(float rol, float pit, float yaw)
{
static float roll_old,pitch_old;
if(ctrl.ctrlRate >= 2) //ÄÚ»·½øÐÐ2´Î¿ØÖÆ Íâ»·½øÐÐ1´Î¿ØÖÆ ÄÚ»·¿ØÖÆƵÂÊΪÍâ»·µÄ2±¶
{
//*****************Íâ»·PID**************************//
//¸©Ñö¼ÆËã//
pit=pit + 0.03;
ctrl.pitch.shell.increment += pit; //¸©Ñö·½ÏòÎó²î»ý·Ö
//»ý·ÖÏÞ·ù
if(ctrl.pitch.shell.increment > ctrl.pitch.shell.increment_max)
ctrl.pitch.shell.increment = ctrl.pitch.shell.increment_max;
else if(ctrl.pitch.shell.increment < -ctrl.pitch.shell.increment_max)
ctrl.pitch.shell.increment = -ctrl.pitch.shell.increment_max;
ctrl.pitch.shell.pid_out = ctrl.pitch.shell.kp * pit + ctrl.pitch.shell.ki * ctrl.pitch.shell.increment + ctrl.pitch.shell.kd * (pit - pitch_old);
pitch_old = pit; //´¢´æ ¸©ÑöÆ«²î
//ºá¹ö¼ÆËã//
rol=rol + 0.2;
ctrl.roll.shell.increment += rol; //ºá¹ö·½ÏòÎó²î»ý·Ö
//»ý·ÖÏÞ·ù
if(ctrl.roll.shell.increment > ctrl.roll.shell.increment_max)
ctrl.roll.shell.increment = ctrl.roll.shell.increment_max;
else if(ctrl.roll.shell.increment < -ctrl.roll.shell.increment_max)
ctrl.roll.shell.increment = -ctrl.roll.shell.increment_max;
ctrl.roll.shell.pid_out = ctrl.roll.shell.kp * rol + ctrl.roll.shell.ki * ctrl.roll.shell.increment + ctrl.roll.shell.kd * (rol - roll_old);
roll_old = rol; //´¢´æ ºá¹öÆ«²î
//º½Ïò¼ÆËã////////////
ctrl.yaw.shell.pid_out = ctrl.yaw.shell.kp * (Rc_Data.YAW - Rc_Data.yaw_offset)/10 + ctrl.yaw.shell.kd * sensor.gyro.origin.z;
ctrl.ctrlRate = 0;
}
ctrl.ctrlRate ++;
//********************ÄÚ»·(½ÇËٶȻ·)PD*********************************//
ctrl.roll.core.kp_out = ctrl.roll.core.kp * (ctrl.roll.shell.pid_out + sensor.gyro.radian.y * RtA);
ctrl.roll.core.kd_out = ctrl.roll.core.kd * (sensor.gyro.origin.y - sensor.gyro.histor.y);
ctrl.pitch.core.kp_out = ctrl.pitch.core.kp * (ctrl.pitch.shell.pid_out + sensor.gyro.radian.x * RtA);
ctrl.pitch.core.kd_out = ctrl.pitch.core.kd * (sensor.gyro.origin.x - sensor.gyro.histor.x);
ctrl.yaw.core.kp_out = ctrl.yaw.core.kp * (ctrl.yaw.shell.pid_out + sensor.gyro.radian.z * RtA);
ctrl.yaw.core.kd_out = ctrl.yaw.core.kd * (sensor.gyro.origin.z - sensor.gyro.histor.z);
ctrl.roll.core.pid_out = ctrl.roll.core.kp_out + ctrl.roll.core.kd_out;
ctrl.pitch.core.pid_out = ctrl.pitch.core.kp_out + ctrl.pitch.core.kd_out;
ctrl.yaw.core.pid_out = ctrl.yaw.core.kp_out + ctrl.yaw.core.kd_out;
sensor.gyro.histor.x = sensor.gyro.origin.x; //´¢´æÍÓÂÝÒÇ£¨½ÇËٶȣ©ÀúÊ·Öµ
sensor.gyro.histor.y = sensor.gyro.origin.y;
sensor.gyro.histor.z = sensor.gyro.origin.z;
if(Rc_Data.THROTTLE>1100) //ÓÍÃÅ
{
int date_THROTTLE = Rc_Data.THROTTLE;///cos(angle.roll/57.3)/cos(angle.pitch/57.3); //ÓÍÃÅÇã½Ç²¹³¥£¬·ÀÖ¹ÒòÇãб¸ß¶ÈϽµÌ«¿ì
/* ¿ØÖƲÉÓÃXģʽ */
/* 1 4 */
/* \ / */
/* \ / */
/* / \ */
/* / \ */
/* 2 3 */
/* 1:Moto_duty[0] 2:Moto_duty[1] */
/* 3:Moto_duty[2] 4:Moto_duty[3] */
//
Moto_duty[0] = date_THROTTLE - 400 + ctrl.roll.core.pid_out + ctrl.pitch.core.pid_out;// +//ctrl.yaw.core.pid_out;
Moto_duty[1] = date_THROTTLE - 400 + ctrl.pitch.core.pid_out - ctrl.roll.core.pid_out;
Moto_duty[2] = date_THROTTLE - 400 - ctrl.roll.core.pid_out - ctrl.pitch.core.pid_out; //-ctrl.yaw.core.pid_out;
Moto_duty[3] = date_THROTTLE - 400 - ctrl.pitch.core.pid_out + ctrl.roll.core.pid_out ;//id_out;
if(Moto_duty[0]<=1400) Moto_duty[0] = 1400;
if(Moto_duty[1]<=1400) Moto_duty[1] = 1400;
if(Moto_duty[2]<=1400) Moto_duty[2] = 1400;
if(Moto_duty[3]<=1400) Moto_duty[3] = 1400;
}
else
{
Moto_duty[0] = Moto_duty[1] = Moto_duty[2] = Moto_duty[3] = 0;
ctrl.pitch.shell.increment = 0;
ctrl.roll.shell.increment = 0;
}
if(ARMED)
Moto_PwmRflash(Moto_duty[0],Moto_duty[1],Moto_duty[2],Moto_duty[3]);
else Moto_PwmRflash(0,0,0,0);
}
/* ÓÍÃÅÐγÉÉèÖà */
void ALGH_set(void)
{
static vs8 flag=1;
static vs16 time=0;
while(flag)
{
/* ÓÍÃÅÐγÉÉèÖà */
/* ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª */
/* | | | | | */
/* | | | | | */
/* | | | \ | */
/* | | | \ | */
/* ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª */
/* ÓÍÃÅÀµ½×î¸ß Ò¡¸ËÍƵ½ÓÒÏÂ½Ç */
if(Rc_Data.ROLL >= 1750 && Rc_Data.PITCH >= 1750)
{
// if(Rc_Data.THROTTLE <= 2500 && Rc_Data.THROTTLE >= 900)
// {
// flag=0;
// }
Moto_PwmRflash(Rc_Data.THROTTLE-750 ,Rc_Data.THROTTLE -750,Rc_Data.THROTTLE -750,Rc_Data.THROTTLE-750);
time=0;
}
else time++;
if(time>=30000) flag=0;
//LED3(ON);
}
//Moto_PwmRflash(0,0,0,0);
//LED3(OFF);
}
// /* ÉÏËø&½âËøº¯Êý */
// void Deblocking(void)
// {
// static vs8 flag=1;
// static vs16 time1=0,time2=0;
// /* Ò£¿Ø½âËø */
// /* ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª */
// /* | | | \ | */
// /* | | | \ | */
// /* | | | | | */
// /* | | | | | */
// /* ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª */
// /* ÓÍÃÅÀµ½×îµÍ Ò¡¸ËÍƵ½×óÉÏ½Ç */
// if(!ARMED && Rc_Data.ROLL <= 1200 && Rc_Data.PITCH <= 1200 && Rc_Data.THROTTLE <= 1200 && Rc_Data.THROTTLE > 900) { time1++; }
// else time1=0;
// if(time1>30 && !ARMED) { ARMED = 1; time1 = 0;}
// /* Ò£¿ØÉÏËø */
// /* ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª */
// /* | | | / | */
// /* | | | / | */
// /* | | | | | */
// /* | | | | | */
// /* ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª ¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª¡ª */
// /* ÓÍÃÅÀµ½×îµÍ Ò¡¸ËÍƵ½ÓÒÉÏ½Ç */
// if(ARMED && Rc_Data.ROLL >= 1800 && Rc_Data.PITCH <= 1200 && Rc_Data.THROTTLE <= 1200 && Rc_Data.THROTTLE > 900) { time2++; }
// else time2=0;
// if(time2>30 && ARMED) { ARMED = 0; time2 = 0;}
// }
|
楼主
标题置顶
标题高亮
