自平衡小车/zh

概述 项目名称：Microduino自平衡车 目的：制作一台可以用遥控板控制的自平衡机器人小车 难度：中 耗时：2小时 制作者： 简介: 两轮自平衡小车是一个集多种功能于一体的综合系统，是自动控制理论与动力学理论及技术相结合的研究课题，其关键问题是在完成自身平衡的同时，还能够适应各种环境下的控制任务。本次教程我们使用Microduino产品模块快速搭建一个可以用遥控板控制的自平衡机器人小车，玩家可以迅速上手，并且看到小车运动和平衡的效果，玩家们可以在制作结束后，继续更深一步的智能控制部分的研究。 材料清单 Microduino设备 模块 数量 功能 Microduino-Core+/zh 1 核心板 Microduino-USBTTL/zh 1 下载程序 Microduino-nRF24/zh 1 无线通信 Microduino-Robot/zh 1 驱动连接地板 Microduino-Stepper/zh 2 驱动步进电机 其他设备 模块 数量 功能 2.4G天线 1 2.4G通讯 固定支架 1 固定支撑 尼龙螺丝 4 固定 尼龙螺母 12 固定 电池盒 1 装载电池 电池 2 供电 Micro-USB数据线 1 串口通信，下载程序 车轴 2 连接车轮 车轮子 2 结构 步进电机 2 驱动车轮 Joypad材料 模块 数量 功能 Microduino-Core 1 核心 Microduino-nRF24 1 无线通讯 Microduino-Joypad 1 遥控器 Microduino-TFT 1 显示 Microduino-USBTTL 1 下载程序 锂电池 1 供电 亚克力面板 1 面板 尼龙柱 4 固定 长尼龙螺丝 4 固定 短尼龙螺丝 4 固定 尼龙螺母 4 固定 海绵板 1 固定 文件:平衡车物料.jpg 600px 实验原理 PID原理 PID在自动控制领域有着极其重要的作用，作为最早实用化的控制技术已经有70多年的历史，近几年一些创客们自制的一些很酷的东西，如：四轴飞行器，自平衡车等等都离不开它的身影。 首先了解什么是PID。PID实指“比例proportional”、“积分integral”、“微分derivative”，如果我们要求被控制的对象最终趋于一个稳定的结果，一般就可以使用PID算法。 假设说，有一辆速度为1m/s的小车，我们要求他的速度改变为5m/s，要完成这样的一件事，我们必须要有： 小车驱动装置（用程序控制它输出多大的电压，电压决定驱动的马力）； 被驱动器控制的部分（即小车）； 检测当前速度的装置（当前速度与目标速度的差称为误差）； 本来，我们可以给小车一个驱动力让小车加速，直到检测到小车速度达到5m/s，撤去驱动力。但是，这样做会带来几个问题： 1)当小车速度达到5m/s时，从装置检测到这个速度，通知控制器，让控制器改变输出的电压，这一个过程需要耗费一定时间，在这个时间里面，小车速度可能增加了不少。 2)撤去驱动力后，外界条件如摩擦会让小车速度进一步改变。 然而，PID算法可以在一定误差内解决这些问题。 使用PID算法时，大致是这样的。每一个采样周期，通过速度检测装置获得当前速度，传入程序，通过程序计算得到电压控制小车得到新速度。下一个采样周期又把新速度传入，获得新电压，再传入速度，再获得电压，如此反复。 PID算法的关键，是如何根据当前得到的速度值，输出一个“恰当”的电压，以致小车最终能够趋于稳定。 PID算法采用比例，积分，微分(Proportion Integral Differential)三种方法进行控制。三种方法都有自己对应的一个常量(pconst，iconst，dconst)。这三个变量都需要在实验中多次尝试得出。用数学公式表达PID算法如下图所示： 此处：e = 期望值 – 输入值 文件:PIDtheory.jpg 600px 平衡车之所以可以自己掌握平衡，首先通过Microduino-10DOF模块的加速度和陀螺仪测出相应的姿态数据，然后利用kalman滤波算法得出当前平衡车的角度。平衡的角度是180度，如果测出的角度偏离180，PID算法会调整输出相应的PWM值给电机，从而保持小车平衡。 PID原理有点像锅炉房里烧锅炉，首先定下来锅炉的恒定温度，比如26摄氏度，锅炉房里的墙上有一个温度计，能够实时测得锅炉的实时温度。锅炉房里通常有个老大爷时不时（每十分钟看一次）的看着墙上的温度计，如果温度高了就给锅炉降温，低了就给锅炉升温。如果让一个没有经验的年轻小伙子来管理锅炉的温度，可以想象温度表的值会上下浮动的，有经验的老大爷会把这个浮动降到最低。其实PID就是这个烧锅炉的例子，在代码中就有这个故事的影子： 1)规定的26度就是setpoint 2)当前的温度就是CurrentAngle 3)实际值与26度的偏差就是error 4)没有经验的小伙子有点像PID中的P 5)有经验的老大爷相当于PID了 6)每十分钟看一次相当于PID计算的周期时间 PID的主要代码： // PD的实施。 DT是毫秒  float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd)  {      float error;      float output;      error = setPoint - input;      // Kd的两部分实施      //仅使用输入（传感器）的一部分而不是设定值输入（T-2）最大的一个      //而第二个使用该设定值      output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT;      //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");      PID_errorOld2 = PID_errorOld;      PID_errorOld = input;  // 误差为Kd值是唯一的输入组件      setPointOld = setPoint;      return(output);  }  //P控制实现。  float speedPControl(float input, float setPoint,  float Kp)  {      float error;      error = setPoint - input;      return(Kp * error);  }  本套件利用陀螺仪和加速度传感器（Microduino-10DOF/zh）来检测车体态的变化，并利用步进电机控制核心（Microduino-Stepper/zh），精确地驱动电机进行相应的调整，以保持系统的平衡。 文件:PIDtheory1.jpg 600px 主要传感器 Microduino-10DOF 文档 调试过程 将Microduino-Core+与Microduino-USBTTL叠加（无上下顺序），通过USB数据线与电脑连接起来 确认你搭建了Microduino的开发环境，否则参考附录1-Arduino IDE安装指导。 文件:Gettingstarted.jpg 600px 打开Arduino IDE编程软件，点击 【文件】->【打开】 浏览到项目程序地址，点击“Joypad_Balance_Reception.ino”程序打开 文件:Balancecaropen1.jpg 600px 文件:Balancecaropen2.jpg 600px 点击“工具”，在板选项里面选择板卡（Microduino-Core+），在处理器选项里面选择处理器（Atmega644pa@16M,5V），再在端口选项里面选择正确的端口号，然后直接烧录程序 文件:WiFiStationopen4.jpg 600px 平衡车骨架搭建 文件:平衡车步骤.jpg 600px 完成这一步平衡车就搭建完成了 Joypad搭建 600px 注意：图片由于页面压缩效果不佳，请点击查看大图. Step 1：给Joypad的Microduino-CorRF下载程序。 打开MultiWii_CoreRF中的【Joypad_RC】程序，在编译结束后，选择好板卡和端口进行直接下载。 Step 2：将Microduino-TFT从Microduino-Joypad面板后面卡进Microduino-Joypad面板上，用尼龙螺丝固定，注意Microduino-TFT安装方向。 Step 3：先在图示位置安装尼龙柱并在Joypad反面用尼龙螺母固定尼龙柱（尼龙柱由两个小尼龙柱组合而成）。再把2.4G天线插在Microduino-CoreRF模块上，并把Microduino-CoreRF插入在Microduino-Joypad底板上的Upin27任意一个接口上。 Step 4：将Microduino-TFT与Microduino-Joypad通过转接线连接起来，接口有防差错设计，转接线插反就会插不进去 Step 5：将电池上面的开关拨到“Dry bat（1.5V）”的一边，电池（7号）装到电池盒里板上，注意正负极别装反了，电池盒标注了正负极；打开Joypad右边的开关观察是否供电，若无请用USB数据线接入左边的MicroUSB接口来激活系统。

