本帖最后由 xcc521 于 2018-6-20 11:37 编辑
看了好几个循迹的程序感觉都不太好，得需要调二值化阈值，还不抗干扰
然后就是开始自己写了
立一个FLAG，开始搞事情
2018.06.20，第一天
分析整体架构，罗列问题，暂定一个框架，先上一个别人的程序，11路光敏电阻采集灰度值，PID控制占空比驱动舵机转向控制方向
#include "control.h"
#include "nrf.h"
#include "PWM.h"
#include "adc.h"
/*************************定义变量****************************/
u16 Location_Data_Arrays[11]={0,0,0,0,0,0,0,0,0,0,0};//位置信息数据 ，传感器的电压值
u16 Manual_Location_Data_Arrays[11]={738 ,703 , 808 , 578 , 848 , 712 , 574 , 551 , 635 , 624 , 648 };//手动设置数组，传感器黑白线判断，黑白线阈值
float G_Black_Line_Arry[5];
float G_Black_Line_Location=0;//黑线位置
float L_Location_Cnt=0;//位置计数
float Location_Parameter=0;//位置参数 计算偏移量
u32 Cross_Line_Cnt=0;
u8 Cross_Line=0;
u8 Cross_Line_Flag=0;
int TRUN_ANGLE= 100;
//定义PID相关参数
typedef struct PID_OUT
{
float DIR_Deviation;//误差
float LAST_DIR_Deviation;//上次误差
float PREV_DIR_Deviation;
float OUTPUT;//输出量
float P;
float I;
float D;
}PID;
/*****************************************************
函数名称：Line_Scan
输入：每个传感器采集到的值
输出：黑线的位置和横线的个数
功能：循线函数
****************************************************/
void Line_Scan(void)
{/***********************************
注释部分是用来打印采集到的传感器数值
************************************/
// int j=0;
Location_Data_Arrays[0]=Get_Adc(ADC1,ADC_Channel_10);
Location_Data_Arrays[1]=Get_Adc(ADC1,ADC_Channel_11);
Location_Data_Arrays[2]=Get_Adc(ADC1,ADC_Channel_12);
Location_Data_Arrays[3]=Get_Adc(ADC1,ADC_Channel_13);
Location_Data_Arrays[4]=Get_Adc(ADC1,ADC_Channel_0);
Location_Data_Arrays[5]=Get_Adc(ADC1,ADC_Channel_1);
Location_Data_Arrays[6]=Get_Adc(ADC1,ADC_Channel_2);
Location_Data_Arrays[7]=Get_Adc(ADC1,ADC_Channel_3);
Location_Data_Arrays[8]=Get_Adc(ADC1,ADC_Channel_4);
Location_Data_Arrays[9]=Get_Adc(ADC1,ADC_Channel_5);
Location_Data_Arrays[10]=Get_Adc(ADC1,ADC_Channel_14);
// for(;j<11;j++)
// {
// printf("%d ",Location_Data_Arrays[j]);
// }
// printf("
");
}
void Auto_ScanBlackLine(void)
{
int i=0;
Line_Scan();
for(;i<11;i++)
{
Manual_Location_Data_Arrays=Manual_Location_Data_Arrays*1.5;
}
}
void Lock_BlackLine(void)
{
/*0*/
if(Location_Data_Arrays[0]<Manual_Location_Data_Arrays[0])
{
L_Location_Cnt++;
Location_Parameter+=5;
}
/*1*/
if(Location_Data_Arrays[1]<Manual_Location_Data_Arrays[1])
{
L_Location_Cnt++;
Location_Parameter+=4;
}
/*2*/
if(Location_Data_Arrays[2]<Manual_Location_Data_Arrays[2])
{
L_Location_Cnt++;
Location_Parameter+=3;
}
/*3*/
if(Location_Data_Arrays[3]<Manual_Location_Data_Arrays[3])
{
L_Location_Cnt++;
Location_Parameter+=2;
}
/*4*/
if(Location_Data_Arrays[4]<Manual_Location_Data_Arrays[4])
{
L_Location_Cnt++;
Location_Parameter+=1;
}
/*5*/
if(Location_Data_Arrays[5]<Manual_Location_Data_Arrays[5])
{
L_Location_Cnt++;
Location_Parameter+=0;
}
/*6*/
if(Location_Data_Arrays[6]<Manual_Location_Data_Arrays[6])
{
L_Location_Cnt++;
Location_Parameter+=-1;
}
/*7*/
if(Location_Data_Arrays[7]<Manual_Location_Data_Arrays[7])
{
L_Location_Cnt++;
Location_Parameter+=-2;
}
/*8*/
if(Location_Data_Arrays[8]<Manual_Location_Data_Arrays[8])
{
L_Location_Cnt++;
Location_Parameter+=-3;
}
/*9*/
if(Location_Data_Arrays[9]<Manual_Location_Data_Arrays[9])
{
L_Location_Cnt++;
Location_Parameter+=-4;
}
/*10*/
if(Location_Data_Arrays[10]<Manual_Location_Data_Arrays[10])
{
L_Location_Cnt++;
Location_Parameter+=-5;
}
if(L_Location_Cnt>0)
{
if(L_Location_Cnt>4)
{
Cross_Line_Cnt++;
if(Cross_Line_Cnt>CROSSLINE_Time)
{
if(Cross_Line_Flag!=1)
{
Cross_Line++;
}
G_Black_Line_Location=(float)(Location_Parameter/L_Location_Cnt);
Cross_Line_Flag=1;
Cross_Line_Cnt=0;
}
}
else
{
Cross_Line_Cnt=0;
Cross_Line_Flag=2;
G_Black_Line_Location=(float)(Location_Parameter/L_Location_Cnt);
}
}
else if(L_Location_Cnt==0)//丢失
{
G_Black_Line_Location=0;
Cross_Line_Flag=0;
}
//printf("%d
",Cross_Line);
Location_Parameter=0;
L_Location_Cnt=0;
}
u16 ANGLE_CONEVERT(int ANGLE)
{
u16 angle=0;
if(ANGLE>100) {ANGLE=100;}
if(ANGLE<-100) {ANGLE=-100;}
angle=ANGLE+100;
angle*=10;
angle+=500;//角度转化
return angle;
}
/*****************************************************
函数名称：Diraction_Contorl
输入：黑线所在位置的偏差
输出：无
功能：方向控制函数
****************************************************/
void Diraction_Contorl(float Black_Line)
{
int In_Servor=0;
PID PID_OUT;
PID_OUT.P=-75; // 比例参数
PID_OUT.I=0.7; //积分参数
PID_OUT.D=2.5; //微分参数
PID_OUT.DIR_Deviation=Black_Line;//获取偏差
PID_OUT.OUTPUT= PID_OUT.P*PID_OUT.DIR_Deviation
-2*PID_OUT.LAST_DIR_Deviation*PID_OUT.I
+PID_OUT.D*PID_OUT.PREV_DIR_Deviation;
if(PID_OUT.OUTPUT>800) {PID_OUT.OUTPUT=800;}
if(PID_OUT.OUTPUT<-800) {PID_OUT.OUTPUT=-800;}//对输出限幅，防止超调损坏舵机
In_Servor=(int)PID_OUT.OUTPUT*RATIO; //对输出进行放大
In_Servor+=1500; //转化为舵机转动的偏角
TIM_SetCompare3(TIM4,In_Servor); //设置舵机工作脉冲
PID_OUT.PREV_DIR_Deviation=PID_OUT.LAST_DIR_Deviation; //保存前两个节拍时的偏差和前一个节拍时的偏差
PID_OUT.LAST_DIR_Deviation=PID_OUT.DIR_Deviation; //保存当前节拍时的偏差
if(Cross_Line==3)//判断横线的个数
{
Speed_Set(0,0); //停止
}
}
再上自己写的
今天写了一点
/*************************定义变量****************************/
#define Photosensitive_resistance 0 //光敏电阻
#define Infra_red 1 //红外对管
#define Linear_CCD 2 //线性CCD
#define Camera 3 //摄像头
#define Electromagnetism 4 //电磁
#define sensor_quantity 11 //识别传感器数量
#define sensor_type Photosensitive_resistance //光敏电阻类型的传感器
#define default_position (sensor_quantity-1)/2//默认中间位置传感器编号
#define Ground_Type 0 //地面类型0:黑地白线,1:白地黑线
uint16_t current_sonsor_data[sensor_quantity]; //传感器数据缓存
/*
获取传感器数据保存到current_sonsor_data
data_:传感器读取到的数据数组
*/
void get_Sonsor_data(uint16_t * data_)
{
uint8_t value;
for(value=0;value<sensor_quantity;value++)
{
current_sonsor_data[value] = *(data_ + value);
}
}
uint8_t gen_Sensor_state(uint16_t * sensor_data,uint16_t * default_sensor_data)
{
uint8_t value;
switch(sensor_type)
{
case Photosensitive_resistance: //光敏电阻类型的传感器
if(0 == Ground_Type)
{
//
}
else
{
//
}
break;
case Infra_red: //红外传感器
break;
case Linear_CCD: //线性CCD
break;
case Camera: //摄像头
break;
case Electromagnetism: //电磁
break;
default:
break;
}
}
PS:Keil上排好版的到这就对不齐了，不太适应哎。。。
先从光敏电阻开始，之前写过一个自己计算阈值的，然后自己比较误差最大的传感器编号，这样哒
int my_abs(int num)
{
return num>=0?num:(-num);
}
uint16_t no_line_data[9]={3324,2989,2892,2368,2919,2439,2981,3310,2375};
uint16_t line_data[9]={4095,4095,4095,3780,4095,3714,4095,4095,3194};
uint16_t calc_line(void)
{
//
uint16_t sensor = 0;
int error = 385;
uint8_t temp;
for(temp = 0;temp < 9;temp++)
{
if(my_abs(AVG_AD[temp]-no_line_data[temp])<error)
{
sensor &= (~1 << ( 8 - temp ));
sensor |= ( 0 << ( 8 - temp ));
printf("0_");
}
else if(my_abs(AVG_AD[temp]-line_data[temp])<error)
{
sensor &= (~1 << ( 8 - temp ));
sensor |= ( 1 << ( 8 - temp ));
printf("1_");
}
else
{
printf("X_");
}
}
printf("
");
return sensor;
}
uint16_t test_data[9];//自动计算的灰度平均值
void Test_Line(void)
{
int times = 150;
uint8_t val;
MinMaxNormalization(TrackingArry,9,50);
HAL_Delay(200);
for(val=0;val<9;val++)
{
test_data[val] = AVG_AD[val];
}
while(times--)
{
MinMaxNormalization(TrackingArry,9,50);
for(val=0;val<9;val++)
{
test_data[val] += AVG_AD[val];
test_data[val] /= 2;
}
HAL_Delay(5);
}
}
uint8_t error_test(void)//判断线的位置
{
uint8_t val,error_num;
int error[9],cur_max;
MinMaxNormalization(TrackingArry,9,50);
HAL_Delay(5);
#if LINE_COLOR //WHITE
for(val=0;val<9;val++)
{
error[val] = test_data[val] - AVG_AD[val];//计算误差
printf("%d_",error[val]);
}
if((test_data[4]-AVG_AD[4] >= 600) && (test_data[5]-AVG_AD[5] >= 600) && (test_data[6]-AVG_AD[6] >= 600))
{
return 0XFF;
}
#else //BLACK
for(val=0;val<9;val++)
{
error[val] = AVG_AD[val] - test_data[val];//计算误差
printf("%d_",error[val]);
}
#endif
printf("
");
for(val=0;val<8;val++)//对误差进行排序
{
if(val == 0)
{
error_num = error[1]>error[0]?2:1;
cur_max = error[1]>error[0]?error[1]:error[0];
}
else
{
if(error[val+1]>cur_max)
{
error_num++;
}
}
}
printf("error_num:%d_
",error_num);
return error_num;
}
大框架里面暂时还是用这个吧，后面有时间再重写，今天先到这，明天继续
{var%20f='http://v.t.sina.com.cn/share/share.php?appkey=1515056452',u=z||d.location,p=['&url=',e(u),'&title=',e(t||d.title),'&source=',e(r),'&sourceUrl=',e(l),'&content=',c||'gb2312','&pic=',e(p||'')].join('');function%20a(){if(!window.open([f,p].join(''),'mb',['toolbar=0,status=0,resizable=1,width=440,height=430,left=',(s.width-440)/2,',top=',(s.height-430)/2].join('')))u.href=[f,p].join('');};if(/Firefox/.test(navigator.userAgent))setTimeout(a,0);else%20a();})(screen,document,encodeURIComponent,'','','https://www.xiaopingtou.cn/data/attach/logo/logo.png', '推荐 xcc521 的问题《超强自适应小车循迹算法开发全过程》','https://www.xiaopingtou.net/q-132017.html','页面编码gb2312|utf-8默认gb2312'));){kind=link}
#include "control.h"
#include "nrf.h"
#include "PWM.h"
#include "adc.h"
#include "delay.h"
#include "math.h"
#include "servor.h"
uint16_t ADC_Value[12];
void Line_Scan(void)
{
ADC_Value[10]=Get_Adc(ADC1,ADC_Channel_10);
ADC_Value[9]=Get_Adc(ADC1,ADC_Channel_11);
ADC_Value[8]=Get_Adc(ADC1,ADC_Channel_12);
ADC_Value[7]=Get_Adc(ADC1,ADC_Channel_13);
ADC_Value[6]=Get_Adc(ADC1,ADC_Channel_0);
ADC_Value[5]=Get_Adc(ADC1,ADC_Channel_1);
ADC_Value[4]=Get_Adc(ADC1,ADC_Channel_4);
ADC_Value[3]=Get_Adc(ADC1,ADC_Channel_5);
ADC_Value[2]=Get_Adc(ADC1,ADC_Channel_6);
ADC_Value[1]=Get_Adc(ADC1,ADC_Channel_14);
ADC_Value[0]=Get_Adc(ADC1,ADC_Channel_15);
#if 0
int j=0;
for(;j<11;j++)
{
printf("%4d,",ADC_Value[j]);
}
printf(" ");
#endif
}
/*************************定义变量****************************/
#define Photosensitive_resistance 0 //光敏电阻
#define Infra_red 1 //红外对管
#define Linear_CCD 2 //线性CCD
#define Camera 3 //摄像头
#define Electromagnetism 4 //电磁
#define Steering_gear_steering 0 //舵机转向
#define Bilateral_differential_speed 1 //双边差速
#define Car_Type Steering_gear_steering //小车类型为舵机转向
#define Step_Motor 0 //步进驱动电机
#define DC_Motor 1 //直流电机
#define Motor_Type DC_Motor //小车电机为直流减速电机
#define sensor_quantity 11 //识别传感器数量
#define sensor_type Photosensitive_resistance //光敏电阻类型的传感器
#define default_position (sensor_quantity-1)/2//默认中间位置传感器编号
#define Ground_Type 0 //地面类型0:黑地白线,1:白地黑线
uint16_t ground_data[sensor_quantity] = {1894,1585,2060,1817,1407,1770,1213,1153,2057,1459,1733};
//uint16_t ground_data[sensor_quantity]; //地面采集的值(黑地白线时为黑 {MOD}地面上的采集值，白地黑线上时为白地上的采集值),必要时调用采集函数采集该值
int16_t line_D_value[sensor_quantity] = {947, 861, 877, 882, 711, 900, 556, 602, 947, 734, 981};
//int16_t line_D_value[sensor_quantity]; //保存(当前)采集值和地面值之间的差值
uint16_t current_sonsor_data[sensor_quantity];//传感器当前采集数据缓存
float calc_Proportion = 0.8f; //误差有效计算比例,越大容错性越强，精确度越低
uint8_t calc_ppt = 80;
/*
获取传感器数据保存到current_sonsor_data
data_:传感器读取到的数据数组
type:7:是否计算当前测量偏差（0:不计算，1:计算）
6:偏差计算方式（0:当前减去基准，1:基准减去当前）
5-1:保留
0:获取类型（0:正常采集数据，1:采集基准(地面)数据）
*/
void get_Sonsor_data(uint16_t * data_,uint8_t type)
{
uint8_t value;
Line_Scan();
for(value=0;value<sensor_quantity;value++)
{
switch(type & 0x01)
{
case 0: //采集正常数据，做线分析使用
current_sonsor_data[value] = *(data_+value);
break;
case 1: //采集地面数据，做对比基准使用
ground_data[value] = *(data_+value);
break;
default:
break;
}
if(type & 0x80)
{
if(type & 0x40)
{
line_D_value[value] = ground_data[value] - current_sonsor_data[value];
}
else
{
line_D_value[value] = current_sonsor_data[value] - ground_data[value];
}
}
}
}
/*
获取传感器检测状态
sensor_data:当前传感器数据
default_sensor_data:基准数据
type:硬件分压方式 0：光照-AD正比例 1：光照-AD反比例
*/
uint32_t get_Sensor_state(uint16_t * sensor_data,uint16_t * default_sensor_data,uint8_t type)
{
uint8_t value = 0;
uint8_t temp = 0;
uint32_t res = 0;
switch(sensor_type)
{
case Photosensitive_resistance: //光敏电阻类型的传感器
if(0 == Ground_Type) //绿地白线情况下计算线位置
{
if(0 == type)
{
for(temp = 0;temp < sensor_quantity;temp++)
{
if(current_sonsor_data[temp] - ground_data[temp] >= (int)((float)(line_D_value[temp] * calc_Proportion))) //当前结果比地面大于误差的60%比例
{
res |= 1 << temp; //对应位设为1，表示白线
}
//否则不设1为0，表示黑线
}
}
else
{
for(temp = 0;temp < sensor_quantity;temp++)
{
if(ground_data[temp] - current_sonsor_data[temp] >= (int)((float)(line_D_value[temp] * calc_Proportion))) //当前结果比地面大于误差的60%比例
{
res |= 1 << temp; //对应位设为1，表示白线
}
//否则不设1为0，表示黑线
}
}
}
else //黑地白线
{
if(0 == type)
{
for(temp = 0;temp < sensor_quantity;temp++)
{
if(current_sonsor_data[temp] - ground_data[temp] >= (int)((float)(line_D_value[temp] * calc_Proportion))) //当前结果比地面大于误差的60%比例
{
res |= 1 << temp; //对应位设为1，表示白线
}
//否则不设1为0，表示黑线
}
}
else
{
for(temp = 0;temp < sensor_quantity;temp++)
{
if(ground_data[temp] - current_sonsor_data[temp] >= (int)((float)(line_D_value[temp] * calc_Proportion))) //当前结果比地面大于误差的60%比例
{
res |= 1 << temp; //对应位设为1，表示白线
}
//否则不设1为0，表示黑线
}
}
}
break;
case Infra_red: //红外传感器
break;
case Linear_CCD:
break;
case Camera:
break;
case Electromagnetism:
break;
default:
break;
}
return res;
}
/*
采集地面数据
times：采集次数
delay：采集时间间隔
*/
void get_Ground_data(uint8_t times,uint8_t delay)
{
uint8_t val;
for(val = 0;val < times;val ++) //计算10次背景平均值
{
uint8_t temp = 0;
get_Sonsor_data(ADC_Value,0x01);
ground_data[temp] += ADC_Value[temp];
ground_data[temp] /= 2;
delay_ms(delay);
}
#if 1
printf("Ground:");
for(val = 0;val < sensor_quantity;val ++)
{
printf("%4d,",ground_data[val]);
}
printf(" ");
#endif
}
/*
获取最大误差
times:测试的次数
delay:大概测量周期
*/
void get_D_error_value(uint8_t times,uint8_t delay)
{
uint8_t val;
uint16_t max_val[sensor_quantity],min_val[sensor_quantity];
// get_Ground_data(100,5); //采集地面平均值作为比较基准
// printf("获取地面数据完成 ");
for(val = 0;val < sensor_quantity;val ++) //当作基准数据
{
max_val[val] = min_val[val] = ground_data[val];
}
printf("Ground:");
for(val = 0;val < sensor_quantity;val ++)
{
printf("%4d,",ground_data[val]);
}
printf(" ");
while(times--)
{
Line_Scan(); //获取传感器数据
for(val = 0;val < sensor_quantity;val++)
{
if(ADC_Value[val] > max_val[val])
{
max_val[val] = ADC_Value[val];
}
else if((ADC_Value[val]) <= min_val[val])
{
min_val[val] = ADC_Value[val];
}
}
delay_ms(delay);
}
for(val = 0;val < sensor_quantity;val++)
{
if((max_val[val] != 0)&&(min_val[val] != 0))
line_D_value[val] = max_val[val] - min_val[val];
}
#if 0
printf("max:");
for(val = 0;val < sensor_quantity;val++)
{
printf("%6d",max_val[val]);
}
printf(" ");
printf("min:");
for(val = 0;val < sensor_quantity;val++)
{
printf("%6d",min_val[val]);
}
printf(" ");
#endif
#if 1
printf("D_val:");
for(val = 0;val < sensor_quantity;val++)
{
printf("%6d,",line_D_value[val]);
}
printf(" ");
#endif
printf("获取极限值数据完成 ");
//还有个小BUG，如果第一次测试结果非常特殊，可能会导致最大值或最小值的某些元素为0，因为初始的数据无法判断那个是最小那个是最大，而且也不确定测试的数据一定会比这个大或者小
//后期可以通过大数据计算的平均值作为比较基准
//2018.06.22新增10次平均值作为极限值基准比较，防止了元素为0的出现
}
//地面数据: 2702,1566,1530,1933,2346,1550,1675,1287,1076,1881,1677
//最大值:2837 1740 1677 2129 2571 1748 1868 1425 1202 2138 2080
//最小值:1421 690 701 1041 1298 794 887 687 541 1129 985
//差值:1416 1050 976 1088 1273 954 981 738 661 1009 1095
uint8_t line[sensor_quantity] = {0,};
uint8_t calc_line(void)
{
uint8_t val = 0;
uint8_t times = 0;
uint8_t white = 0,black = 0;
for(val=0;val<sensor_quantity;val++)
{
get_Sonsor_data(ADC_Value,0x00);
// if(current_sonsor_data[val] - ground_data[val] >= (int)((float)(line_D_value[val] * calc_Proportion)))
// {
// line[val] = 0;
// black ++;
// }
// else if(current_sonsor_data[val] - ground_data[val] < -(int)((float)(line_D_value[val] * calc_Proportion)))
// {
// line[val] = 1;
// white ++;
// }
if(my_abs(current_sonsor_data[val] - ground_data[val]) <= (int)(line_D_value[val] * calc_Proportion))
{
line[val] = 0;
black ++;
}
else
{
line[val] = 1;
white ++;
}
}
if(white > 7)
{
printf("白 {MOD}横线 ");
return 0xEE;
}
else if(white == 0)
{
printf("跑出线 ");
return 0xCC;
}
else if((white<=3)&&(white>0))
{
printf("正常循迹 ");
return 0x77;
}
else
{
float f_val = 0.99;
printf("循迹拼命计算中 ");
times = 50;
while(times-- >0)
{
black = white = 0;
for(val=0;val<sensor_quantity;val++)
{
get_Sonsor_data(ADC_Value,0x00);
if(my_abs(current_sonsor_data[val] - ground_data[val]) <= (int)(line_D_value[val] * f_val))
{
line[val] = 0;
black ++;
}
else
{
line[val] = 1;
white ++;
}
}
if((white <= 3)&&(white > 0))
{
break;
}
else
f_val -= 0.01;
}
return 0x77;
}
#if 0
int err[sensor_quantity];
printf("差值");
for(val = 0;val < sensor_quantity;val ++)
{
err[val] = my_abs(current_sonsor_data[val] - ground_data[val]);
printf("%5d,",err[val]);
}
printf(" ");
printf("百分比");
for(val = 0;val < sensor_quantity;val ++)
{
printf("%5d,",(err[val]*1000/(int)line_D_value[val])/10);
}
printf(" ");
printf("line:%2d%2d%2d%2d%2d%2d%2d%2d%2d%2d%2d black:%d,white:%d ",line[0],line[1],line[2],line[3],line[4],line[5],line[6],line[7],line[8],line[9],line[10],black,white);
#endif
return white;
}
float getLineData(void)
{
printf("line:%2d%2d%2d%2d%2d%2d%2d%2d%2d%2d%2d ",line[0],line[1],line[2],line[3],line[4],line[5],line[6],line[7],line[8],line[9],line[10]);
if(calc_line() == 0xEE)
{
//遇到白线
Direction(0);
}
else if(calc_line() == 0xCC)
{
//跑出线
Direction(0);
}
else
{
float start = 0;
float lenth = 0;
float end = 0;
uint8_t val = 0;
for(val=0;val<sensor_quantity;val++)
{
if(line[val] == 1)
{
break;
}
else if(line[val] == 0)
{
start += 1;
}
}
for(val = 0;val < sensor_quantity;val ++)
{
lenth += line[val] == 1 ? 1 : 0;
}
end = (float)(sensor_quantity-lenth-start);
//printf("%.2f/%.2f/%.2f比例%.2f ",start,lenth,end,start/end);
Direction((end-start)*5);
}
}
float get_line_data(void)
{
uint8_t value = 0,res = 0;
uint8_t l[3] = {0,};
for(;value<sensor_quantity;value++)
{
res += line[value];
}
if(res == 0)
{
return 107;
}
else if(res == sensor_quantity)
{
return 117;
}
else
{
for(value = 0;value < sensor_quantity-1;value++)
{
if(line[value] == 0)
{
l[0] ++;
}
else
{
break;
}
}
for(value = l[0]-2;value < sensor_quantity-1;value++)
{
if(line[value] == 1)
{
l[1] ++;
}
else
{
break;
}
}
for(value = l[1]-2;value < sensor_quantity;value++)
{
if(line[value] == 0)
{
l[2] ++;
}
else
{
break;
}
}
printf("Test:%5d,%5d,%5d ",l[0],l[1],l[2]);
if((l[0] > l[2])&&(l[0] != 0))
{
return (float)l[2]/l[0];
}
else if((l[2] > l[0])&&(l[2] != 0))
{
return (float)(-(l[0]/l[2]));
}
else
return 1;
}
}
一周热门 更多>