基于Arduino的四轴飞行器
- 格式：docx
- 大小：154.66 KB
- 文档页数：186
第2章 基于Arduino的四轴飞行器 2.3.3 模块介绍 1、电机输出模块 （2）相关代码 //主要函数定义 extern uint8_t PWM_PIN[8]; void initOutput();//初始化函数 void mixTable();//PID计算 void writeMotors();//信号输出到电机 //输出程序 uint8_t PWM_PIN[8] = {9,10,11,3,6,5,A2,12}; //定义输出接口 void initOutput() { for(uint8_t i=0;i<4;i++) { pinMode(PWM_PIN[i],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[i]>maxMotor) maxMotor=motor[i]; for(i=0; i<4; i++) { if (maxMotor > MAXTHROTTLE) //保证当某一个油门达到最大时，陀螺仪仍有良好的信号连接 motor[i] -= maxMotor - MAXTHROTTLE; motor[i] = constrain(motor[i], conf.minthrottle, MAXTHROTTLE); if ((rcData[THROTTLE] < MINCHECK) && !f.BARO_MODE) motor[i] = conf.minthrottle; if (!f.ARMED) motor[i] = 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[i]=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); blinkLED(4,20, 2); #endif } exit; #endif*/
2、遥控器发送/接收模块 （2）相关代码 //RX.h主要函数定义 #include "Arduino.h" #define RC_CHANS 8 #define PCINT_PIN_COUNT5 #define PCINT_RX_PORTPORTB #define PCINT_RX_MASKPCMSK0 #define PCIR_PORT_BIT(1<<0) #define RX_PC_INTERRUPTPCINT0_vect #define RX_PCINT_PIN_PORTPINB #define ROLLPIN4//预定义各信道的名称 #define THROTTLEPIN 3 #define PITCHPIN5 #define YAWPIN2 #define AUX1PIN6 #define AUX2PIN 7 #define AUX3PIN 1 //保留 #define AUX4PIN 0 //保留 #define PCINT_RX_BITS (1<<1),(1<<2),(1<<3),(1<<4),(1<<0) void configureReceiver(); void computeRC(); uint16_t readRawRC(uint8_t chan);//初始信号读取函数 //接收代码 volatile uint16_t rcValue[RC_CHANS] = {1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502}; static uint8_t rcChannel[RC_CHANS] = {ROLLPIN, PITCHPIN, YAWPIN, THROTTLEPIN, AUX1PIN,AUX2PIN,AUX3PIN,AUX4PIN}; static uint8_t PCInt_RX_Pins[PCINT_PIN_COUNT] = {PCINT_RX_BITS}; void configureReceiver() { for(uint8_t i = 0; i < PCINT_PIN_COUNT; i++){ PCINT_RX_PORT |= PCInt_RX_Pins[i]; PCINT_RX_MASK |= PCInt_RX_Pins[i]; } PCICR = PCIR_PORT_BIT; PCICR |= (1 << 0) ; #define RX_PIN_CHECK(pin_pos, rc_value_pos) \ if (mask & PCInt_RX_Pins[pin_pos]) { \ if (!(pin & PCInt_RX_Pins[pin_pos])) { \ dTime = cTime-edgeTime[pin_pos]; \ if (900 rcValue[rc_value_pos] = dTime; \ } \ } else edgeTime[pin_pos] = cTime; \ } ISR(RX_PC_INTERRUPT){//中断函数用于响应 uint8_t mask; uint8_t pin; uint16_t cTime,dTime; static uint16_t edgeTime[8]; static uint8_t PCintLast; pin = RX_PCINT_PIN_PORT; mask = pin ^ PCintLast; cTime = micros(); sei(); PCintLast = pin; #if (PCINT_PIN_COUNT > 0) RX_PIN_CHECK(0,2); #endif #if (PCINT_PIN_COUNT > 1) RX_PIN_CHECK(1,4); #endif #if (PCINT_PIN_COUNT > 2) RX_PIN_CHECK(2,5); #endif #if (PCINT_PIN_COUNT > 3) RX_PIN_CHECK(3,6); #endif #if (PCINT_PIN_COUNT > 4) RX_PIN_CHECK(4,7); #endif #if (PCINT_PIN_COUNT > 5) RX_PIN_CHECK(5,0); #endif #if (PCINT_PIN_COUNT > 6) RX_PIN_CHECK(6,1); #endif #if (PCINT_PIN_COUNT > 7) RX_PIN_CHECK(7,3); #endif ISR(PCINT0_vect) { uint8_t pin; uint16_t cTime,dTime; static uint16_t edgeTime; pin = PINB; cTime = micros(); sei(); dTime = cTime-edgeTime; if (900dTime;} else edgeTime = cTime; //如果bit2在高电平(PPM pulse上升),存储时间 } uint16_t readRawRC(uint8_t chan) {//读取原始信号 uint16_t data; uint8_t oldSREG; oldSREG = SREG; cli(); // 禁止中断 data = rcValue[rcChannel[chan]]; SREG = oldSREG; return data; }
void computeRC(){//提取遥控器信号，进行数据处理，主要取平均值