也可以不用电池，直接通过USB线接入左边的MicroUSB来供电。 Step 6：用塑料螺丝将底板和面板固定；先将遥感帽安装在摇杆上，按钮帽安装在按钮键上，再盖上上板用尼龙螺丝固定。（若按键与上板的按键口不好连接，可先将按键插入按键口，再与底板按键连接）。 Step 7：你可以打开侧面电源开关，观察供电是否正常，是否正常进入系统。 自平衡小车和Joypad测试 Joypad操作说明 文件:Joypadtest.jpg 300px 左上边是油门控制开关，打开（拨到上面），才能进行控制，你可以摇动摇杆，观察屏幕的变化。 右边开关是精度调整开关，开关拨到上面可以最大幅度控制，否则只能小幅度控制了，小幅度有助于稳定控制。 左边摇杆本次未使用。 右边摇杆在垂直方向上控制前后方向移动，往上向前，往下向后，在水平方向上控制左右方向移动。 Joypad开机设置 文件:Joypadtest1.jpg 300px 打开遥控器电源开关，按下复位按键（左边USB接口右边那个）进入系统，请在4S内按下【key1】按键，进入遥控器校准和控制选择模式。 360度最大幅度旋转两个摇杆，遥控板会读入摇杆的位置数据，摇动至示数不再变化即可 文件:Joypadtest2.jpg 300px 选择控制模式，可以通过【key3】按键来选择是控制四轴飞行器（Quad.）还是机器人（Robot），Robot模式可控制自平衡车和BOXZ mini，黑色表示选中。因此我们需要选择Robot模式。还可以通过【key4】按键来选择是否是体感控制模式，如果选择体感模式，你必须叠加Microduino-10DOF模块，选择“MPU ON”。如果是摇杆控制模式，选择“MPU OFF”。 这次搭建没有使用10DOF模块，因此选择MPU OFF模式； 文件:Joypadtest3.jpg 300px 选择完成后，通过【key2】按键退出配置，进入操作 文件:Joypadtest4.jpg 300px 将左上边控制开关打开（拨到上面），才能进行控制，你可以摇动摇杆，观察屏幕的变化 文件:Joypadtest5.jpg 300px 右边开关是幅度调节模式，开关拨到上面可以最大幅度控制Robot，否则只能小幅度控制。如果使用小幅度控制小车，右边摇杆拨到最大位置，小车速度也只能小范围变化，这样有助于稳定控制 文件:Joypadtest6.jpg 300px 当启动小车时，只需要用到右边的摇杆，摇杆的方向和平衡车的方向一致，你可以尝试摇杆控制是否正确。 测试通过后，就可以打开平衡车上Microduino-Robot底板上的电源开关，拨到ON（左边），如果可以看到核心板上的红色led亮，说明供电正常。这样制作就完成了，可以愉快的玩耍了 文件:Joypadtest7.jpg 300px 注意问题 下载程序时候最好只叠加core（core+）和USBTTL，虽然本次搭建涉及的nRF24不会引起冲突，但是别的通信模块有时会造成串口冲突，养成好习惯。 Core+要叠在nRF24，USB的底下，紧贴ROBOT板。 锂电池正负极别接错了，否则会烧坏电路。 调试好后，实际运行时不要使用USB供电，供电电压不足，请使用电池 程序说明 Joypad程序及说明 Joypad_RC.ino #include "Arduino.h" #include "def.h" #include "time.h" #include "bat.h" #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) #include "mpu.h" #endif #include "joy.h" #include "key.h" #include "data.h" #include "nrf.h" #include "mwc.h" #include "tft.h" #include "eep.h" #if defined(__AVR_ATmega128RFA1__) #include <ZigduinoRadio.h> #endif //joypad================================ #include <Joypad.h> //eeprom================================ #include <EEPROM.h> //TFT=================================== #include <Adafruit_GFX.h> // Core graphics library #include <Adafruit_ST7735.h> // Hardware-specific #include <SPI.h> //rf==================================== #include <RF24Network.h> #include <RF24.h> #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) //MPU=================================== #include "Wire.h" #include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" #endif //spi=================================== #include <SPI.h> void setup() { // initialize serial communication at 115200 bits per second: #ifdef Serial_DEBUG Serial.begin(115200); delay(100); Serial.println("========hello========"); #endif //--------------- key_init(); //--------------- #ifdef Serial_DEBUG Serial.println("\n\r EEPROM READ..."); #endif eeprom_read(); //--------------- #ifdef Serial_DEBUG Serial.println("\n\r TFT INIT..."); #endif TFT_init(true, tft_rotation); //--------------- #ifdef Serial_DEBUG Serial.println("\n\r TFT BEGIN..."); #endif TIME1 = millis(); while (millis() - TIME1 < interval_TIME1) { TFT_begin(); if (!Joypad.readButton(CH_SWITCH_1)) { #ifdef Serial_DEBUG Serial.println("\n\rCorrect IN..."); #endif //--------------- #ifdef Serial_DEBUG Serial.println("\n\r TFT INIT..."); #endif TFT_init(false, tft_rotation); while (1) { if (!TFT_config()) break; } #ifdef Serial_DEBUG Serial.println("\n\rCorrect OUT..."); #endif //--------------- #ifdef Serial_DEBUG Serial.println("\n\r EEPROM WRITE..."); #endif eeprom_write(); } } //--------------- #ifdef Serial_DEBUG Serial.println("\n\r TFT CLEAR..."); #endif TFT_clear(); //--------------- #ifdef Serial_DEBUG Serial.println("\n\r TFT READY..."); #endif TFT_ready(); //---------------.l if (mode_protocol) //Robot { SPI.begin(); //初始化SPI总线 radio.begin(); network.begin(/*channel*/ nrf_channal, /*node address*/ this_node); } else //QuadCopter { unsigned long _channel; #if !defined(__AVR_ATmega128RFA1__) switch (mwc_channal) { case 0: _channel = 9600; break; case 1: _channel = 19200; break; case 2: _channel = 38400; break; case 3: _channel = 57600; break; case 4: _channel = 115200; break; } #else if _channel = mwc_channal; #endif mwc_port.begin(_channel); } //--------------- #ifdef Serial_DEBUG Serial.println("===========start==========="); #endif #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) if (mode_mpu) initMPU(); //initialize device #endif } void loop() { // unsigned long time = millis(); #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) //MPU-------------------------------- if (mode_mpu) getMPU(); #endif //DATA_begin------------------------------ data_begin(); //DATA_send------------------------------- if (millis() < time2) time2 = millis(); if (millis() - time2 > interval_time2) { if (mode_protocol) nrf_send(); //Robot else data_send(); //QuadCopter time2 = millis(); } //节点查错------------------------------- vodebug(); //BAT-------------------------------- if (time3 > millis()) time3 = millis(); if (millis() - time3 > interval_time3) { vobat(); time3 = millis(); } //TFT------------------------------------ TFT_run(); //=================================== // time = millis() - time; // Serial.println(time, DEC); //loop time } </cpp> BAT.h <source lang="cpp"> int8_t _V_bat = _V_min; boolean mcu_voltage = true; // 5.0 or 3.3 #define _V_fix 0.2 //fix battery voltage #define _V_math(Y) (_V_fix+((Y*analogRead(PIN_bat)/1023.0f)/(33.0f/(51.0f+33.0f)))) void vobat() { //_V_bat=10*((voltage*analogRead(PIN_bat)/1023.0f)/(33.0f/(51.0f+33.0f))); _V_bat = _V_math(mcu_voltage ? 50 : 33); _V_bat = constrain(_V_bat, _V_min, _V_max); #ifdef Serial_DEBUG Serial.print("_V_bat: "); Serial.println(_V_bat); #endif } data.h #include "Arduino.h" byte inBuf[16]; int16_t outBuf[8] = { Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID, Joy_MID }; boolean AUX[4] = {0, 0, 0, 0}; //====================================== void data_begin() { Joy(); if (mode_protocol) //Robot { if (!sw_l) { Joy_x = Joy_MID; Joy_y = Joy_MID; Joy1_x = Joy_MID; Joy1_y = Joy_MID; } } else //QuadCopter { if (!sw_l) Joy_y = Joy_MID - Joy_maximum; } //but--------------------------------- for (uint8_t a = 0; a < 4; a++) { if (key_get(a, 1)) AUX[a] = !AUX[a]; } outBuf[0] = Joy1_x; outBuf[1] = Joy1_y; outBuf[2] = Joy_x; outBuf[3] = Joy_y; outBuf[4] = map(AUX[0], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); outBuf[5] = map(AUX[1], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); outBuf[6] = map(AUX[2], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); outBuf[7] = map(AUX[3], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); } def.h #include "Arduino.h" //DEBUG----------- #define Serial_DEBUG //MWC------------- uint8_t mwc_channal = 11; //RF channel #if defined(__AVR_ATmega32U4__) #define mwc_port Serial1 //Serial1 is D0 D1 #elif defined(__AVR_ATmega128RFA1__) #define mwc_port ZigduinoRadio //RF #else #define mwc_port Serial //Serial is D0 D1 #endif //nRF------------- #define interval_debug 2000 //节点查错间隔 uint8_t nrf_channal = 70; //0~125 //Battery--------- #define PIN_bat A7 //BAT #define _V_max 41 //锂电池满电电压4.2V #define _V_min 36 //锂电池没电电压3.7V #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) //MPU------------- #define MPU_maximum 70 #endif //Time------------ #define interval_TIME1 2000 //setup delay #define interval_time2 40 //send interval #define interval_time3 1000 //battery interval eep.h #include "Arduino.h" #include <EEPROM.h> #define EEPROM_write(address, p) {int i = 0; byte *pp = (byte*)&(p);for(; i < sizeof(p); i++) EEPROM.write(address+i, pp[i]);} #define EEPROM_read(address, p) {int i = 0; byte *pp = (byte*)&(p);for(; i < sizeof(p); i++) pp[i]=EEPROM.read(address+i);} struct config_type { int16_t eeprom_correct_min[4]; int16_t eeprom_correct_max[4]; uint8_t eeprom_Joy_deadzone_val; #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) boolean eeprom_mode_mpu; #endif boolean eeprom_mode_protocol; uint8_t eeprom_mwc_channal; uint8_t eeprom_nrf_channal; boolean eeprom_tft_theme; boolean eeprom_tft_rotation; boolean eeprom_mcu_voltage; }; //====================================== void eeprom_read() { //EEPROM读取赋值 config_type config_readback; EEPROM_read(0, config_readback); for (uint8_t a = 0; a < 4; a++) { joy_correct_min[a] = config_readback.eeprom_correct_min[a]; joy_correct_max[a] = config_readback.eeprom_correct_max[a]; } Joy_deadzone_val = config_readback.eeprom_Joy_deadzone_val; mode_protocol = config_readback.eeprom_mode_protocol; #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) mode_mpu = config_readback.eeprom_mode_mpu; #endif mwc_channal = config_readback.eeprom_mwc_channal; nrf_channal = config_readback.eeprom_nrf_channal; tft_theme = config_readback.eeprom_tft_theme; tft_rotation = config_readback.eeprom_tft_rotation; mcu_voltage = config_readback.eeprom_mcu_voltage; } void eeprom_write() { // 定义结构变量config，并定义config的内容 config_type config; for (uint8_t a = 0; a < 4; a++) { config.eeprom_correct_min[a] = joy_correct_min[a]; config.eeprom_correct_max[a] = joy_correct_max[a]; } config.eeprom_Joy_deadzone_val = Joy_deadzone_val; config.eeprom_mode_protocol = mode_protocol; #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) config.eeprom_mode_mpu = mode_mpu; #endif config.eeprom_mwc_channal = mwc_channal; config.eeprom_nrf_channal = nrf_channal; config.eeprom_tft_theme = tft_theme; config.eeprom_tft_rotation = tft_rotation; config.eeprom_mcu_voltage = mcu_voltage; // 变量config存储到EEPROM，地址0写入 EEPROM_write(0, config); } joy.h #include "Arduino.h" #include <Joypad.h> //Joy------------- //1000~2000 uint8_t Joy_deadzone_val = 10; #define Joy_s_maximum 200 //MAX 300 #define Joy_maximum 450 //MAX 500 #define Joy_MID 1500 //1500 boolean mode_mpu, mode_protocol; //{(0: 0 is mwc, 1 is nrf),(1: 0 is mpu, 1 is no mpu)} int16_t joy_correct_max[4], joy_correct_min[4]; int16_t Joy_x, Joy_y, Joy1_x, Joy1_y; int16_t s_lig, s_mic; boolean Joy_sw, Joy1_sw; boolean but1, but2, but3, but4; boolean sw_l, sw_r; //====================================== int16_t Joy_dead_zone(int16_t _Joy_vol) { if (abs(_Joy_vol) > Joy_deadzone_val) return ((_Joy_vol > 0) ? (_Joy_vol - Joy_deadzone_val) : (_Joy_vol + Joy_deadzone_val)); else return 0; } int16_t Joy_i(int16_t _Joy_i, boolean _Joy_b, int16_t _Joy_MIN, int16_t _Joy_MAX) { int16_t _Joy_a; switch (_Joy_i) { case 0: _Joy_a = Joy_dead_zone(Joypad.readJoystickX()); break; case 1: _Joy_a = Joypad.readJoystickY(); //throt break; case 2: _Joy_a = Joy_dead_zone(Joypad.readJoystick1X()); break; case 3: _Joy_a = Joy_dead_zone(Joypad.readJoystick1Y()); break; } if (_Joy_b) { if (_Joy_a < 0) _Joy_a = map(_Joy_a, joy_correct_min[_Joy_i], 0, _Joy_MAX, Joy_MID); else _Joy_a = map(_Joy_a, 0, joy_correct_max[_Joy_i], Joy_MID, _Joy_MIN); if (_Joy_a < _Joy_MIN) _Joy_a = _Joy_MIN; if (_Joy_a > _Joy_MAX) _Joy_a = _Joy_MAX; } return _Joy_a; } void Joy() { sw_l = Joypad.readButton(CH_SWITCH_L); sw_r = Joypad.readButton(CH_SWITCH_R); //------------------------------------ //s_lig=Joypad.readLightSensor(); //s_mic=Joypad.readMicrophone(); //------------------------------------ Joy_sw = Joypad.readButton(CH_JOYSTICK_SW); Joy1_sw = Joypad.readButton(CH_JOYSTICK1_SW); //------------------------------------ but1 = Joypad.readButton(CH_SWITCH_1); but2 = Joypad.readButton(CH_SWITCH_2); but3 = Joypad.readButton(CH_SWITCH_3); but4 = Joypad.readButton(CH_SWITCH_4); #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) //==================================== int16_t y[3]; //MPU--------------------------------- if (mode_mpu) //MPU--------------------------------- { for (uint8_t a = 0; a < 3; a++) { y[a] = ypr[a] * 180 / M_PI; if (y[a] > MPU_maximum) y[a] = MPU_maximum; if (y[a] < -MPU_maximum) y[a] = -MPU_maximum; } } #endif if (Joypad.readButton(CH_SWITCH_R)) { Joy_x = Joy_i(0, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); Joy_y = Joy_i(1, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) if (mode_mpu) //MPU--------------------------------- { Joy1_x = map(y[2], -MPU_maximum, MPU_maximum, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); Joy1_y = map(y[1], -MPU_maximum, MPU_maximum, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); } else #endif { Joy1_x = Joy_i(2, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); Joy1_y = Joy_i(3, true, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); } } else { Joy_x = Joy_i(0, true, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum); Joy_y = Joy_i(1, true, mode_protocol ? Joy_MID - Joy_s_maximum : Joy_MID - Joy_maximum, mode_protocol ? Joy_MID + Joy_s_maximum : Joy_MID + Joy_maximum); // Robot,QuadCopter #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) if (mode_mpu) //MPU--------------------------------- { Joy1_x = map(y[2], -MPU_maximum, MPU_maximum, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum); Joy1_y = map(y[1], -MPU_maximum, MPU_maximum, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum); } else #endif { Joy1_x = Joy_i(2, true, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum); Joy1_y = Joy_i(3, true, Joy_MID - Joy_s_maximum, Joy_MID + Joy_s_maximum); } } } key.h #include "arduino.h" uint8_t key_pin[4] = {CH_SWITCH_1, CH_SWITCH_2, CH_SWITCH_3, CH_SWITCH_4}; //按键1 2 3 4 boolean key_status[4]; //按键 boolean key_cache[4]; //检测按键松开缓存 void key_init() { for (uint8_t a = 0; a < 4; a++) { key_status[a] = LOW; key_cache[a] = HIGH; } } boolean key_get(uint8_t _key_num, boolean _key_type) { key_cache[_key_num] = key_status[_key_num]; //缓存作判断用 key_status[_key_num] = !Joypad.readButton(key_pin[_key_num]); //触发时 switch (_key_type) { case 0: if (!key_status[_key_num] && key_cache[_key_num]) //按下松开后 return true; else return false; break; case 1: if (key_status[_key_num] && !key_cache[_key_num]) //按下松开后 return true; else return false; break; } } mpu.h #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) #include "Wire.h" #include "I2Cdev.h" #include "MPU6050_6Axis_MotionApps20.h" MPU6050 mpu; //MPU------------- #define MPU_maximum 70 // MPU control/status vars boolean dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer // orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector void initMPU() { Wire.begin(); #ifdef Serial_DEBUG Serial.println(F("Initializing I2C devices...")); #endif mpu.initialize(); // verify connection #ifdef Serial_DEBUG Serial.println(F("Testing device connections...")); #endif if (mpu.testConnection()) { #ifdef Serial_DEBUG Serial.println("MPU6050 connection successful"); #endif } #ifdef Serial_DEBUG else Serial.println(F("MPU6050 connection failed")); #endif // load and configure the DMP #ifdef Serial_DEBUG Serial.println(F("Initializing DMP...")); #endif devStatus = mpu.dmpInitialize(); // make sure it worked (returns 0 if so) if (devStatus == 0) { // turn on the DMP, now that it's ready #ifdef Serial_DEBUG Serial.println(F("Enabling DMP...")); #endif mpu.setDMPEnabled(true); mpuIntStatus = mpu.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it // Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady = true; // get expected DMP packet size for later comparison packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed // (if it's going to break, usually the code will be 1) #ifdef Serial_DEBUG Serial.print(F("DMP Initialization failed (code ")); Serial.print(devStatus); Serial.println(F(")")); #endif } } void getMPU() { if (!dmpReady) return; { // reset interrupt flag and get INT_STATUS byte mpuIntStatus = mpu.getIntStatus(); // get current FIFO count fifoCount = mpu.getFIFOCount(); // check for overflow (this should never happen unless our code is too inefficient) if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // reset so we can continue cleanly mpu.resetFIFO(); #ifdef Serial_DEBUG Serial.println(F("FIFO overflow!")); #endif // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (mpuIntStatus & 0x02) { // wait for correct available data length, should be a VERY short wait while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); // read a packet from FIFO mpu.getFIFOBytes(fifoBuffer, packetSize); // track FIFO count here in case there is > 1 packet available // (this lets us immediately read more without waiting for an interrupt) fifoCount -= packetSize; // display ypr angles in degrees mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //Serial.print("ypr\t"); //Serial.print(ypr[0] * 180/M_PI); //Serial.print("\t"); //Serial.print(ypr[1] * 180/M_PI); //Serial.print("\t"); // Serial.println(ypr[2] * 180/M_PI); } } } #endif mwc.h #include "Arduino.h" #if defined(__AVR_ATmega128RFA1__) #include <ZigduinoRadio.h> #endif int16_t RCin[8], RCoutA[8], RCoutB[8]; int16_t p; uint16_t read16() { uint16_t r = (inBuf[p++] & 0xFF); r += (inBuf[p++] & 0xFF) << 8; return r; } uint16_t t, t1, t2; uint16_t write16(boolean a) { if (a) { t1 = outBuf[p++] >> 8; t2 = outBuf[p - 1] - (t1 << 8); t = t1; } else t = t2; return t; } typedef unsigned char byte; byte getChecksum(byte length, byte cmd, byte mydata[]) { //三个参数分别为： 数据长度 ， 指令代码 ， 实际数据数组 byte checksum = 0; checksum ^= (length & 0xFF); checksum ^= (cmd & 0xFF); for (uint8_t i = 0; i < length; i++) { checksum ^= (mydata[i] & 0xFF); } return checksum; } void data_rx() { // s_struct_w((int*)&inBuf,16); p = 0; for (uint8_t i = 0; i < 8; i++) { RCin[i] = read16(); /* Serial.print("RC["); Serial.print(i+1); Serial.print("]:"); Serial.print(inBuf[2*i],DEC); Serial.print(","); Serial.print(inBuf[2*i+1],DEC); Serial.print("---"); Serial.println(RCin[i]); */ // delay(50); // delay in between reads for stability } } void data_tx() { p = 0; for (uint8_t i = 0; i < 8; i++) { RCoutA[i] = write16(1); RCoutB[i] = write16(0); /* Serial.print("RC["); Serial.print(i+1); Serial.print("]:"); Serial.print(RCout[i]); Serial.print("---"); Serial.print(RCoutA[i],DEC); Serial.print(","); Serial.print(RCoutB[i],DEC); Serial.println(""); */ // delay(50); // delay in between reads for stability } } /* if Core RF [head,2byte,0xAA 0xBB] [type,1byte,0xCC] [data,16byte] [body,1byte(from getChecksum())] Example: AA BB CC 1A 01 1A 01 1A 01 2A 01 3A 01 4A 01 5A 01 6A 01 0D ** */ void data_send() { data_tx(); #if !defined(__AVR_ATmega128RFA1__) static byte buf_head[3]; buf_head[0] = 0x24; buf_head[1] = 0x4D; buf_head[2] = 0x3C; #endif #define buf_length 0x10 //16 #define buf_code 0xC8 //200 static byte buf_data[buf_length]; for (uint8_t a = 0; a < (buf_length / 2); a++) { buf_data[2 * a] = RCoutB[a]; buf_data[2 * a + 1] = RCoutA[a]; } static byte buf_body; buf_body = getChecksum(buf_length, buf_code, buf_data); //---------------------- #if defined(__AVR_ATmega128RFA1__) mwc_port.beginTransmission(); mwc_port.write(0xaa); mwc_port.write(0xbb); mwc_port.write(0xcc); #else for (uint8_t a = 0; a < 3; a++) { mwc_port.write(buf_head[a]); } mwc_port.write(buf_length); mwc_port.write(buf_code); #endif for (uint8_t a = 0; a < buf_length; a++) { mwc_port.write(buf_data[a]); } mwc_port.write(buf_body); #if defined(__AVR_ATmega128RFA1__) mwc_port.endTransmission(); #endif } nrf.h #include "Arduino.h" #include <RF24Network.h> #include <RF24.h> #include <SPI.h> // nRF24L01(+) radio attached using Getting Started board RF24 radio(9, 10); //ce,cs RF24Network network(radio); #define this_node 0 //设置本机ID #define other_node 1 //-------------------------------- struct send_a //发送 { uint32_t ms; uint16_t rf_CH0; uint16_t rf_CH1; uint16_t rf_CH2; uint16_t rf_CH3; uint16_t rf_CH4; uint16_t rf_CH5; uint16_t rf_CH6; uint16_t rf_CH7; }; struct receive_a //接收 { uint32_t node_ms; }; //-------------------------------- unsigned long node_clock, node_clock_debug, node_clock_cache = 0; //节点运行时间、节点响应检查时间、节点时间缓存 //debug-------------------------- boolean node_clock_error = false; //节点响应状态 unsigned long time_debug = 0; //定时器 //====================================== void vodebug() { if (millis() - time_debug > interval_debug) { node_clock_error = boolean(node_clock == node_clock_debug); //一定时间内，节点返回的运行时间若不变则有问题 node_clock_debug = node_clock; time_debug = millis(); } } void nrf_send() { #ifdef Serial_DEBUG Serial.print("Sending..."); #endif send_a sen = { millis(), outBuf[0], outBuf[1], outBuf[2], outBuf[3], outBuf[4], outBuf[5], outBuf[6], outBuf[7] }; //把这些数据发送出去，对应前面的发送数组 RF24NetworkHeader header(other_node); if (network.write(header, &sen, sizeof(sen))) { #ifdef Serial_DEBUG Serial.print("Is ok."); #endif delay(50); network.update(); // If it's time to send a message, send it! while ( network.available() ) { // If so, grab it and print it out RF24NetworkHeader header; receive_a rec; network.read(header, &rec, sizeof(rec)); node_clock = rec.node_ms; //运行时间赋值 } } #ifdef Serial_DEBUG else Serial.print("Is failed."); Serial.println(""); #endif } tft.h #include "Arduino.h" #include <Adafruit_GFX.h> // Core graphics library #include <Adafruit_ST7735.h> // Hardware-specific library #include <SPI.h> Adafruit_ST7735 tft = Adafruit_ST7735(5, 4, -1); //cs,dc,rst //-------字体设置，大、中、小 #define setFont_M tft.setTextSize(2) #define setFont_S tft.setTextSize(0) #define tft_width 128 #define tft_height 160 boolean tft_theme = false; //0 is white,1 is black boolean tft_rotation = 1; #define TFT_TOP ST7735_BLACK #define TFT_BUT ST7735_WHITE uint16_t tft_colorA = TFT_BUT; uint16_t tft_colorB = TFT_TOP; uint16_t tft_colorC = 0x06FF; uint16_t tft_colorD = 0xEABF; #define tft_bat_x 24 #define tft_bat_y 12 #define tft_bat_x_s 2 #define tft_bat_y_s 6 #define tft_font_s_height 8 #define tft_font_m_height 16 #define tft_font_l_height 24 #define _Q_x 33 #define _Q_y 36 #define _W_x 93 #define _W_y 5 #define _Q_font_x 2 #define _Q_font_y (_Q_y - 1) int8_t tft_cache = 1; //====================================== void TFT_clear() { tft.fillScreen(tft_colorB); } void TFT_init(boolean _init, boolean _rot) { tft_colorB = tft_theme ? TFT_TOP : TFT_BUT; tft_colorA = tft_theme ? TFT_BUT : TFT_TOP; if (_init) { tft.initR(INITR_BLACKTAB); // initialize a ST7735S chip, black tab // Serial.println("init"); tft.fillScreen(tft_colorB); if (_rot) tft.setRotation(2); } tft.fillRect(0, 0, tft_width, 40, tft_colorA); tft.setTextColor(tft_colorB); setFont_M; tft.setCursor(26, 6); tft.print("Joypad"); setFont_S; tft.setCursor(32, 24); tft.print("Microduino"); tft.fillRect(0, 40, tft_width, 120, tft_colorB); } void TFT_begin() { setFont_S; tft.setTextColor(tft_colorA); tft.setCursor(_Q_font_x, 44); tft.println("[key1] enter config"); setFont_M; tft.setCursor(4, 150); for (uint8_t a = 0; a < (millis() - TIME1) / (interval_TIME1 / 10); a++) { tft.print("-"); } } int8_t menu_num_A = 0; int8_t menu_num_B = 0; int8_t menu_sta = 0; #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) char *menu_str_a[5] = { "Joystick Config", "Protocol Config", "System Config", "Gyroscope Config", "Exit" }; #else char *menu_str_a[4] = { "Joystick Config", "Protocol Config", "System Config", "Exit" }; #endif #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) char *menu_str_b[4][3] = { {"Joystick Correct.", "Dead Zone config"}, {"Mode", "Quadrotor Channel", "nRF24 Channel"}, {"TFT Theme", "TFT Rotation", "MCU Voltage"}, {"Gyroscope OFF", "Gyroscope ON"} }; #else char *menu_str_b[3][3] = { {"Joystick Correct.", "Dead Zone config"}, {"Mode", "Quadrotor Channel", "nRF24 Channel"}, {"TFT Theme", "TFT Rotation", "MCU Voltage"}, }; #endif void TFT_menu(int8_t _num, char *_data) { tft.drawRect(7, 49 + 15 * _num, 114, 16, tft_colorA); tft.setCursor(10, 54 + 15 * _num); tft.print(_data); } void TFT_menu(int8_t _num, int16_t _data) { tft.drawRect(7, 49 + 15 * _num, 114, 16, tft_colorA); tft.setCursor(10, 54 + 15 * _num); tft.print(_data); } void TFT_cursor(int8_t _num) { tft.drawLine(1, 51 + 15 * _num, 4, 56 + 15 * _num, tft_colorA); tft.drawLine(4, 57 + 15 * _num, 1, 62 + 15 * _num, tft_colorA); tft.drawLine(1, 51 + 15 * _num, 1, 62 + 15 * _num, tft_colorA); } boolean return_menu = false; boolean TFT_config() { tft.setTextColor( tft_colorA); if (key_get(0, 1)) { menu_sta --; tft_cache = 1; if (menu_sta <= 0) menu_num_B = 0; //zero } if (key_get(1, 1)) { if (return_menu) menu_sta --; else menu_sta ++; tft_cache = 1; } if (menu_sta > 2) menu_sta = 2; if (menu_sta < 0) menu_sta = 0; return_menu = false; //------------------------------- if (tft_cache) tft.fillRect(0, 40, tft_width, 100, tft_colorB); if (menu_sta == 2) { switch (menu_num_A) { case 0: { switch (menu_num_B) { case 0: { if (tft_cache) { for (uint8_t a = 0; a < 4; a++) { joy_correct_min[a] = 0; joy_correct_max[a] = 0; } } for (uint8_t a = 0; a < 4; a++) { tft.setCursor(2, 120); tft.print("Move Joystick MaxGear"); int16_t _c = Joy_i(a, false, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum); if (_c > joy_correct_max[a]) { tft.fillRect(0, 40, tft_width, 100, tft_colorB); joy_correct_max[a] = _c; } // joy_correct_max[a] = constrain(joy_correct_max[a], 0, Joy_maximum); if (_c < joy_correct_min[a]) { tft.fillRect(0, 40, tft_width, 100, tft_colorB); joy_correct_min[a] = _c; } // joy_correct_min[a] = constrain(joy_correct_min[a], -Joy_maximum, 0); } for (uint8_t d = 0; d < 2; d++) { tft.drawFastHLine(12 + 70 * d, 80, 33, tft_colorA); tft.drawFastVLine(28 + 70 * d, 64, 33, tft_colorA); // tft.fillRect(2, 90-4, 20, 12, tft_colorB); tft.drawCircle(44 + 70 * d, 80, map(joy_correct_min[0 + 2 * d], 0, -512, 1, 10), tft_colorA); tft.drawCircle(12 + 70 * d, 80, map(joy_correct_max[0 + 2 * d], 0, 512, 1, 10), tft_colorA); tft.drawCircle(28 + 70 * d, 64, map(joy_correct_min[1 + 2 * d], 0, -512, 1, 10), tft_colorA); tft.drawCircle(28 + 70 * d, 96, map(joy_correct_max[1 + 2 * d], 0, 512, 1, 10), tft_colorA); } return_menu = true; } break; case 1: { if (key_get(2, 1)) { Joy_deadzone_val--; tft.fillRect(0, 40, tft_width, 100, tft_colorB); } if (key_get(3, 1)) { Joy_deadzone_val++; tft.fillRect(0, 40, tft_width, 100, tft_colorB); } Joy_deadzone_val = constrain(Joy_deadzone_val, 0, 25); TFT_menu(0, Joy_deadzone_val); TFT_cursor(0); return_menu = true; } break; } } break; case 1: { switch (menu_num_B) { case 0: { char *menu_str_c[2] = { "Quadro.", "nRF24"}; if (key_get(2, 1) || key_get(3, 1)) { mode_protocol = !mode_protocol; tft.fillRect(0, 40, tft_width, 100, tft_colorB); } for (uint8_t c = 0; c < 2; c++) { TFT_menu(c, menu_str_c[c]); } TFT_cursor(mode_protocol); return_menu = true; } break; case 1: { #if !defined(__AVR_ATmega128RFA1__) char *menu_str_c[5] = {"9600", "19200", "38400", "57600", "115200"}; #endif if (key_get(2, 1)) { mwc_channal--; tft.fillRect(0, 40, tft_width, 100, tft_colorB); } if (key_get(3, 1)) { mwc_channal++; tft.fillRect(0, 40, tft_width, 100, tft_colorB); } #if !defined(__AVR_ATmega128RFA1__) mwc_channal = constrain(mwc_channal, 0, 4); TFT_menu(0, menu_str_c[mwc_channal]); #else mwc_channal = constrain(mwc_channal, 11, 26); TFT_menu(0, mwc_channal); #endif TFT_cursor(0); return_menu = true; } break; case 2: { if (key_get(2, 1)) { nrf_channal--; tft.fillRect(0, 40, tft_width, 100, tft_colorB); } if (key_get(3, 1)) { nrf_channal++; tft.fillRect(0, 40, tft_width, 100, tft_colorB); } nrf_channal = constrain(nrf_channal, 0, 125); TFT_menu(0, nrf_channal); TFT_cursor(0); return_menu = true; } break; } } break; case 2: { switch (menu_num_B) { case 0: { tft_theme = !tft_theme; TFT_init(true, tft_rotation); tft_cache = 1; tft.setTextColor(tft_colorA); menu_sta --; } break; case 1: { tft_rotation = !tft_rotation; TFT_init(true, tft_rotation); tft_cache = 1; tft.setTextColor(tft_colorA); menu_sta --; } break; case 2: { char *menu_str_c[2] = { "3.3V", "5.0V"}; return_menu = true; if (key_get(2, 1) || key_get(3, 1)) { mcu_voltage = !mcu_voltage; tft.fillRect(0, 40, tft_width, 100, tft_colorB); } TFT_cursor(mcu_voltage); for (uint8_t c = 0; c < 2; c++) { TFT_menu(c, menu_str_c[c]); } // tft.fillRect(0, 40, tft_width, 100,tft_colorB); } break; } } break; #if !(defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__)) case 3: { //mpu mode_mpu = menu_num_B; tft_cache = 1; menu_sta = 0; //back main menu menu_num_B = 0; //zero } break; #endif } } /* Serial.print(menu_sta); Serial.print(","); Serial.print(menu_num_A); Serial.print(","); Serial.println(menu_num_B); */ //---------------------------- if (menu_sta == 1) { #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) int8_t meun_b_max[5] = {1, 2, 2, 1, 0}; #else int8_t meun_b_max[4] = {1, 2, 2, 0}; #endif if (!meun_b_max[menu_num_A]) return false; else { if (key_get(2, 1)) { tft.fillRect(0, 40, 5, 100, tft_colorB); menu_num_B--; } if (key_get(3, 1)) { tft.fillRect(0, 40, 5, 100, tft_colorB); menu_num_B++; } menu_num_B = constrain(menu_num_B, 0, meun_b_max[menu_num_A]); TFT_cursor(menu_num_B); if (tft_cache) { for (uint8_t b = 0; b < (meun_b_max[menu_num_A] + 1); b++) { TFT_menu(b, menu_str_b[menu_num_A][b]); } } } } //main menu if (menu_sta == 0) { //custer if (key_get(2, 1)) { tft.fillRect(0, 40, 5, 100, tft_colorB); menu_num_A--; } if (key_get(3, 1)) { tft.fillRect(0, 40, 5, 100, tft_colorB); menu_num_A++; } #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) menu_num_A = constrain(menu_num_A, 0, 4); #else menu_num_A = constrain(menu_num_A, 0, 3); #endif TFT_cursor(menu_num_A); if (tft_cache) { #if defined(__AVR_ATmega1284P__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega128RFA1__) for (uint8_t a = 0; a < 5; a++) { #else for (uint8_t a = 0; a < 4; a++) { #endif TFT_menu(a, menu_str_a[a]); } } } if (tft_cache) { //BACK tft.fillCircle(12, 149, 8, tft_colorA); tft.drawLine(11, 145, 7, 149, tft_colorB); tft.drawLine(7, 149, 11, 153, tft_colorB); tft.drawLine(7, 149, 17, 149, tft_colorB); //ENTER tft.fillCircle(12 + 20, 149, 8, tft_colorA); tft.drawLine(10 + 20, 146, 7 + 20, 149, tft_colorB); tft.drawLine(7 + 20, 149, 10 + 20, 152, tft_colorB); tft.drawLine(7 + 20, 149, 15 + 20, 149, tft_colorB); tft.drawLine(15 + 20, 146, 15 + 20, 149, tft_colorB); //PREV tft.fillCircle(127 - 12, 149, 8, tft_colorA); tft.drawLine(127 - 12, 153, 127 - 8, 149, tft_colorB); tft.drawLine(127 - 12, 153, 127 - 16, 149, tft_colorB); tft.drawLine(127 - 12, 153, 127 - 12, 145, tft_colorB); //NEXT tft.fillCircle(127 - 32, 149, 8, tft_colorA); tft.drawLine(127 - 32, 145, 127 - 28, 149, tft_colorB); tft.drawLine(127 - 32, 145, 127 - 36, 149, tft_colorB); tft.drawLine(127 - 32, 145, 127 - 32, 153, tft_colorB); } tft_cache --; if (tft_cache < 0) tft_cache = 0; return true; } //------------------ #define _C_x_S (_Q_x + 1) #define _C_x_M (_Q_x + ((_W_x + 1) / 2)) #define _C_x_E (_Q_x + _W_x - 1) char *NAME[8] = { "ROLL", "PITCH", "YAW", "THROT", "AUX1", "AUX2", "AUX3", "AUX4" }; void TFT_ready() { tft.fillRect(0, 0, 128, 26, tft_colorA); tft.drawRect(tft_width - tft_bat_x - tft_bat_x_s - 2, 2, tft_bat_x, tft_bat_y, tft_colorB); tft.drawRect(tft_width - tft_bat_x_s - 2, 2 + (tft_bat_y - tft_bat_y_s) / 2, tft_bat_x_s, tft_bat_y_s, tft_colorB); tft.setTextColor(tft_colorB); setFont_S; tft.setCursor(_Q_font_x, 3); tft.print(mode_protocol ? "nRF24" : "Quadr"); tft.print(" CHAN."); tft.print(mode_protocol ? nrf_channal : mwc_channal); tft.setCursor(_Q_font_x, 16); tft.print("Time:"); tft.setTextColor(tft_colorA); for (uint8_t a = 0; a < 8; a++) { tft.setCursor(_Q_font_x, _Q_font_y + a * 15); tft.print(NAME[a]); //------------------------------------------ tft.drawRect(_Q_x, _Q_y + a * 15, _W_x, _W_y, tft_colorA); } } boolean _a = false, _b = false; void TFT_run() { if (outBuf[3] > (Joy_MID - Joy_maximum)) { if (_a) { Joy_time[0] = millis() - Joy_time[1]; _a = false; } Joy_time[1] = millis() - Joy_time[0]; } else _a = true; if (!_b && ((Joy_time[1] / 1000) % 2)) { _b = !_b; tft.fillRect(_Q_font_x + 30, 16, 50, 7, tft_colorA); tft.setTextColor(tft_colorB); tft.setCursor(_Q_font_x + 30, 16); tft.print((Joy_time[1] / 1000) / 60); tft.print("m"); tft.print((Joy_time[1] / 1000) % 60); tft.print("s"); } _b = boolean((Joy_time[1] / 1000) % 2); //battery------------------ tft.fillRect(tft_width - tft_bat_x - 3, 3, map(_V_bat, _V_min, _V_max, 0, tft_bat_x - 2) , tft_bat_y - 2, tft_colorB); tft.fillRect(tft_width - tft_bat_x - 3 + map(_V_bat, _V_min, _V_max, 0, tft_bat_x - 2), 3, map(_V_bat, _V_min, _V_max, tft_bat_x - 2, 0) , tft_bat_y - 2, tft_colorA); for (uint8_t a = 0; a < 8; a++) { int8_t _C_x_A0, _C_x_B0, _C_x_A, _C_x_B, _C_x_A1, _C_x_B1; int8_t _C_x; if (outBuf[a] < Joy_MID) { _C_x = map(outBuf[a], Joy_MID - Joy_maximum, Joy_MID, _C_x_S, _C_x_M); _C_x_A0 = _C_x_S; _C_x_B0 = _C_x - _C_x_S; _C_x_A = _C_x; _C_x_B = _C_x_M - _C_x; _C_x_A1 = _C_x_M; _C_x_B1 = _C_x_E - _C_x_M; } else if (outBuf[a] > Joy_MID) { _C_x = map(outBuf[a], Joy_MID, Joy_MID + Joy_maximum, _C_x_M, _C_x_E); _C_x_A0 = _C_x_S; _C_x_B0 = _C_x_M - _C_x_S; _C_x_A = _C_x_M; _C_x_B = _C_x - _C_x_M; _C_x_A1 = _C_x; _C_x_B1 = _C_x_E - _C_x; } else { _C_x_A0 = _C_x_S; _C_x_B0 = _C_x_M - _C_x_S; _C_x_A = _C_x_M; _C_x_B = 0; _C_x_A1 = _C_x_M; _C_x_B1 = _C_x_E - _C_x_M; } tft.fillRect(_C_x_A0, _Q_y + a * 15 + 1, _C_x_B0, _W_y - 2, tft_colorB); tft.fillRect(_C_x_A, _Q_y + a * 15 + 1, _C_x_B, _W_y - 2, tft_colorC); tft.fillRect(_C_x_A1, _Q_y + a * 15 + 1, _C_x_B1, _W_y - 2, tft_colorB); tft.fillRect(_C_x_M, _Q_y + a * 15 - 1, 1, _W_y + 2, tft_colorD); } //netsta------------------ tft.fillRect(0, 158, 128, 2, node_clock_error ? tft_colorD : tft_colorC); } time.h #include "Arduino.h" //unsigned long time; unsigned long TIME1; //setup delay unsigned long time2; //send data unsigned long time3; //battery unsigned long Joy_time[2] = {0, 0}; //joy 视频