枪控单手模式的实现 - 5iMX.com 我爱模型玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年）

/*
名称：枪控的单手控制转向和油门
功能：利用陀螺仪的X轴偏转代替枪式遥控器手轮的旋转，满足单手控制油门和方向的需求
硬件： Arduino Pro Micro，30元
陀螺仪： JY901，100元
DAC： MCP4725，10元
附件： FUTABA 方向盘下拉器，100元。FUTABA 7PX 需要此附件内安装以上3个模块
作者：武汉关山-小孙
QQ： 271198361
日期： 2019/12/2
版本： 1.0
陀螺仪与Arduino的接线
JY901 Arduino
SDA <---> SDA
SCL <---> SCL
MCP4725通过JY901串接至Arduino, 连线方便
MCP4725 JY901
SDA <---> SDA
SCL <---> SCL
*/
//#define HARD_DEBUG //如此行未被注释，开启USB串口的调试输出
//#define SOFT_DEBUG //如此行未被注释，开启软串口的调试输出
#ifdef SOFT_DEBUG //使用软件模拟的串口查看调试信息，使用如“USR-TCP232”工具软件查看
#include <SoftwareSerial.h> //软串口库文件
SoftwareSerial mySerial(14, 15); //RX接收, TX发送
#endif
#include <EEPROM.h> //基础库文件，保存变量
#include <Wire.h> //基础库文件
#include <JY901.h> //陀螺仪库文件
#include <Adafruit_MCP4725.h> //MCP4725库文件
Adafruit_MCP4725 dac_x; //创建MCP4725的类型变量
#define MCP4725_MIN 0 //12位精度时最小值
#define MCP4725_MAX 4095 //12位精度时最大值
#define MCP4725_HALF MCP4725_MAX / 2 //12位精度的一半
#define SURFACE_PIN A7 //手轮，手轮电位器输出线连接到Arduino的端口号，接模拟输入
#define SURFACE_X_CENTER 522 //522，为开发期间调试时检测到FUTABA 7PX手轮的中立点
#define SURFACE_X_DIEZONE 5 /*手轮，死区。即会忽略的轻微的动作量范围。
最小0，最大不宜超过20。
过大，手轮反应会感觉迟钝。
过小，当手轮的电位器中立点跳动较大时，易导致认为手轮有动作而忽略陀螺仪传递的变化值*/
#define SURFACE_IDLE_TIME 2500 //手轮空闲时间，在此时间内手轮无动作后，切换至陀螺仪控制，单位：毫秒
#define GYRO_INITIALIZE_TIME 1500 //等待陀螺仪启动和初始化，如无此等待程序将出现异常，单位：毫秒
#define STATISTICS_COUNT 500 //统计取样数，用统计方法计算中立点
#define GYRO_X_MIN -4500 /*陀螺仪x轴，最大允许‘逆时针’时旋转的度数（数值除100，为实际度数）
角度过大，手腕旋转不过来，且控制速度过慢，太伤手腕
角度过小，手腕旋转速度快，但控制精度降低
可根据实际情况调整
最大，9000，即90度
最小，0。即禁用陀螺仪控制。*/
#define GYRO_X_MAX 4500 //陀螺仪x轴，最大允许‘顺时针’时旋转的度数。其他同上变量说明。
#define GYRO_Y_MAX 3500 //陀螺仪y轴，当枪控被‘举起’的角度，正数
#define GYRO_Y_MIN -7500 //陀螺仪y轴，当枪控被‘放下’的角度，负数
//符合此值说明的条件：左手控制油门，右手控制手轮。
//如果手轮的安装与此相反，上述两值取反。
int surface_x = 0 ; //手轮，读取到手轮的电压值
int surface_x_max = 0 ; //手轮，电压值最大值
int surface_x_min = 0 ; //手轮，电压值最小值
int surface_x_center = 0 ; //手轮，中立点
unsigned long surface_x_sum = 0 ; //手轮，中立点的平均值汇总
unsigned int surface_x_c_left = 0 ; //手轮，轻微动作，小于中立点
unsigned int surface_x_c_right= 0 ; //手轮，轻微动作，大于中立点
unsigned long surface_action_time = 0 ; //手轮，有动作的开始时间
bool gyro_lead = true ; //陀螺仪，优先状态。默认优先陀螺仪控制转向。
float gyro_x = 0 ; //陀螺仪x轴，有符号数值
float gyro_y = 0 ; //陀螺仪y轴，有符号数值
float gyro_x_min = 0 ; //陀螺仪x轴，实际参与计算的变量
float gyro_x_max = 0 ; //陀螺仪x轴，实际参与计算的变量
float gyro_y_min = 0 ; //陀螺仪y轴，实际参与计算的变量
float gyro_y_max = 0 ; //陀螺仪y轴，实际参与计算的变量
long gyro_x_sum = 0 ; //陀螺仪x轴，缺省为0度
long gyro_y_sum = 0 ; //陀螺仪y轴，缺省为0度
int gyro_x_center = 0 ; //陀螺仪x轴，经计算后的中立点
int gyro_y_center = 0 ; //陀螺仪y轴，经计算后的中立点
int final_x = 0 ; /*最终输出的x值
使用MCP 4275，输出精度为12位，最大值4096。
如使用Arduino自带的PWM模拟输出，会看到遥控器的舵量条有严重抖动*/
union save_data{
int val;
byte v[2];
};
save_data surface_x_save ;
//性能统计
//unsigned long process_count = 0 ; //测试程序性能计数器
//unsigned long system_begin_time = 0 ;
void setup() {
//如果更换了ADC模块，导致手轮的最大值和最小值，从大值变为了小值，需要将EEPROM清零
//此功能代码还需要另写
//从EEPROM读取保存过的手轮的最大值和最小值
for(int n=0;n<2;n++) surface_x_save.v[n]=EEPROM.read(n);
surface_x_save.v[0]=EEPROM.read(0);
surface_x_save.v[1]=EEPROM.read(1);
surface_x_max = surface_x_save.val; //手轮，记录到的旋转最大值
surface_x_save.v[0]=EEPROM.read(2);
surface_x_save.v[1]=EEPROM.read(3);
surface_x_min = surface_x_save.val; //手轮，记录到的旋转最小值
if(surface_x_min == 0) surface_x_min=10000 ;
JY901.StartIIC(); //陀螺仪初始化
dac_x.begin(0x61); //输出X轴的DAC初始化
#ifdef HARD_DEBUG
Serial.begin(9600);
#endif
#ifdef SOFT_DEBUG
mySerial.begin(9600);
#endif
delay(GYRO_INITIALIZE_TIME); //必要等待，让陀螺仪启动和初始化，否则后续程序可能不会执行
//计算中立点
calc_center();
//X及Y轴的中立点，不能偏移超过设定的限定值
while( gyro_x_center < GYRO_X_MIN ){
show(MCP4725_MIN, 1000 ); //最大左转
delay(1500);
calc_center();
}
while( gyro_x_center > GYRO_X_MAX ){
show(MCP4725_MAX, 1000 ); //最大右转
delay(1500);
calc_center();
}
while( gyro_y_center < GYRO_Y_MIN ){
show(MCP4725_MAX/4, 1000 ); //中度左转
delay(1500);
calc_center();
}
while( gyro_y_center > GYRO_Y_MAX ){
show(MCP4725_MAX/4*3, 1000 ); //中度右转
delay(1500);
calc_center();
}
//中立点计算结束，展示两个转向端点
show(MCP4725_MIN, 100 );//最大左转
show(MCP4725_MAX, 100 );//最大右转
#ifdef HARD_DEBUG
Serial.println(); Serial.println();
Serial.print("Center:");
Serial.print(surface_x_center);Serial.print(",");
Serial.print(gyro_x_center);Serial.print(",");
Serial.print(gyro_y_center);Serial.print(",");
Serial.println();
Serial.print("S_x_sum:");Serial.print(surface_x_sum);Serial.print(",");
Serial.print("G_x_sum:");Serial.print(gyro_x_sum);Serial.print(",");
Serial.print("F_y_sum:");Serial.print(gyro_y_sum);Serial.print(",");
Serial.print("COUNT:");Serial.print(STATISTICS_COUNT);
Serial.println(); Serial.println();
Serial.println("Begin:");
#endif
//性能统计
//system_begin_time = millis();
}
void loop() {
if( gyro_lead ){ //默认由陀螺仪进行控制
//2019/11/13 从陀螺仪取值，最多每 0.0716 秒输出一次控制量
//2019/11/21 10/8347= 0.001198s/次 //将同时读取手轮电压值分离
//2019/12/2 10/8364= 0.001195s/次 //可能电压变化，可视无差别
//手轮无动作时间后才取陀螺仪的各值，减少有手轮控制时又去取陀螺仪各值的无用cpu消耗
JY901.GetAngle(); //取陀螺仪的角度值
gyro_x = (float)JY901.stcAngle.Angle[0]/32768*18000 ; //陀螺仪，X轴绝对值。度数放大100倍，用于提高用角度进行map转换的精度
gyro_y = (float)JY901.stcAngle.Angle[1]/32768*18000 ; //陀螺仪，Y轴绝对值。度数放大100倍
//gyro_z = (float)JY901.stcAngle.Angle[2]/32768*180 ; //陀螺仪，Z轴绝对值。原始度数
if(gyro_x < gyro_x_min ) gyro_x = gyro_x_min ; //限定逆时针时的旋转最大度角
if(gyro_x > gyro_x_max ) gyro_x = gyro_x_max ; //限定顺时针时的旋转最大度角
/*当枪控被举起（Y轴为正数）或放下（Y轴为负数）的角度大于设定值时
1，将X轴的输出值置回中立点，让转向快速回到中立点，是否能引出特殊控制方式，效果待测
2，只有此时，手轮才‘将’可控。
鉴于arduino的analogRead函数工作原理。理论每秒最多取值一万次，实际最多约接近九千次/秒。
如果每个循环周期不论如何都读取一次手轮的电压值，实际却是在用陀螺仪做控制，这将导致无意义的大量cpu被消耗。
将实时读取手轮的电压值，改为枪控被举起或放下后的一段时间内转向才受手轮的控制。
整体性能提升约40%，得到控制量更细腻，舵量条变量更顺滑。
3，能让手臂休息一下，避免产生误操作。
*/
if(gyro_y < gyro_y_min or gyro_y > gyro_y_max ){ //当枪控被举起或放下
gyro_x = gyro_x_center; //转向回到中立点
gyro_lead = false ; //优先手轮
surface_action_time = millis() ; //确定手轮将可以动作的起始时间
}
//使用陀螺仪的角度值
final_x = map(gyro_x, gyro_x_min, gyro_x_max, MCP4725_MIN, MCP4725_MAX);
}else{
//2019/11/13 从手轮取值，最多每 0.0167 秒输出一次控制量
//2019/11/21 10/37845= 0.000264s/次 //大幅调整读取逻辑
//2019/12/2 10/37709= 0.000265s/次 //增加最大及最小值保存至EEPROM
surface_x = analogRead(SURFACE_PIN) ; //读取手轮的电压值
if(surface_x > surface_x_max ){ //取手轮电压值的最大值
surface_x_max = surface_x ;
surface_x_save.val = surface_x ; //保存手轮的电压值最大值
EEPROM.write(0, surface_x_save.v[0]);
EEPROM.write(1, surface_x_save.v[1]);
}
if(surface_x < surface_x_min ){ //取手轮电压值的最小值
surface_x_min = surface_x ;
surface_x_save.val = surface_x ; //保存手轮的电压值最小值
EEPROM.write(2, surface_x_save.v[0]);
EEPROM.write(3, surface_x_save.v[1]);
}
if(surface_x < surface_x_c_left or surface_x > surface_x_c_right ){//手轮有动作
//更新手轮动作时的发生时间，能持续一段时间让手轮控制转向
surface_action_time = millis();
//使用手轮的电压值
final_x = map(surface_x, surface_x_min, surface_x_max, MCP4725_MIN, MCP4725_MAX);
}else{
//手轮没有动作时，即认为在中立点。使用DAC最大值的一半作为输出值
final_x = MCP4725_HALF ;
//重新让陀螺仪优先控制
if( millis() - surface_action_time > SURFACE_IDLE_TIME ) gyro_lead = true ;
}
}
#ifdef HARD_DEBUG
Serial.print("G_x:");Serial.print(gyro_x);Serial.print(",");
Serial.print("G_x_min:");Serial.print(gyro_x_min);Serial.print(",");
Serial.print("G_x_max:");Serial.print(gyro_x_max);Serial.print(",");
Serial.print("G_x_c:");Serial.print(gyro_x_center);Serial.print(",");
Serial.print("G_y:");Serial.print(gyro_y);Serial.print(",");
Serial.print("G_y_min:");Serial.print(gyro_y_min);Serial.print(",");
Serial.print("G_y_max:");Serial.print(gyro_y_max);Serial.print(",");
Serial.print("G_y_c:");Serial.print(gyro_y_center);Serial.print(",");
Serial.print("S_x:");Serial.print(surface_x);Serial.print(",");
Serial.print("S_max:");Serial.print(surface_x_max);Serial.print(",");
Serial.print("S_min:");Serial.print(surface_x_min);Serial.print(",");
Serial.print("S_c:");Serial.print(surface_x_center);Serial.print(",");
Serial.print("S_L:");Serial.print(surface_x_c_left);Serial.print(",");
Serial.print("S_R:");Serial.print(surface_x_c_right);Serial.print(",");
Serial.print("x:"); Serial.println(final_x);
delay(100);
#endif
#ifdef SOFT_DEBUG
mySerial.print("G_x:");mySerial.print(gyro_x);mySerial.print(",");
mySerial.print("G_x_min:");mySerial.print(gyro_x_min);mySerial.print(",");
mySerial.print("G_x_max:");mySerial.print(gyro_x_max);mySerial.print(",");
mySerial.print("G_x_c:");mySerial.print(gyro_x_center);mySerial.print(",");
mySerial.print("G_y:");mySerial.print(gyro_y);mySerial.print(",");
mySerial.print("G_y_min:");mySerial.print(gyro_y_min);mySerial.print(",");
mySerial.print("G_y_max:");mySerial.print(gyro_y_max);mySerial.print(",");
mySerial.print("G_y_c:");mySerial.print(gyro_y_center);mySerial.print(",");
mySerial.print("S_x:");mySerial.print(surface_x);mySerial.print(",");
mySerial.print("S_max:");mySerial.print(surface_x_max);mySerial.print(",");
mySerial.print("S_min:");mySerial.print(surface_x_min);mySerial.print(",");
mySerial.print("S_c:");mySerial.print(surface_x_center);mySerial.print(",");
mySerial.print("X:");mySerial.print(final_x, DEC);
mySerial.println();
delay(100);
#endif
dac_x.setVoltage(final_x, false); //输出转向
//性能统计
/* process_count++ ;
if(millis() - system_begin_time > 10000){
Serial.print(process_count);
while(1);
} */
}
void calc_center() {
/* 计算中立点
初始化完成后，计算手轮和陀螺仪X、Y轴的中立点
即：在打开枪控电源后，保持遥控器处于水平位置不动，并且不要旋转手轮。
直待转向轮左右摇摆一次并回中后，中立点计算完成。
主要目的：
陀螺仪芯片在枪控里安装的位置狭小，难以保证绝对的水平和垂直。
为了实现即使有点歪斜也不影响使用，让系统自动计算中立点。
但为了使用感受，最好还是让遥控器在水平的位置计算各轴的相对中立点。*/
surface_x_sum = 0 ;
gyro_x_sum = 0 ;
gyro_y_sum = 0 ;
for(int i = 0 ; i < STATISTICS_COUNT ; i++){
JY901.GetAngle(); //取陀螺仪生产的各值
gyro_x = (float)JY901.stcAngle.Angle[0]/32768*18000 ; //陀螺仪，读X轴。度数放大100倍，用于提高用角度进行map转换的精度
gyro_y = (float)JY901.stcAngle.Angle[1]/32768*18000 ; //陀螺仪，读Y轴。度数放大100倍
//gyro_z = (float)JY901.stcAngle.Angle[2]/32768*180 ; //陀螺仪，读Z轴。原始度数
surface_x_sum += analogRead(SURFACE_PIN) ; //累加手轮的中立点值
gyro_x_sum += gyro_x ; //累加陀螺仪X轴的中立点值
gyro_y_sum += gyro_y ; //累加陀螺仪Y轴的中立点值
#ifdef HARD_DEBUG
Serial.print("surface_x_sum:");Serial.print(surface_x_sum);Serial.print("\t");
Serial.print("gyro_x_sum:");Serial.print(gyro_x_sum);Serial.print("\t");
Serial.print("gyro_y_sum:");Serial.println(gyro_y_sum);
#endif
}
//计算手轮的中立点
if(surface_x_sum == 0 ) surface_x_center = SURFACE_X_CENTER ; else surface_x_center = surface_x_sum / STATISTICS_COUNT ;
//计算陀螺仪X轴的中立点
if(gyro_x_sum == 0 ) gyro_x_center = 0 ; else gyro_x_center = gyro_x_sum / STATISTICS_COUNT ;
//计算陀螺仪Y轴的中立点
if(gyro_y_sum == 0 ) gyro_y_center = 0 ; else gyro_y_center = gyro_y_sum / STATISTICS_COUNT ;
surface_x_c_left = surface_x_center - SURFACE_X_DIEZONE ;
surface_x_c_right = surface_x_center + SURFACE_X_DIEZONE ;
gyro_x_min = GYRO_X_MIN + gyro_x_center;
gyro_x_max = GYRO_X_MAX + gyro_x_center;
gyro_y_min = GYRO_Y_MIN + gyro_y_center;
gyro_y_max = GYRO_Y_MAX + gyro_y_center;
}
void show(int v , int t ) {
dac_x.setVoltage(v, false);
delay(t);
dac_x.setVoltage(MCP4725_MAX/2, false);
delay(t);
}

复制代码

名称	型号	数量	单位	价格
手轮下拉器	FUTABA 7PX适用	1	个	88
陀螺仪	JY901	1	块	98
DAC	MCP 4725	1	块	4.3
MCU	Arduino Pro Micro	1	块	26
合计				216.3

5iMX.com 我爱模型 玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年）

5iMX.com 我爱模型玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年）