[C] 纯文本查看 复制代码
#include <Wire.h>
#include <EEPROM.h>
#include <Servo.h>
#include "Stepper.h"
#include "SoftModem.h"
SoftModem modem; //软件猫
//--------------------------------------------------------------------------------------------------
#define DEBUG 1 //是否显示调试信息
#define PI 3.14159265359 //圆周率
#define LedPin 13 //LED管脚
#define LedGps 12 //LED管脚 gps ready
#define LedHom 11 //LED管脚 home ready
#define SoundPin 8 //蜂鸣器管脚
#define SERVO_V_Pin 10 //俯仰舵机管脚
#define STEPPER_Pins 5,4,3,2 //步进电机管脚 ★请确认管脚与电路图一致★
//#define SoundPin A1 //原版程序中蜂鸣器管脚
//#define STEPPER_Pins 10,11,12,13 //原版程序中定义的步进电机管脚★
//--------------------------------------------------------------------------------------------------
#define USE_GY85 1 //是否使用电子罗盘(加电后自动调整方位)
#if USE_GY85
#include "Gy85Support.h"
#endif
//--------------------------------------------------------------------------------------------------
#define UseSoftwareSerial 0 //是否使用软串口读写信息(软串口可避免电脑串口下载程序时与蓝牙冲突)
#if UseSoftwareSerial
#include "SoftwareSerial.h"
SoftwareSerial SoftSerial(2,3); // 使用Arduino的D2、D3端口(RX, TX)作为软串口
#define ExcSerial SoftSerial
#else
#define ExcSerial Serial
#endif
//--------------------------------------------------------------------------------------------------
void EEPROM_write2(int address, int p) {byte *_pp_=(byte*)&(p);for(int i=0; i<2; i++) EEPROM.write(address+i, _pp_[i]);} //写p到EEPROM
void EEPROM_read2(int address, int& p) {byte *_pp_=(byte*)&(p);for(int i=0; i<2; i++) _pp_[i]=EEPROM.read(address+i);} //从EEPROM读取p
//--------------------------------------------------------------------------------------------------
int wxs; //有效卫星数
struct Location {
double jd; //经度
double wd; //纬度
double gd; //高度
};
struct Location HomeLoc = {0,0,0}; //家的坐标
struct Location CurrLoc = {0,0,0}; //飞机当前坐标
struct Location TempLoc = {0,0,0}; //临时坐标
//--------------------------------------------------------------------------------------------------
#define CMD_LEN 16 //命令缓冲区长度
char CmdBuf[CMD_LEN]; //命令缓冲区
String Cmd; //命令
bool IsShowLocation=false; //是否显示接收到的坐标数据
//bool IsShowLocation=true;
int AutoSetHome=8; //自动设置家的卫星数量(0不自动设置家)
int StepsOfCircuit=2100; //步进电机每转步数
bool HomeReady=false; //家是否已经设置
int MagAttitude=3; //电子罗盘姿态( 0:xy 1:yx 2:xz 3:zx 4:yz 5:zy )【正面向上时MagAttitude=0,正面向下时MagAttitude=1 standup is 3】
int QuickOrient=0; //根据电子罗盘快速定位
int tzjd=0; //安装调整角度(天线指向正北时与电子罗盘正向的夹角)
float fjspj,fjgdj; //飞机水平角(飞机与正北夹角),飞机高度角(飞机与地平面夹角),
//--------------------------------------------------------------------------------------------------
struct Servo_Dgree {
float H; //水平
float V; //俯仰
};
struct Servo_Dgree cur_dgree = {0.0, 0.0};
struct Servo_Dgree lst_dgree = {0.0, 0.0};
Stepper stepper; //水平伺服器,对应飞机方向;使用步进电机【设置步进电机的每转步数(总减速比)和连接步进电机的第1-4个arduino管脚】
Servo SERVO_V; //俯仰伺服器,对应飞机高度
int Servo_V_Rev=1; //俯仰伺服器是否反向
int SERVO_V_1=1100; //俯仰伺服器极限位置1
int SERVO_V_2=1900; //俯仰伺服器极限位置2【可通过命令调整该两项,调整俯仰舵机水平和垂直的脉宽】
//--------------------------------------------------------------------------------------------------
void QuickOrientByMag() { //快速定位正北,精度低
float tlpjd;
Gy85.GetSensorsData();
Gy85.CalcMagData();
switch( MagAttitude ) {
case 0 : tlpjd=atan2( Gy85.magRaw[0],Gy85.magRaw[1]); break;
case 1 : tlpjd=atan2(-Gy85.magRaw[1],Gy85.magRaw[0]); break;
case 2 : tlpjd=atan2( Gy85.magRaw[0],Gy85.magRaw[2]); break;
case 3 : tlpjd=atan2(-Gy85.magRaw[2],Gy85.magRaw[0]); break;
case 4 : tlpjd=atan2( Gy85.magRaw[1],Gy85.magRaw[2]); break;
case 5 : tlpjd=atan2(-Gy85.magRaw[2],Gy85.magRaw[1]); break;
}
tlpjd=tlpjd/PI*180;
stepper.StepNumberYT=0;
stepper.GotoAngle(tlpjd+tzjd);
stepper.StepNumberYT=0;
}
//--------------------------------------------------------------------------------------------------
void OrientByMag() { //定位正北,精度高
int i,j,k=0;
float tmpv,maxv=-9999.9*9999.9;
stepper.MoveSteps(1.2*stepper.Phantom+8);
stepper.StepNumberYT=0;
for(i=1;i<=stepper.StepsOfCircuit;i++) {
stepper.MoveSteps(1);
for(j=0,tmpv=0;j<4;j++) { //取值4次,避免扰动
Gy85.GetSensorsData();
switch( MagAttitude ) {
case 1 :
case 3 : tmpv+=Gy85.magRaw[0]; break;
case 0 :
case 5 : tmpv+=Gy85.magRaw[1]; break;
case 2 :
case 4 : tmpv+=Gy85.magRaw[2]; break;
}
}
if( tmpv>maxv ) { maxv=tmpv; k=i; }
}
k += stepper.AngleToSteps(tzjd);
stepper.MoveSteps( stepper.SimplifySteps(k) );
stepper.StepNumberYT=0;
}
//--------------------------------------------------------------------------------------------------
void setup() {
ExcSerial.begin(9600); //与手机蓝牙波特率一致
modem.begin();
stepper.Init(StepsOfCircuit, STEPPER_Pins);
ReadConfig();
stepper.SetSpeed(9);
pinMode(LedPin,OUTPUT);
pinMode(LedGps,OUTPUT);
pinMode(LedHom,OUTPUT);
delay(2500); //延时,在此期间手动调整俯仰伺服器使天线指向水平至45度之间,减少伺服器损伤
SERVO_V.attach(SERVO_V_Pin);
cur_dgree.H = 0.0; stepper.StepNumberYT=0;
lst_dgree.V = 89.0; cur_dgree.V = 90.0;
Servo_Move(); //调用Servo_Update可能会是水平位置,所以不用
delay(500); //等待俯仰伺服器运动到位
#if USE_GY85
Gy85.Init();
if( QuickOrient ) QuickOrientByMag(); //根据电子罗盘快速定位正北
else OrientByMag(); //自检后旋转一周,测出地磁北后,根据安装调整角驱动步进电机使天线指北
#endif
fjspj=0.0; fjgdj=0.0;
Servo_Init(); //天线指向水平(正北)
}
//--------------------------------------------------------------------------------------------------
int GPS_UPDATE; //GPS信息是否更新,false:无更新;true:坐标更新
unsigned long CurrTime,GPSUpdateTime=0,SoundTime=0; //当前时间,GPS信息更新时间,上次高音提示收到有效数据时间
int wxsj=-10; //无效数据连续提示次数
//--------------------------------------------------------------------------------------------------
void loop() {
GPS_UPDATE=ReadGpsInfo();
CurrTime=millis();
if( GPS_UPDATE>0 ) { //收到有效信息
digitalWrite(LedGps,HIGH);
GPSUpdateTime=CurrTime;
wxsj=-10;
/// if( CurrTime-SoundTime>1000 ) { //每秒最多提示1次
/// SoundTime=CurrTime;
/// Spiel(SoundPin,"A",50); //高音提示
/// }
// if( GPS_UPDATE ) { //收到有效坐标
if( IsShowLocation ) ShowLocation(TempLoc);
if( !HomeReady && AutoSetHome>0 && wxs>=AutoSetHome ) { //自动设置家(卫星数达到自动设置家的值)
HomeLoc=CurrLoc=TempLoc;
HomeReady=true;
digitalWrite(LedHom,HIGH);
Spiel(SoundPin,"135A",300);
delay(400);
digitalWrite(LedHom,LOW);
delay(400);
digitalWrite(LedHom,HIGH);
delay(400);
digitalWrite(LedHom,LOW);
delay(400);
digitalWrite(LedHom,HIGH);
delay(400);
digitalWrite(LedHom,LOW);
delay(400);
digitalWrite(LedHom,HIGH);
}
if( HomeReady ) {
if( TempLoc.jd!=CurrLoc.jd || TempLoc.wd!=CurrLoc.wd || TempLoc.gd!=CurrLoc.gd ) {
CurrLoc=TempLoc;
CalcAirplaneDgree();
CalcServoDgree();
}
// }
}
digitalWrite(LedGps,LOW);
}
// stepper.MoveToObj(1);
if( CurrTime-GPSUpdateTime>6000 ) { //6秒内未收到有效GPS数据
GPSUpdateTime=CurrTime-2000; //每隔2秒报警一次
digitalWrite(LedPin,HIGH);
wxsj++;
Spiel(SoundPin,"a",50); //低音
/// digitalWrite(LedGps,LOW);/////
}
digitalWrite(LedPin,LOW);
if( ExcSerial.available()>0) { //检测并执行命令
digitalWrite(LedPin,HIGH);
int buflen=ExcSerial.readBytes(CmdBuf,CMD_LEN-1);
ExcSerial.flush();
CmdBuf[buflen]=0;
while( --buflen>=0 ) {
if( CmdBuf[buflen]=='\r' || CmdBuf[buflen]=='\n' )
CmdBuf[buflen]=0;
}
if( CmdBuf[0]<'A' || CmdBuf[0]>'Z' ) Cmd=CmdBuf; //部分手机助手会再命令前增加字符'A'-'Z'
else Cmd=&(CmdBuf[1]);
if( ExecCmd(Cmd) ) ExcSerial.println("Ok!");
else ExcSerial.println("Err!!!");
digitalWrite(LedPin,LOW);
}
}
//--------------------------------------------------------------------------------------------------
void AdjustAngle(int angle) { //调整角度,逆时针为正
int a=stepper.AngleToSteps(angle);
int c=stepper.StepNumberYT;
stepper.MoveSteps( stepper.SimplifySteps(a) );
stepper.StepNumberYT=c;
tzjd+=angle;
SaveConfig();
}
//--------------------------------------------------------------------------------------------------
bool ExecCmd(String cmd) {
ExcSerial.println("Exec Command '"+cmd+"'...");
if( Cmd=="h" ) { //手动设置家(4颗星以上即可)
if( wxs<4 ) {
ExcSerial.println("Satellitic not enough!");
return false;
}
HomeLoc=CurrLoc=TempLoc;
HomeReady=true;
ShowLocation(HomeLoc);
digitalWrite(LedHom,HIGH);
Spiel(SoundPin,"135A",300); //成功设置家后演奏音乐
delay(400);
digitalWrite(LedHom,LOW);
delay(400);
digitalWrite(LedHom,HIGH);
delay(400);
digitalWrite(LedHom,LOW);
delay(400);
digitalWrite(LedHom,HIGH);
delay(400);
digitalWrite(LedHom,LOW);
delay(400);
digitalWrite(LedHom,HIGH);
}
else if( Cmd=="i" ) { //初始化伺服器参数
SERVO_V_1=1100;
SERVO_V_2=1900;
Servo_V_Rev=1;
MagAttitude=3;
QuickOrient=0; //根据电子罗盘快速定位
tzjd=0;
AutoSetHome=8; //自动设置家的卫星数量(0不自动设置家)
StepsOfCircuit=2100;
SaveConfig();
}
else if( Cmd=="c" ) { //清空家数据
HomeReady=false;
HomeLoc.jd=0; HomeLoc.wd=0; HomeLoc.gd=0;
}
else if( Cmd=="s" ) { //显示接收到的坐标数据
ShowLocation(TempLoc);
}
else if( Cmd=="sh" ) { //显示家的坐标数据
ShowLocation(HomeLoc);
}
else if( Cmd=="js" ) { //开始监视,显示接收到的坐标数据
IsShowLocation=true;
}
else if( Cmd=="tz" ) { //停止监视
IsShowLocation=false;
}
else if( Cmd=="spfx" ) { //水平伺服器反向
stepper.RevDir = stepper.RevDir==0 ? 1 : 0 ;
SaveConfig();
}
else if( Cmd=="fyfx" ) { //俯仰伺服器反向
Servo_V_Rev = Servo_V_Rev==0 ? 1 : 0 ;
Servo_Move();
SaveConfig();
}
else if( Cmd=="qo" ) { //是否根据电子罗盘快速定位
QuickOrient = QuickOrient==0 ? 1 : 0 ;
SaveConfig();
}
else if( Cmd=="dw" ) { //重新定位
if( QuickOrient ) QuickOrientByMag(); //根据电子罗盘快速定位正北
else OrientByMag(); //自检后旋转一周,测出地磁北后,根据安装调整角驱动步进电机使天线指北
}
else if( Cmd=="ma" ) { //电子罗盘姿态变更
if( ++MagAttitude>=6 ) MagAttitude=0;
SaveConfig();
}
else if( Cmd=="bs+" ) { StepsOfCircuit+=1; SaveConfig(); } //步进电机每转步数+1
else if( Cmd=="bs++" ) { StepsOfCircuit+=9; SaveConfig(); } //步进电机每转步数+9
else if( Cmd=="bs-" ) { StepsOfCircuit-=1; SaveConfig(); } //步进电机每转步数-1
else if( Cmd=="bs--" ) { StepsOfCircuit-=9; SaveConfig(); } //步进电机每转步数-9
else if( Cmd=="xw+" ) { stepper.Phantom+=1; stepper.TestPhantom(); SaveConfig(); } //步进电机虚位+1
else if( Cmd=="xw++" ) { stepper.Phantom+=9; stepper.TestPhantom(); SaveConfig(); } //步进电机虚位+9
else if( Cmd=="xw-" ) { stepper.Phantom-=1; stepper.TestPhantom(); SaveConfig(); } //步进电机虚位-1
else if( Cmd=="xw--" ) { stepper.Phantom-=9; stepper.TestPhantom(); SaveConfig(); } //步进电机虚位-9
else if( Cmd=="jd+" ) AdjustAngle(1); //安装调整角度+1
else if( Cmd=="jd++" ) AdjustAngle(5); //安装调整角度+5
else if( Cmd=="jd+++") AdjustAngle(50); //安装调整角度+50
else if( Cmd=="jd-" ) AdjustAngle(-1); //安装调整角度-1
else if( Cmd=="jd--" ) AdjustAngle(-5); //安装调整角度-5
else if( Cmd=="jd---") AdjustAngle(-50); //安装调整角度-50
else if( Cmd=="fy1+" ) { SERVO_V_1+=5; SaveConfig(); cur_dgree.V=0; Servo_Update(); } //俯仰伺服器极限位置1脉宽+5
else if( Cmd=="fy1++") { SERVO_V_1+=50; SaveConfig(); cur_dgree.V=0; Servo_Update(); } //俯仰伺服器极限位置1脉宽+50
else if( Cmd=="fy1-" ) { SERVO_V_1-=5; SaveConfig(); cur_dgree.V=0; Servo_Update(); } //俯仰伺服器极限位置1脉宽-5
else if( Cmd=="fy1--") { SERVO_V_1-=50; SaveConfig(); cur_dgree.V=0; Servo_Update(); } //俯仰伺服器极限位置1脉宽-50
else if( Cmd=="fy2+" ) { SERVO_V_2+=5; SaveConfig(); cur_dgree.V=90; Servo_Update(); } //俯仰伺服器极限位置2脉宽+5
else if( Cmd=="fy2++") { SERVO_V_2+=50; SaveConfig(); cur_dgree.V=90; Servo_Update(); } //俯仰伺服器极限位置2脉宽+50
else if( Cmd=="fy2-" ) { SERVO_V_2-=5; SaveConfig(); cur_dgree.V=90; Servo_Update(); } //俯仰伺服器极限位置2脉宽-5
else if( Cmd=="fy2--") { SERVO_V_2-=50; SaveConfig(); cur_dgree.V=90; Servo_Update(); } //俯仰伺服器极限位置2脉宽-50
else if( Cmd=="spcs" ) { //测试水平伺服器
stepper.GotoAngle(-90);
stepper.GotoAngle(0);
stepper.GotoAngle(90);
stepper.GotoAngle(0);
}
else if( Cmd=="fycs" ) { //测试俯仰伺服器
cur_dgree.V = 90; Servo_Move();
cur_dgree.V = 0; Servo_Move();
}
else if( Cmd=="cs" ) TestAAT(2000); //测试跟踪
else if( Cmd=="zdszj" ) { //自动设置家
AutoSetHome--;
if( AutoSetHome<0 ) AutoSetHome=8;
if( AutoSetHome<4 ) AutoSetHome=0;
SaveConfig();
}
else if( Cmd=="?" ) { //显示帮助信息
ExcSerial.println("h/c;d/s/sh;js/tz;zdszj;cs;q");
ExcSerial.println("[sp/fy][fx/cs/1/2];jd;bs/xw;ma");
}
#if DEBUG
else if( Cmd=="d" ) { //显示调试信息
ExcSerial.println("Servo_H_Rev=" + String(stepper.RevDir));
ExcSerial.println("Servo_V_Rev=" + String(Servo_V_Rev));
ExcSerial.println("SERVO_V_1=" + String(SERVO_V_1));
ExcSerial.println("SERVO_V_2=" + String(SERVO_V_2));
ExcSerial.println("Auto Home=" + String(AutoSetHome));
ExcSerial.print("H="); ExcSerial.println(lst_dgree.H);
ExcSerial.print("V="); ExcSerial.println(lst_dgree.V);
ExcSerial.println("Adj Angle=" + String(tzjd));
ExcSerial.println("MagAttitude=" + String(MagAttitude));
ExcSerial.println("Steps/c=" + String(StepsOfCircuit));
ExcSerial.println("Phantom=" + String(stepper.Phantom));
ExcSerial.println("QuickOrient=" + String(QuickOrient));
}
#endif
else {
ExcSerial.println("No such command!");
return false;
}
#if DEBUG
checkFreeMem();
#endif
return true;
}
//--------------------------------------------------------------------------------------------------
void SaveConfig() {
EEPROM_write2(10,stepper.RevDir);
EEPROM_write2(12,Servo_V_Rev);
EEPROM_write2(14,SERVO_V_1);
EEPROM_write2(16,SERVO_V_2);
EEPROM_write2(18,stepper.Phantom);
EEPROM_write2(20,QuickOrient);
EEPROM_write2(22,AutoSetHome);
EEPROM_write2(24,MagAttitude);
while( tzjd<0 ) tzjd+=360;
while( tzjd>360 ) tzjd-=360;
EEPROM_write2(26,tzjd);
EEPROM_write2(28,StepsOfCircuit);
}
//--------------------------------------------------------------------------------------------------
void ReadConfig() {
int bd=0x55a3,tl;
EEPROM_read2(1,tl);
if( bd!=tl ) { //First Run
EEPROM_write2(1,bd);
SaveConfig();
}
else {
EEPROM_read2(10,stepper.RevDir);
EEPROM_read2(12,Servo_V_Rev);
EEPROM_read2(14,SERVO_V_1);
EEPROM_read2(16,SERVO_V_2);
EEPROM_read2(18,stepper.Phantom);
EEPROM_read2(20,QuickOrient);
EEPROM_read2(22,AutoSetHome);
EEPROM_read2(24,MagAttitude);
EEPROM_read2(26,tzjd);
EEPROM_read2(28,StepsOfCircuit);
}
}
//--------------------------------------------------------------------------------------------------
#if DEBUG
void checkFreeMem() {
for(int i = 2040;i>0;i--) {
char* c = (char*)malloc(i);
if(c) {
free(c);
ExcSerial.print("Mem left:");
ExcSerial.println(i);
break;
}
}
}
#endif
//--------------------------------------------------------------------------------------------------