void Display_integer3 (uint8 x,uint8 y,int16 i)
{
int8 a[3]={0};
a[0] = i/100;
a[1] = i%100/10;
a[2] = i%10;
for(uint8 j=0;j<3;j++)
{
switch(a[j])
{
case 0:{OLED_P6x8Str(x,y,"0");}break;
case 1:{OLED_P6x8Str(x,y,"1");}break;
case 2:{OLED_P6x8Str(x,y,"2");}break;
case 3:{OLED_P6x8Str(x,y,"3");}break;
case 4:{OLED_P6x8Str(x,y,"4");}break;
case 5:{OLED_P6x8Str(x,y,"5");}break;
case 6:{OLED_P6x8Str(x,y,"6");}break;
case 7:{OLED_P6x8Str(x,y,"7");}break;
case 8:{OLED_P6x8Str(x,y,"8");}break;
case 9:{OLED_P6x8Str(x,y,"9");}break;
default:break;
}
x+=6;
// if(x==80)
// x=20;
}
}
/*************************************************************************
* 野火嵌入式开发工作室
*
* 函数名称：Task_Start
* 功能说明：启动任务,初始化滴答时钟
* 参数说明：p_arg 没用
* 函数返回：无
* 修改时间：2012-2-10
* 备 注：
*************************************************************************/
void Task_Start(void *p_arg)
{
/**************** 初始化 系统滴答时钟 ：用于定时任务调度****************/
u32 cnts;
cnts = core_clk_mhz * ( (u32)1000000u / (u32)OS_TICKS_PER_SEC ); /* Determine nbr SysTick increments. */
OS_CPU_SysTickInit(cnts); /* Init uC/OS periodic time src (SysTick). */
#if (OS_TASK_STAT_EN > 0)
/* 开启统计任务 */
OSStatInit();
#endif
(void)p_arg; // 'p_arg' 并没有用到，防止编译器提示警告
/******************* 用户任务区 ************************/
u32 i = 0;
OLED_Init();
OLED_P6x8Str(0,0,"This is a test",0);
while(1)
{
sprintf(mystr,"count:%d",i++);
OLED_P6x8Str(0,3,mystr,0);
OSTimeDlyHMSM(0, 0, 0, 10);
}
}
void DisPlay_Layer6(void)
{
if( Plane_Mode.Rec_Img_ok==true)
{
cam_pos_ctr.binary_rec_temp=camera_image_buf[0];
Draw_Pic(camera_image_buf+1);
Plane_Mode.Rec_Img_ok=false;
start_receive_img=false;
rx_start=0;
pix_count=0;
}
OLED_P6x8Str(86,0,"binary");
Dis_Float(1,86,cam_pos_ctr.binaryzation_threshold,1);
Dis_Float(2,86,cam_pos_ctr.binary_rec_temp ,1);
OLED_P6x8Str(0,7,"RecImg:");
if(Plane_Mode.Receive_Img==true)
{
OLED_P6x8Str(64,7,"YES");
}
else
{
OLED_P6x8Str(64,7,"NO ");
}
}
void DisPlay_Layer1(void)
{
/*************状态显示**********************/
//定高状态
if(Ultra.SetHigh.Mode==SetHigh_ing)
OLED_P6x8Str(0,2,"Althold");
else if(Ultra.SetHigh.Mode==0)
OLED_P6x8Str(0,2,"AltStop");
//循迹状态
if(tracking_offset.track_mode==END_TRACK)
OLED_P6x8Str(0,3,"endtra ");
else if(tracking_offset.track_mode==START_TRACK)
OLED_P6x8Str(0,3,"starttra ");
else if(tracking_offset.track_mode==TRACK_ING)
OLED_P6x8Str(0,3,"traing ");
//定点状态
if(cam_pos_ctr.FixedPos_Mode==FIX_ING)
OLED_P6x8Str(0,4,"Pos: ON");
else if(cam_pos_ctr.FixedPos_Mode==CLOSED)
OLED_P6x8Str(0,4,"Pos:OFF");
//降落状态
if(Plane_Mode.land==true)
OLED_P6x8Str(0,5,"LAND: ON");
else if(Plane_Mode.land==false)
OLED_P6x8Str(0,5,"LAND:OFF");
OLED_4num(80,2,tracking_offset.Check.circle_find);
Dis_Float(4,80,tracking_offset.goal_angle,2);
}
//输出汉字和字符混合字符串
void OLED_Print(unsigned char x, unsigned char y, unsigned char ch[])
{
unsigned char ch2[3];
unsigned char ii=0;
while(ch[ii] != '\0')
{
if(ch[ii] > 127)
{
ch2[0] = ch[ii];
ch2[1] = ch[ii + 1];
ch2[2] = '\0'; //汉字为两个字节
OLED_P14x16Str(x , y, ch2); //显示汉字
x += 14;
ii += 2;
}
else
{
ch2[0] = ch[ii];
ch2[1] = '\0'; //字母占一个字节
OLED_P6x8Str(x , y , ch2 , 0); //显示字母
x += 8;
ii+= 1;
}
}
}
void DisPlay_Layer5(void)
{
OLED_P6x8Str(16,0,"AUTTITUDE.PID");
Dis_Float(2,6,ctrl.roll.shell.kp ,3);
Dis_Float(2,42,ctrl.roll.shell.ki,4);
Dis_Float(2,84,ctrl.roll.shell.kd,2);
Dis_Float(3,6, ctrl.roll.core.kp,2);
Dis_Float(3,42, ctrl.roll.core.ki,4);
Dis_Float(3,84, ctrl.roll.core.kd,3);
OLED_P6x8Str(16,5,"YAW.PID");
Dis_Float(6,6,ctrl.yaw.shell.kp ,3);
Dis_Float(6,42,ctrl.yaw.shell.ki,4);
Dis_Float(6,84,ctrl.yaw.shell.kd,2);
Dis_Float(7,6, ctrl.yaw.core.kp,2);
Dis_Float(7,42, ctrl.yaw.core.ki,4);
Dis_Float(7,84, ctrl.yaw.core.kd,3);
}
void OLED_clear_right(void)
{
unsigned char y,x;
for(y=0;y<8;y++)
{
for(x=64;x<121;x+=6)
{
OLED_P6x8Str(x,y," ",0);
}
}
}
void OLED_clear_left(void)
{
unsigned char y,x;
for(y=0;y<8;y++)
{
for(x=0;x<55;x+=6)
{
OLED_P6x8Str(x,y," ",0);
}
}
}
void Layer2_Reduce_Par(const u16 *next)
{
u16 line=*next-1;
switch(line)
{
case 0://第一行改变比赛模式
{
Set_Layer.line0--;
if( Set_Layer.line0<=(mode_min-1) )
{
Set_Layer.line0=mode_max;
}
fly_mode=(_FLY_MODE) Set_Layer.line0;
OLED_P6x8Str(30,0," ");
break;
}
case 1://
{
break;
}
case 2://第二行改变超声波定高高度
{
Ultra.FINAL_HIGH--;
break;
}
case 3://第三行改变循迹前进的角度
{
tracking_offset.forward_angle-=0.1;
break;
}
case 4://
{
break;
}
case 5://
{
Ultra.Add_Thro--;
break;
}
case 6://
{
break;
}
case 7://
{
break;
}
default:break;
}
}
void OLED_Update(void *p_arg)
{
(void)p_arg; // 'p_arg' 并没有用到，防止编译器提示警告
u32 i = 0;
while(1)
{
sprintf(mystr,"count:%d",i++);
OLED_P6x8Str(0,2,mystr,0);
OSTimeDlyHMSM(0, 0, 0, 100);
}
}
void DisPlay_Layer0(void)
{
OLED_P6x8Str(0,0,"ROLL");
OLED_P6x8Str(0,1,"PITCH");
OLED_P6x8Str(0,2,"YAW");
OLED_P6x8Str(0,3,"HIGH");
OLED_P6x8Str(0,4,"POS.X");
OLED_P6x8Str(0,5,"POS.Y");
OLED_P6x8Str(0,6,"X.V");
OLED_P6x8Str(0,7,"Y.V");
OLED_P6x8Str(80,0,"y:");
OLED_P6x8Str(80,1,"yv");
Dis_Float(0,40,angle.roll,1);
Dis_Float(1,40,angle.pitch,1);
Dis_Float(2,40,angle.yaw,1); OLED_4num(80,2,tracking_offset.Check.circle_find);
Dis_Float(3,40,Ultra.z,2); OLED_4num(80,3,tracking_offset.circle_count);
Dis_Float(4,40, cam_pos_ctr.pos_ctr_x.valid_distance,1); Dis_Float(4,80,tracking_offset.goal_angle,2);
Dis_Float(5,40, cam_pos_ctr.pos_ctr_y.valid_distance,1);
Dis_Float(6,40, cam_pos_ctr.pos_ctr_x.v ,1); OLED_4num(80,6, tracking_offset.line_find);
Dis_Float(7,40, cam_pos_ctr.pos_ctr_y.v ,1);
Dis_Float(0,96, tracking_offset.valid_offset ,1);
Dis_Float(1,96, tracking_offset.off_v ,1);
}
/********************************oled显示函数**********************************/
void oledplay()
{
OLED_P6x8Str(0,0,"angle_fusex:");
OLED_P6x8Str(0,1,"angle_fusey:");
// OLED_P6x8Str(0,2,"gyro_speedy:");
// OLED_P6x8Str(0,3,"angle_fusey:");
if(flag==2)
{
OLED_P6x8Str(0,4,"distance:");
OLED_P6x8Str(0,5,"angle_set:");
Display_number7(74,4,(int16)distance);
DisplayFloat3(74,5,(int16)(angle_set*1000));
}
Display_number7(74,0,(int16)angle_fuse);
Display_number7(74,1,(int16)angle_fuse1);
// Display_number7(74,2,(int16)gyro_speed1);
// Display_number7(74,3,(int16)angle_fuse1);
OLED_P6x8Str(0,7,"mode");
if(flag==1)
OLED_P6x8Str(24,7,"1");
else if (flag==2)
OLED_P6x8Str(24,7,"2");
else if (flag==3)
OLED_P6x8Str(24,7,"3");
else if (flag==4)
OLED_P6x8Str(24,7,"4");
}
//显示PID参数及设定速度
void showPID(void)
{
OLED_P6x8Str(0,0,"DirP");
OLED_PrintValueF(50,0,PIDVaule.DirectionControlP,2);
OLED_P6x8Str(0,1,"DirD");
OLED_PrintValueF(50,1,PIDVaule.DirectionControlD,2);
OLED_P6x8Str(0,2,"SpeedP");
OLED_PrintValueF(50,2,PIDVaule.SpeedControlP,2);
OLED_P6x8Str(0,3,"SpeedD");
OLED_PrintValueF(50,3,PIDVaule.SpeedControlD,2);
OLED_P6x8Str(0,4,"TSpeed");
OLED_PrintValueF(50,4,setTargetSpeed,2);
OLED_P6x8Str(0,5,"NSpeed");
OLED_PrintValueF(50,5,MotorParameter.Speed,2);
}
void DisPlay_Layer4(void)
{
OLED_P6x8Str(30,0,"POSHOLD.PID");
OLED_P6x8Str(0,2,"S.P");
OLED_P6x8Str(0,3,"S.I");
OLED_P6x8Str(0,4,"S.D");
OLED_P6x8Str(0,5,"C.P");
OLED_P6x8Str(0,6,"C.I");
OLED_P6x8Str(0,7,"C.D");
Dis_Float(2,24, pos_pid_ctr.PosPid_Y.Shell.P ,3);
Dis_Float(3,24,pos_pid_ctr.PosPid_Y.Shell.I ,3);
Dis_Float(4,24,pos_pid_ctr.PosPid_Y.Shell.D ,2);
Dis_Float(5,24, pos_pid_ctr.PosPid_Y.Core.P ,2);
Dis_Float(6,24,pos_pid_ctr.PosPid_Y.Core.I ,3);
Dis_Float(7,24,pos_pid_ctr.PosPid_Y.Core.D ,3);
}
void DisPlay_Layer3(void)
{
OLED_P6x8Str(30,0,"ALTHOLD.PID");
OLED_P6x8Str(0,2,"S.P");
OLED_P6x8Str(0,3,"S.I");
OLED_P6x8Str(0,4,"S.D");
OLED_P6x8Str(0,5,"C.P");
OLED_P6x8Str(0,6,"C.I");
OLED_P6x8Str(0,7,"C.D");
Dis_Float(2,24,sonar_shell_pid.P/100.00,2);
Dis_Float(3,24,sonar_shell_pid.I/10000.0,4);
Dis_Float(4,24,sonar_shell_pid.D/100.00,2);
Dis_Float(5,24,sonar_core_pid.P/100.00,2);
Dis_Float(6,24,sonar_core_pid.I/10000.00,4);
Dis_Float(7,24,sonar_core_pid.D/100.00,2);
}
void DisPlay_Layer2(void)
{
//可设置的比赛模式
OLED_P6x8Str(0,0,"Mode:");
switch(fly_mode)
{
case 1:
{
OLED_P6x8Str(30,0,"ALTHOLD");
break;
}
case 2:
{
OLED_P6x8Str(30,0,"FIXED.POINT");
break;
}
case 3:
{
OLED_P6x8Str(30,0,"TRACK.LINE");
break;
}
case 4:
{
OLED_P6x8Str(30,0,"LINE.ABC");
break;
}
case 5:
{
OLED_P6x8Str(30,0,"TRACK.ABA");
break;
}
default:break;
}
OLED_P6x8Str(0,2,"goalhigh");
Dis_Float(2,54,Ultra.FINAL_HIGH,1);
OLED_P6x8Str(0,3,"forward.angle");
Dis_Float(3,84,tracking_offset.forward_angle ,1);
OLED_P6x8Str(0,4,"RecImg");
if(Plane_Mode.Receive_Img==true)
{
OLED_P6x8Str(64,4,"YES");
}
else
{
OLED_P6x8Str(64,4,"NO ");
}
OLED_P6x8Str(0,5,"Add_Thro");
Dis_Float(5,54,Ultra.Add_Thro,1);
OLED_P6x8Str(16,6,"sure to start?");
OLED_P6x8Str(0,7,"Start:");
if(Plane_Mode.fly_start==true)
{
OLED_P6x8Str(56,7,"YES");
}
else
{
OLED_P6x8Str(56,7,"NO ");
}
}
void Display_number7 (uint8 x,uint8 y,int16 i)
{
int8 a[8]={0};
if(i<0)
{
a[0]='-';
i=-i;
}
else
a[0]='+';
a[1] = i/1000000;
a[2] = i/100000%10;
a[3] = i/10000%10;
a[4] = i/1000%10;
a[5] = i/100%10;
a[6] = i/10%10;
a[7] = i%10;
for(uint8 j=0;j<8;j++)
{
switch(a[j])
{
case 0:{OLED_P6x8Str(x,y,"0");}break;
case 1:{OLED_P6x8Str(x,y,"1");}break;
case 2:{OLED_P6x8Str(x,y,"2");}break;
case 3:{OLED_P6x8Str(x,y,"3");}break;
case 4:{OLED_P6x8Str(x,y,"4");}break;
case 5:{OLED_P6x8Str(x,y,"5");}break;
case 6:{OLED_P6x8Str(x,y,"6");}break;
case 7:{OLED_P6x8Str(x,y,"7");}break;
case 8:{OLED_P6x8Str(x,y,"8");}break;
case 9:{OLED_P6x8Str(x,y,"9");}break;
case '-':{OLED_P6x8Str(x,y,"-");}break;
case '+':{OLED_P6x8Str(x,y,"+");}break;
default:break;
}
x+=6;
// if(x==80)
// x=20;
}
}
//==============================================================
//函数名：void DisplayFloat(int16 i,uint8 x,uint8 y)
//功能描述：显示动态浮点数字
//参数：起始点坐标(x,y),x的范围0～127，y为页的范围0～7
//返回：无
//==============================================================
void DisplayFloat3(uint8 x,uint8 y,int32 i)
{
int8 a[6]={0};
a[0]=i/10000;
a[1]=i%10000/1000;
a[2]='.';
a[3]=i%1000/100;
a[4]=i%100/10;
a[5]=i%10;
for(uint8 j=0;j<6;j++)
{
switch(a[j])
{
case 0:{OLED_P6x8Str(x,y,"0");}break;
case 1:{OLED_P6x8Str(x,y,"1");}break;
case 2:{OLED_P6x8Str(x,y,"2");}break;
case 3:{OLED_P6x8Str(x,y,"3");}break;
case 4:{OLED_P6x8Str(x,y,"4");}break;
case 5:{OLED_P6x8Str(x,y,"5");}break;
case 6:{OLED_P6x8Str(x,y,"6");}break;
case 7:{OLED_P6x8Str(x,y,"7");}break;
case 8:{OLED_P6x8Str(x,y,"8");}break;
case 9:{OLED_P6x8Str(x,y,"9");}break;
case '.':{OLED_P6x8Str(x,y,".");}break;
default:break;
}
x+=6;
// if(x==90)
// x=20;
}
}
void Layer2_Add_Par(const u16 *next)
{
u16 line=*next-1;
switch(line)
{
case 0://第一行改变比赛模式
{
Set_Layer.line0++;
if( Set_Layer.line0>=(mode_max+1) )
{
Set_Layer.line0=mode_min;
}
fly_mode=(_FLY_MODE) Set_Layer.line0;
OLED_P6x8Str(30,0," ");
break;
}
case 1://
{
break;
}
case 2://第二行改变超声波定高高度
{
Ultra.FINAL_HIGH++;
break;
}
case 3://第三行改变循迹前进的角度
{
tracking_offset.forward_angle+=0.1;
break;
}
case 4://
{
Set_Layer.line4++;
if(Set_Layer.line4==1)
{
Plane_Mode.Receive_Img=true;
Data_Send_Image_Par(Image_Par_buf,ORDER_FRAME, 0xA4,0);//开
usart1_Send_Data(Image_Par_buf,9);
}
if(Set_Layer.line4>=2)
{
Set_Layer.line4=0;
Plane_Mode.Receive_Img=false;
Data_Send_Image_Par(Image_Par_buf,ORDER_FRAME, 0x0f,0);//关
usart1_Send_Data(Image_Par_buf,9);
}
break;
}
case 5://
{
Ultra.Add_Thro++;
break;
}
case 6://
{
break;
}
case 7://
{
Set_Layer.line7++;
if(Set_Layer.line7==1)
{
Plane_Mode.fly_start=true;
}
if(Set_Layer.line7>=2)
{
Set_Layer.line7=0;
Plane_Mode.fly_start=false;
}
break;
}
default:break;
}
}
/***********************************主函数**************************************/
void main(void)
{
for(i = 0; i < 110; i++) //装载摄像头边线初值
{
slope.left_initial_value[i] = left_initial[i];
slope.right_initial_value[i] = right_initial[i];
}
camera_init(imgbuff); //初始化摄像头
//配置中断服务函数
set_vector_handler(PORTA_VECTORn , PORTA_IRQHandler); //设置LPTMR的中断服务函数为 PORTA_IRQHandler
set_vector_handler(DMA0_VECTORn , DMA0_IRQHandler); //设置LPTMR的中断服务函数为 PORTA_IRQHandler
set_vector_handler(PORTE_VECTORn ,PORTE_IRQHandler); //设置PORTE的中断服务函数为 PORTE_IRQHandler
enable_irq (PORTE_IRQn); //使能PORTE中断
port_init(PTE2, ALT1 | PULLUP ); //5 蓝牙同时发车
if(!gpio_get (PTE2)) //蓝牙通讯同时发车
{
key_init(KEY_A);
OLED_Init();
while(1)
{
#if ( CAR_NUMBER == 2 )
OLED_P6x8Str(0, 6, "Press K6 to start!");
if(key_check(KEY_A) == KEY_DOWN) //按K3发车
{
uart_putchar(VCAN_PORT, ch);
OLED_P6x8Str(0, 6, " ");
bluetooth = 1;
break;
}
#endif
#if ( CAR_NUMBER == 1 )
OLED_P6x8Str(0, 6, "Start 2B to start!");
while(uart_query (VCAN_PORT) == 0);
OLED_P6x8Str(0, 6, " ");
bluetooth = 1;
break;
#endif
}
}
else
{
bluetooth = 2;
}
mk60int();
init_PID();
while(1)
{
oled_display();
camera_get_img(); //摄像头获取图像
img_extract(img, imgbuff, CAMERA_SIZE); //解压图像
get_slope(img, &slope); //获取斜率
boundary_detection();//扫描图像获取边界
picture_analysis();//获取中线
//SetMotorVoltage(0.2,0.2);
if(!gpio_get (PTE1))
{
vcan_sendimg(imgbuff,CAMERA_SIZE);//摄像头看图像
}
if(!gpio_get (PTE2)) //蓝牙同时发车
{
#if ( CAR_NUMBER == 1 )
if(uart_querychar (VCAN_PORT, &ch) != 0)
{
if(ch == 's')
{
stop_done = 1;
}
}
#endif
#if ( CAR_NUMBER == 2 )
if(stop_done)
{
lptmr_delay_ms(500); //延时确保后车过线
while(1)
{
uart_putchar(VCAN_PORT, ch_stop);
}
}
#endif
}
if(!gpio_get (PTE3)) //速度40
{
vPID.Setpoint = 40;
}
if(!gpio_get (PTE4)) //速度45
{
vPID.Setpoint = 45;
}
if(!gpio_get (PTE5)) //速度50
{
vPID.Setpoint = 50;
}
if(!gpio_get (PTE6)) //速度47
{
vPID.Setpoint = 46;
}
if(!gpio_get (PTE7)) //速度48
{
vPID.Setpoint = 47;
}
if(!gpio_get (PTE8)) //速度49
{
vPID.Setpoint = 48;
}
//ware1[0]=xielv;
//vcan_sendware(ware1, sizeof(xielv));
/*
Display_number(36, 6, ttt);
Display_number(0, 6, sss);
printf("%d,", left_diuxian1);
printf("%d\n",tt-t);
*/
//printf("%d\n",speed1);
//printf("%d,",left_diuxian1);
//printf("%d\n",slope3);
//printf("%d,",left_chazi);
//printf("%d\n",right_chazi);
//printf("%d,",m);
//printf("%d\n",a);
// if(!gpio_get (PTE8))
// {
// for(l=0;l<120;l++)
// {
// printf("%d,",left_bianjie[l]);
// }
// printf("\n");
// }
}
}
void My_Key()
{
switch(key_val)
{
case 0:
str[key_n]='0';
str[key_n+1]='\0';
key_n++;
OLED_P6x8Str(55,4,str);
if(pot_flag==0)
{
input_val=input_val*10;
}
else
{
pot_flag++;
}
key_val=0;
break;
case 1:
case 2:
case 3:
case 4:
case 5:
case 6:
case 7:
case 8:
case 9:
str[key_n]=key_val%10+'0';
str[key_n+1]='\0';
key_n++;
OLED_P6x8Str(55,4,str);
if(pot_flag==0)
{
input_val=input_val*10+key_val;
}
else
{
input_val=input_val+key_val/(pow(10,pot_flag));
pot_flag++;
}
key_val=0;
break;
case 10:
key_n=0;
str[0]='0';
str[1]='\0';
OLED_Fill_page(0x00);//黑屏
OLED_P6x8Str(55,4,str);
input_val=0;
pot_flag=0;
key_val=0;
break;
case 14:
str[key_n]='.';
str[key_n+1]='\0';
key_n++;
OLED_P6x8Str(55,4,str);
pot_flag=1;
key_val=0;
break;
}
}
void OledDispaly(void)
{
OLED_4num(4,5,tracking_offset.goal_angle);
OLED_4num(4,6,tracking_offset.Check.circle_find);
Dis_Float(5,93,key,1);
Dis_Float(6,93,cam_pos_ctr.PITCH,1);
Dis_Float(7,6,angle.roll,1);
Dis_Float(7,48,angle.pitch,1);
Dis_Float(7,90,angle.yaw,1);
#ifdef DOUBLE_PIDHOLD_DEBUG
if(Ultra.SetHigh.Mode==SetHigh_ing)
OLED_P6x8Str(0,2,"Althold");
else if(Ultra.SetHigh.Mode==0)
OLED_P6x8Str(0,2,"AltStop");
#endif
#ifdef PID_POS_DEBUG
if(tracking_offset.track_mode==END_TRACK)
OLED_P6x8Str(0,2,"endtra");
else if(tracking_offset.track_mode==START_TRACK)
OLED_P6x8Str(0,2,"starttra");
else if(tracking_offset.track_mode==TRACK_ING)
OLED_P6x8Str(0,2,"traing");
#endif
if(cam_pos_ctr.FixedPos_Mode==FIX_ING)
OLED_P6x8Str(0,3,"Pos: ON");
else if(cam_pos_ctr.FixedPos_Mode==CLOSED)
OLED_P6x8Str(0,3,"Pos:OFF");
if(Plane_Mode.land==true)
OLED_P6x8Str(0,4,"LAND: ON");
else if(Plane_Mode.land==false)
OLED_P6x8Str(0,4,"LAND:OFF");
#ifdef DOUBLE_PIDHOLD_DEBUG //
Dis_Float(2,70,sonar_shell_pid.setgoal,2);//
Dis_Float(3,70,Ultra.z,2);// Ultra.z cm 高度 //超声波高度
Dis_Float(4,70,Ultra.Add_Thro,1);
Dis_Float(0,6,sonar_shell_pid.P/100.00,2);
Dis_Float(0,42,sonar_shell_pid.I/10000.0,4);
Dis_Float(0,84,sonar_shell_pid.D/100.00,2);
Dis_Float(1,6,sonar_core_pid.P/100.00,2);
Dis_Float(1,42,sonar_core_pid.I/10000.00,4);
Dis_Float(1,84,sonar_core_pid.D/100.00,2);
#endif
#ifdef PID_POS_DEBUG
Dis_Float(0,6, pos_pid_ctr.PosPid_Y.Shell.P ,3);
Dis_Float(0,42,pos_pid_ctr.PosPid_Y.Shell.I ,3);
Dis_Float(0,84,pos_pid_ctr.PosPid_Y.Shell.D ,2);
Dis_Float(1,6, pos_pid_ctr.PosPid_Y.Core.P ,2);
Dis_Float(1,42,pos_pid_ctr.PosPid_Y.Core.I ,3);
Dis_Float(1,84,pos_pid_ctr.PosPid_Y.Core.D ,3);
Dis_Float(2,50,Ultra.z,2);// Ultra.z cm 高度 //超声波高度
Dis_Float(3,50, cam_pos_ctr.pos_ctr_x.valid_distance,1);
Dis_Float(4,50, cam_pos_ctr.pos_ctr_y.valid_distance,1);
Dis_Float(3,90, tracking_offset.Change_yaw.final_alladd ,1);
Dis_Float(4,90, cam_pos_ctr.pos_ctr_y.v ,1);
#endif
#ifdef ATTITUDE_PID_DEBUG
Dis_Float(0,6,ctrl.roll.shell.kp ,3);
Dis_Float(0,42,ctrl.roll.shell.ki,4);
Dis_Float(0,84,ctrl.roll.shell.kd,2);
Dis_Float(1,6, ctrl.roll.core.kp,2);
Dis_Float(1,42, ctrl.roll.core.ki,4);
Dis_Float(1,84, ctrl.roll.core.kd,3);
Dis_Float(3,100, pitch_offset,2);
Dis_Float(4,100, roll_offset,2);
#endif
}
/*!
* @brief OLED显示
* @since v1.0
* @note 没啥
* Sample usage: oled_display();
*/
void oled_display()
{
//第一行显示拨码开关状态
OLED_P6x8Str(0, 0, "12345678");
OLED_P6x8Str(50, 0, "speed:"); Display_number(86, 0, vPID.Setpoint);
OLED_P6x8Str(50, 1, "Error:"); Display_number(86, 1, Error);
OLED_P6x8Str(0, 2, "slope:"); DisplayFloat(36, 2, slope.slope);
OLED_P6x8Str(80, 2, "P:"); DisplayFloatpid(92, 2, actuator_P);
OLED_P6x8Str(0, 3, "Distance:"); Display_number(54, 3, distance);
OLED_P6x8Str(86, 3, "D:"); DisplayFloatpid(98, 3, actuator_D);
OLED_P6x8Str(0, 4, "Bluetooth:");
if(bluetooth == 1)
OLED_P6x8Str(60, 4, "connected ");
else if(bluetooth == 0)
OLED_P6x8Str(60, 4, "disconnect");
else
OLED_P6x8Str(60, 4, "off ");
OLED_P6x8Str(0, 5, "stop?");
if(stop_done)
OLED_P6x8Str(36, 5, "yes");
else
OLED_P6x8Str(36, 5, "no");
#if ( CAR_NUMBER == 1 )
OLED_P6x8Str(0, 7, "Interesting");
OLED_P6x8Str(91, 7, "Car:1A");
#endif
#if ( CAR_NUMBER == 2 )
OLED_P6x8Str(0, 7, "Interesting");
OLED_P6x8Str(91, 7, "Car:2B");
#endif
if(!gpio_get (PTE1))
OLED_P6x8Str(0, 1, "^");
else
OLED_P6x8Str(0, 1, " ");
if(!gpio_get (PTE2))
OLED_P6x8Str(6, 1, "^");
else
OLED_P6x8Str(6, 1, " ");
if(!gpio_get (PTE3))
OLED_P6x8Str(12, 1, "^");
else
OLED_P6x8Str(12, 1, " ");
if(!gpio_get (PTE4))
OLED_P6x8Str(18, 1, "^");
else
OLED_P6x8Str(18, 1, " ");
if(!gpio_get (PTE5))
OLED_P6x8Str(24, 1, "^");
else
OLED_P6x8Str(24, 1, " ");
if(!gpio_get (PTE6))
OLED_P6x8Str(30, 1, "^");
else
OLED_P6x8Str(30, 1, " ");
if(!gpio_get (PTE7))
OLED_P6x8Str(36, 1, "^");
else
OLED_P6x8Str(36, 1, " ");
if(!gpio_get (PTE8))
OLED_P6x8Str(42, 1, "^");
else
OLED_P6x8Str(42, 1, " ");
if(key_check(KEY_A) == KEY_DOWN)
{
OLED_Init(); //OLED初始化 防花屏
}
if(key_check(KEY_U) == KEY_DOWN) //按K2重新获取两边线数组，串口接收
{
get_initial_value(img, &slope);
printf("const int left_initial[110] ={");
for(i = 0; i < 110; i++)
{
printf("%d, ", slope.left_initial_value[i]);
//printf("[%d]=%d\n", i, slope.right_initial_value[i]);
}
printf("};\nconst int right_initial[110] ={");
for(i = 0; i < 110; i++)
{
printf("%d, ", slope.right_initial_value[i]);
//printf("[%d]=%d\n", i, slope.right_initial_value[i]);
}
printf("};//Pay attention to delete the last \",\"\n");
//initial_value_get=1;
}
}
