|
本帖最后由 DreamLovely 于 2021-6-16 12:20 编辑
这是主函数的源码
extern uint8_t PWM_PIN[8];
void initOutput();//初始化函数
void mixTable();//PID计算
void writeMotors();//信号输出到电机
//输出程序
uint8_t PWM_PIN[8] = {9,10,11,3,6,5,16,12}; //定义输出接口
void initOutput() {
for(uint8_t i=0;i<4;i++) {
pinMode(PWM_PIN,OUTPUT);
}//初始化,使PWM引脚作为输出引脚
}
void mixTable(){
int16_t maxMotor;//定义最大转速电机编号
uint8_t i;
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL]*X + axisPID[PITCH]*Y + YAW_DIRECTION * axisPID[YAW]*Z //PID算法
motor[0] = PIDMIX(-1,+1,-1);
motor[1] = PIDMIX(-1,-1,+1);
motor[2] = PIDMIX(+1,+1,+1);
motor[3] = PIDMIX(+1,-1,-1);//4个电机输出计算(PID)
maxMotor=motor[0];//以下代码限制最大输出油门,防止异常
for(i=1; i<4; i++)
if (motor>maxMotor) maxMotor=motor;
for(i=0; i<4; i++) {
if (maxMotor > MAXTHROTTLE) //保证当某一个油门达到最大时,陀螺仪仍有良好的信号连接
motor -= maxMotor - MAXTHROTTLE;
motor = constrain(motor, conf.minthrottle,
MAXTHROTTLE);
if ((rcData[THROTTLE] < MINCHECK) && !f.BARO_MODE)
motor = conf.minthrottle;
if (!f.ARMED)
motor = MINCOMMAND;
}
}
void writeMotors() {
OCR1A = motor[0]>>3; // pin 9输出电机1号
OCR1B = motor[1]>>3; // pin 10输出电机2号
OCR2A = motor[2]>>3; // pin 11输出电机3号
OCR2B = motor[3]>>3; // pin 3输出电机4号
}
void writeAllMotors(int16_t mc) { //所有电机输出设定为mc
for (uint8_t i =0;i<4;i++) {
motor=mc;
}
writeMotors();
}
/*电调初始化函数,电调初始化完成后注释掉defined重新上传
#if defined(ESC_CALIB_CANNOT_FLY)
writeAllMotors(ESC_CALIB_HIGH);
blinkLED(2,20, 2);
delay(4000);
writeAllMotors(ESC_CALIB_LOW);
blinkLED(3,20, 2);
while (1) {
delay(5000);SSSS
blinkLED(4,20, 2);
#endif
}
exit;
#endif*/
这里老是会错误
求助各位大佬如何解决
|
|