#include<Servo.h>
Servo servol; //建立一个舵机对象
unsigned char ch;
unsigned char selc[6];//查询专用数组
int sel=0;//查询专用计数
unsigned char comdata[200];//原始数据数据数组
int i ;
int j ;
int Z ;
int x;//距离值计数
float distance[100];
float this_dm;//本次一个数组内最小距离值
float last_dm=3000;//上次一个数组内最小距离值
float this_angle;
float last_angle;
float dset=1000;//警报距离的设定值
int dx; //最大距离值对应的x值
float q;//存放舵机指向角度
int ;//坐标转换后，舵机旋转角度
int h=145;//雷达坐标原点与舵机转动原点间距离q
int N ;//有效数据长度
long leng ;//一帧数据长度
float ogo ;//零点偏移量
float S; //数据帧初始起始角度值
float RS; //数据帧真实起始角度值
int sangle;//舵机指向角度
int alarm=13;//警报器引脚8
void setup()
{
/*************建立查询用数组，存放初始角度值****************/
selc[0]=0x08; //代表22.5°的初始角度
selc[1]=0x11;
selc[2]=0x1a;
selc[3]=0x23;
selc[4]=0x2b;
selc[5]=0x34;
Serial.begin(115200); //波特率为115200
servol.attach(8); //舵机连接引脚5
servol.write(90); //设置舵机初始位置
pinMode(alarm,OUTPUT); //警报器引脚为输出模式
}
void loop()
{
Z=1;
Serial.println( "sel:");
Serial.println( sel);
ODRead(); //读取原始数据，返回数组comdata[]
/*for(x=0;x<(leng+3);x++)
{Serial.println("the x is:");
Serial.println( x);
Serial.println(comdata[x],HEX);
}*/
N=(leng-13)/3; //有效数据长度比一帧数据总长度少13个字节，每组有效数据包含三个字节
ogo=(comdata[9]*256+comdata[10])/100.0; //零点偏移量是16位的有符合数
S=(comdata[11]*256+comdata[12])/100.0; //数据帧初始起始角度值，16位无符号数
RS=ogo+S; //真实起始角度
//Serial.println("the ogo is:");
//Serial.println( ogo);
//Serial.println("the S is:");
//Serial.println( S);
//Serial.println("the RS is:");
Serial.println( RS);
/*--计算距离值，判断最短距离值极其角度值，然后与上一次结果比较-----*/
j=0;
for(x=1;x<=N;x++)
{
distance[x]=(comdata[14+j]*256+comdata[15+j])*0.25;
if(distance[x]==0) //如果距离值返回为0，强行转为最大值
{distance[x]=65535;}
// Serial.println( distance[x]);
j=j+3;
} //实际距离值，单位是mm
this_dm=distance[1]; //将距离数组中第一个距离信息赋值给dm
for(x=2;x<=N;x++)
{
if(distance[x]<this_dm)
{
this_dm=distance[x]; //求这组距离信息中最小值
dx=x;
} //并记录下最小距离对应的次序，方便计算对应的角度信息
}
this_angle=(RS+22.5*(dx-1)/N); //计算对应角度信息
Serial.println( this_angle);
Serial.println( this_dm);
if(this_dm<last_dm) //本次的距离最小值和上次的相比，哪个更小。如果这次更小，那么我们就将其转换
{
last_dm=this_dm;
last_angle=this_angle;
}
/*--------------初始角度判断条件加1，然后判断是否完成了6帧数据的读取，进行警报及炮口角度控制---------------*/
sel=sel+1; //查询用的数组加1，更新我们需要查的起始角度值
//Serial.println( "sel=sel+1;");
//Serial.println( sel);
if(sel==6) //sel达到上限，22.5°到157.5°的信息算是全部记录过一遍了
{
/*--------------画重点：如果上次的最短距离值比设定值要小，那么就要采取行动了---------------------------*/
if(last_dm<dset) //距离值小于警报值，进入警报范围
{ Serial.println( last_angle);
Serial.println( last_dm);
digitalWrite(alarm,HIGH); //进入警报范围，拉响警报
q=last_angle;
q=q*PI/180;//转弧度制
q=atan((last_dm*sin(q)-h)/(last_dm*cos(q)));//我自己写的坐标转换公式
if(last_angle<90){
q=int(q*180/PI+5);}
else{
q=int(q*180/PI+180);}
servol.write(q) ; //舵机指向
Serial.println(q);
delay(15); //延时15ms让舵机转到制定位置
digitalWrite(alarm,HIGH); //进入警报范围，拉响警报
}
else
{
digitalWrite(alarm,LOW); //没有进入警报，关闭警报
}
sel=0; //初始角度设定值归位，重新开始查询
last_dm=3000;
}
}
unsigned char ODRead()
{
while (Z==1)
{
ch = Serial.read(); //读串口发来的字节
if(ch == 0xaa)//如果这个信号是aa
{
comdata[0] = ch;//数组第零位赋值aa
//Serial.println("the num 0 is:");
// Serial.println( comdata[0][0],HEX);
comdata[1]= Serial.read();
// Serial.println("the num 1 is:");
// Serial.println( comdata[1],HEX);
leng=comdata[2]= Serial.read();
// Serial.println("the num 2 is:");
// Serial.println( comdata[2],HEX);
comdata[3]= Serial.read();
comdata[4]= Serial.read();
comdata[5]= Serial.read();
comdata[6]= Serial.read();
comdata[7]= Serial.read();
comdata[8]= Serial.read();
comdata[9]= Serial.read();
comdata[10]= Serial.read();
comdata[11]= Serial.read();
if(comdata[11]==selc[sel])
{ //Serial.println(selc[sel],HEX);
for(i=12;i<(leng+3);i++)
{
comdata[i]=Serial.read();
//Serial.println("the num is ");
// Serial.println(i);
// Serial.println( comdata[i],HEX);
}
Z=0;
}
}
while(Serial.read() >= 0){}//清缓存
delay(50);
}
} |
发表于 2022-3-26 21:23
楼主