/*
* 函数名: LBTest
* 描 述: 灯板测试函数
* 输 入: 无
* 输 出: 无
* 调 用: 外部调用
*/
void LBTest(void)
{
rt_uint32_t key_value;
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("=========LB=========");
LCD_SetXY(0,1);
LCD_WriteString("LB1:");
LCD_WriteInt(LB_Count(1));
LCD_SetXY(10,1);
LCD_WriteString("LB2:");
LCD_WriteInt(LB_Count(2));
LCD_SetXY(0,2);
LCD_WriteString("LB1:");
LCD_Printf("%s",itob(LB_recv[0].v));
LCD_SetXY(0,4);
LCD_WriteString("LB2:");
LCD_Printf("%s",itob(LB_recv[1].v));
LCD_SetXY(0,6);
LCD_WriteString("1.LB_Init");
LCD_SetXY(10,6);
LCD_WriteString("2.LB_State");
LCD_SetXY(0,7);
LCD_WriteString("3.LB_Test");
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1:
LB_Init();
break;
case key2:
LB_State();
break;
case key3:
LB_Test();
break;
case keyback:
return;
}
}
Delay_ms(10);
}
}
/*
void LCD_ShowCursor(uchar x,uchar y)
{
return;
//LCD_CE = 0;
LCD_Comm(0x97); //光标开
LCD_Write2(x,y,0x21);
}
// 取消光标
void LCD_HideCursor(void)
{
return;
//LCD_CE = 0;
LCD_Comm(0x9c);
}
*/
void LCD_PutImg(uchar x,uchar y,uchar w,uchar h,uchar *img)
{
uint data addr;
uchar data i,j;
uchar data c;
addr = LCD_GRAPH_HOME_ADDR + LCD_WIDTH * y + (x / 8);
//LCD_CE = 0;
for(j=0;j<h;j++)
{
LCD_WriteInt(addr,0x24);
LCD_Comm(0xb0);
LCD_CheckBusy1();
for(i=0;i<w;i++)
{
c = img[j*w+i] ;
LCD_AutoWrite(c);
//LCD_WriteInt(addr +i,0x24); //+ LCD_WIDTH *j
//LCD_Write1(c,0xc0);
}
LCD_CheckBusy3();
LCD_Comm(0xb2);
addr += LCD_WIDTH;
}
}
void LCD_WriteNum(double data)//写double型数，小数点后保留6位，从高位向低位写
{
char str[5];
int flag=0,i;
if(data<0)
{
LCD_WriteChar('-');//写负号
data=-data;
}
LCD_WriteInt((int)data);//写整数部分
data=data-(int)data;
for(i=0;i<5;i++)//将小数部分保留在一个字符串内
{
data=data*10;
str[i]=(int)data+'0';
if(str[i]!='0')
{
flag=i+1;
}
data=data-(int)data;
}
if(flag!=0)
LCD_WriteChar('.');//写小数点
for(i=0;i<flag;i++)
{
LCD_WriteChar(str[i]);//写小数部分
}
for(i=flag;i<6;i++)
{
LCD_WriteChar(' ');//小数点后不够6位则补空格
}
}
void LCD_ClsBlock(uchar x1,uchar y1,uchar x2,uchar y2)
{
uint addr;
uchar h,w;
uchar i,j;
//x坐标要8字节对齐
x1 =(x1 / 8) * 8;
x2 =(x2 / 8) * 8;
h = y2 - y1;
w = (x2 - x1)/8;
addr = LCD_GRAPH_HOME_ADDR + LCD_WIDTH * y1 + x1/8;
for(j=0;j<h;j++)
{
LCD_WriteInt(addr,0x24);
LCD_Comm(0xb0);
LCD_CheckBusy1();
for(i=0;i<w;i++)
{
LCD_AutoWrite(0x00);
}
LCD_CheckBusy3();
LCD_Comm(0xb2);
addr += LCD_WIDTH;
}
}
void parameter_setting (void)
{
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("PARAMETER SETTING");
LCD_SetXY(0,1);
LCD_WriteString("1.SPEED:");
LCD_WriteDouble(AGV_speed,1);
LCD_SetXY(0,2);
LCD_WriteString("2.DELTA:");
LCD_WriteDouble(delta_x,0);
LCD_WriteString("/");
LCD_WriteDouble(delta_y,0);
LCD_SetXY(0,3);
LCD_WriteString("3.STOPTIME:");
LCD_WriteInt(Stop_time);
refresh();
if (KeyReady==1)
{
KeyReady=0;
switch (KeyValue)
{
case 1:
Input_FloatValue(&AGV_speed,"SPEED");
flash_save[0].u16_data[0]=AGV_speed;
break;
case 2:
Input_FloatValue(&delta_x,"DELTA_X");
flash_save[0].u16_data[1]=delta_x;
Input_FloatValue(&delta_y,"DELTA_Y");
flash_save[0].u16_data[2]=delta_y;
break;
case 3:
Input_IntValue(&Stop_time,"STOPTIME");
flash_save[0].u16_data[3]=Stop_time;
break;
case 5:
return;
}
Data_Save();
init_parameter();
}
delay(LCD_DELAY);
}
}
void LCD_Putpixel(uchar x,uchar y)
{
uchar b;
uint addr = LCD_GRAPH_HOME_ADDR + y * LCD_WIDTH + x / 8;
b = 7 - (x % 8);
LCD_WriteInt(addr,0x24);
LCD_Comm(0xf8|b);
}
/*
* 函数名: HallTest
* 描 述:
* 输 入: 无
* 输 出: 无
* 调 用: 外部调用
*/
void HallTest(void)
{
rt_uint32_t key_value;
extern u8 Hall_Count;
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("========Hall========");
LCD_SetXY(0,1);
LCD_WriteString("1.SS_init");
LCD_SetXY(0,2);
LCD_WriteString("2.open hand_SS");
LCD_SetXY(0,3);
LCD_WriteString("3.leg_on");
LCD_SetXY(0,4);
LCD_WriteString("4.leg_off");
LCD_SetXY(0,5);
LCD_WriteString("5.open hand_SW");
LCD_SetXY(0,6);
LCD_WriteString("6.PW_init");
LCD_SetXY(0,7);
LCD_WriteInt(Hall_Count-1);
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1:
Hall_Send(1);
break;
case key2:
Hall_Send(2);
break;
case key3:
Hall_Send(3);
break;
case key4:
Hall_Send(4);
break;
case key5:
Hall_Send(5);
break;
case key6:
Hall_Send(6);
break;
case keyback:
return;
}
}
Delay_ms(10);
}
}
void LCD_Cls(void)
{
uint i;
//LCD_CE = 0;
//LCD_Write2(0x00,0x00,LC_ADD_POS);// 置地址指针为从零开始
LCD_WriteInt(LCD_TEXT_HOME_ADDR,LC_ADD_POS);
LCD_Comm(LC_AUT_WR); // 自动写
for(i=0;i< (LCD_HEIGHT+8)*LCD_WIDTH;i++) // 清一屏
{
LCD_AutoWrite(0x00); // 写数据0x00
}
LCD_Comm(LC_AUT_OVR); // 自动写结束
LCD_Write2(0x00,0x00,LC_ADD_POS);// 重置地址指针
}
/*------------------初始化-----------------*/
void LCD_Init (void)
{
//LCD_RST=0;
//;
//LCD_RST=1;
//LCD_FS=0;
//LCD_CE=0;
LCD_CE = 1;
LCD_WR=1;
LCD_RD=1;
LCD_FS=0;
LCD_WriteInt(LCD_TEXT_HOME_ADDR,0x40); //文本显示区首地址
LCD_WriteInt(LCD_GRAPH_HOME_ADDR,0x42); //图形显示区首地址
LCD_Write2(LCD_WIDTH,0x00,0x41); //文本显示区宽度
LCD_Write2(LCD_WIDTH,0x00,0x43); //图形显示区宽度
LCD_Comm(0x81); //逻辑"异或"
LCD_Write2(0x02,0x00,0x22); //CGRAM偏置地址设置
LCD_Comm(0x9c); //启用文本显示,启用图形显示
//LCD_Comm(0x98); //启用文本显示,启用图形显示
LCD_Cls();
}
/*
* 函数名: DataBack
* 描 述: 数据回传函数
* 输 入: 无
* 输 出: 无
* 调 用: 外部调用
*/
void DataBackTest(void)
{
// rt_uint32_t key_value;
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("======DataBack======");
LCD_SetXY(0,1);
#if DataBackEnable
LCD_WriteString("DataNumber:");
LCD_WriteInt(DataBack_GetCount());
LCD_SetXY(0,2);
LCD_WriteString("Press OK to send");
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case keyOK:
LCD_Clear();
LCD_SetXY(5,3);
LCD_WriteString("Waiting...");
DataBack_Write(DataBack_GetCount());
DataBack_Send();
Com_SendByte(2,'$');
LCD_Clear();
LCD_SetXY(7,3);
LCD_WriteString("Done~!");
Delay_ms(500);
break;
case keyback:
return;
}
}
#else
LCD_WriteString("Please Enable the DataBack Function");
#endif
Delay_ms(10);
}
}
void LCD_WriteDouble(double data,int m)//写double型数，m为小数点后位数，从高位向低位写
{
if(data<0&&data>-1)
LCD_WriteChar('-');//写负号
LCD_WriteInt((int)data);//写整数部分
if(m>0)
{
int i;
LCD_WriteChar('.');//写小数点
if(data<0)
data=-data;
data=data-(int)data;
for(i=0;i<m;i++)//写小数部分
{
data=data*10;
LCD_WriteChar((int)data+'0');
data=data-(int)data;
}
}
}
void FM_DisplayChannel(int channel, xpos, ypos){
LCD_Clear();
LCD_GoTo(xpos,ypos);
if(channel<1000){
LCD_WriteInt(digitAtPos(channel,1), true);
LCD_GoTo(xpos+LARGE_SPACE, ypos);
LCD_WriteInt(digitAtPos(channel,2), true);
LCD_GoTo(xpos+LARGE_SPACE+SMALL_SPACE, ypos);
LCD_WriteInt(digitAtPos(channel,3),true);
}
else{
LCD_WriteInt(digitAtPos(channel,1), true);
LCD_GoTo(xpos+LARGE_SPACE, ypos);
LCD_WriteInt(digitAtPos(channel,2), true);
LCD_GoTo(xpos+LARGE_SPACE+LARGE_SPACE, ypos);
LCD_WriteInt(digitAtPos(channel,3),true);
LCD_GoTo(xpos+2*LARGE_SPACE+SMALL_SPACE);
LCD_WriteInt(digitAtPos(channel,4),true);
}
}
/*
void LCD_PrintNumStr(uchar x,uchar y,uchar *s)
{
x =(x / 8) * 8;
while(*s)
{
LCD_PutImg(x,y,1,11,Num_Tab + (*s - '0') * 11);
x = x + 8;
s++;
}
}
void LCD_PrintBlackBlock(uchar x,uchar y,bool not_empty)
{
x =(x / 8) * 8;
LCD_PutImg(x,y,1,11,BlockTab + (not_empty?0:1) * 11);
}
*/
void LCD_ReverseRect(uchar x,uchar y,uchar w,uchar h)
{
uint data addr;
uchar data i,j;
uchar data c;
x =(x / 8) * 8;
addr = LCD_GRAPH_HOME_ADDR + LCD_WIDTH * y + x/8;
for(j=0;j<h;j++)
{
for(i=0;i<w;i++)
{
LCD_WriteInt(addr +i,0x24);
LCD_Comm(LC_NOC_RD);
c = LCD_Read();
c = ~c;
LCD_Write1(c,LC_NOC_WR);
}
addr += LCD_WIDTH;
}
}
/*
* 函数名: Mecanum_test
* 描 述: 轮子调试
* 输 入: 轮子状态信息
* 输 出: 无
* 调 用: 外部调用
*/
void Mecanum_test(struct Mecanum_State *mec)
{
rt_uint32_t key_value;
double speed_pwm=50;
/*->*/mec_test:
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("======Mecanum=======");
LCD_SetXY(0,1);
LCD_WriteString("1.Speed");
LCD_SetXY(0,2);
LCD_WriteString("2.ChangeArg");
LCD_SetXY(0,3);
LCD_WriteString("3.ChangePort now:");
LCD_WriteInt((*mec).port);
//
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1:
goto mec_speed;
case key2:
Input_IntValue(&(*mec).arg,"Argument");
break;
case key3:
// Input_IntValue(int(&(*mec).ID_NUM),"Can");
break;
case keyback:
return;
}
}
Delay_ms(10);
}
/*->*/mec_speed:
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("======Mecanum=======");
LCD_SetXY(0,1);
LCD_WriteString("1.SpeedUp");
LCD_SetXY(0,2);
LCD_WriteString("2.SpeedDown");
LCD_SetXY(0,3);
LCD_WriteString("3.Stop");
LCD_SetXY(0,4);
LCD_WriteString("4.Set Speed");
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1:
speed_pwm+=1;
break;
case key2:
speed_pwm-=1;
break;
case key3:
speed_pwm=50;
break;
case key4:
Input_DoubleValue(&speed_pwm,"Goal Speed");
break;
case keyback:
// motor_speed=0;
goto mec_test;
}
PWM_SetDuty((*mec).port,speed_pwm);
}
Delay_ms(10);
}
}
void GoRoute3(void)//放最远的树叶
{
struct Pointfp64 pos_scan1,pos_scan2,pos_tmp;
struct Pointfp64 pos_real1,pos_real2;
struct Pointfp64 pos_ring1= C_Ring1,pos_ring2=C_Ring2;
struct Pointfp64 pos_robot;
uint8 flag1=0,flag2=0,i;
fp64 dist1,dist2,radian1,radian2,robot_radian;
struct Pointfp64 error1,error2;
int flaglidar;
struct Pointfp64 temp;
Route_Num=3;
// SetPointPath(Route[2]+1,Route[3],PointRoute[Route[3]].selfangle,PointRoute[Route[3]].position);
flaglidar=1;
while(RouteFinish)
{
if((Point_NowNum>=Point_EndNum-250)&&flaglidar==1)
{
pos_robot=Gps_List[0].position;
dist1=MATH_Distance(pos_robot,pos_ring1);
dist2=MATH_Distance(pos_robot,pos_ring2);
radian1=atan((pos_robot.y-pos_ring1.y)/(pos_robot.x-pos_ring1.x))-Gps_List[0].radian;
radian2=atan((pos_robot.y-pos_ring2.y)/(pos_robot.x-pos_ring2.x))-Gps_List[0].radian;
pos_real1.x=dist1*sin(-radian1);
pos_real1.y=dist1*cos(-radian1)-335;
pos_real2.x=dist2*sin(-radian2);
pos_real2.y=dist2*cos(-radian2)-335;
Lidar_Stat=Lidar_Route_3;//第三段的修正方式
flaglidar=0;
robot_radian=Gps_List[0].radian;
#if WalkGroundEnable
RouteForm=RouteStop;
#endif
}
if(Lidar_Stat==Lidar_Route_3)//在雷达传输完成前，液晶屏显示"waiting"
{
LCD_SetXY(0,0);
LCD_WriteString("Waiting1");
}
else if (flaglidar==0)
{
//雷达任务会给出 Lidar_Num_Result 个圆环中心
for (i=0;i<Lidar_Num_Result;i++)//处理雷达数据，将其他环的圆心剔除
{
pos_tmp.x=Lidar_ResultX[i]-Ring1_Shift.x;
pos_tmp.y=Lidar_ResultY[i]-Ring1_Shift.y;
dist1=MATH_Distance(pos_tmp,pos_real1);
dist2=MATH_Distance(pos_tmp,pos_real2);
if (dist1<500)//若圆心相对车体距离误差小于500，取之
{
flag1=i+1; pos_scan1=pos_tmp;
error1.x=pos_tmp.x-pos_real1.x;
error1.y=pos_tmp.y-pos_real1.y;
}
if (dist2<200)
{
flag2=i+1; pos_scan2=pos_tmp;
error2.x=pos_tmp.x-pos_real2.x;
error2.y=pos_tmp.y-pos_real2.y;
}
}
error2=error1;
if (flag1!=0)//修改坐标
{
temp.y=(error1.y+error2.y)/2;
temp.x=(error1.x+error2.x)/2;
Gps_List[0].position.x-=temp.y*cos(-robot_radian)-temp.x*sin(-robot_radian);
Gps_List[0].position.y+=temp.y*sin(-robot_radian)+temp.x*cos(-robot_radian);
fix_2.x=temp.y*cos(-robot_radian)-temp.x*sin(-robot_radian);
fix_2.y=-(temp.y*sin(-robot_radian)+temp.x*cos(-robot_radian));
#if WalkGroundEnable
rGPJDAT|=0x1000; //开蜂鸣器
#endif
flaglidar=-1;
}
else
{
flaglidar=-2;
rGPJDAT&=~0x1000; //关蜂鸣器
}
LCD_Clear();
LCD_SetXY(10,0);
LCD_WriteInt(flaglidar); //-3
LCD_SetXY(0,3);
LCD_WriteInt(pos_real1.x); //495
LCD_SetXY(10,3);
LCD_WriteInt(pos_real1.y); //1448
LCD_SetXY(0,4);
LCD_WriteInt(pos_scan1.x); //
LCD_SetXY(10,4);
LCD_WriteInt(pos_scan1.y); //
LCD_SetXY(0,5);
LCD_WriteInt(temp.y*cos(-robot_radian)-temp.x*sin(-robot_radian)); //
LCD_SetXY(10,5);
LCD_WriteInt(-(temp.y*sin(-robot_radian)+temp.x*cos(-robot_radian))); //
LCD_SetXY(0,6);
LCD_WriteNum(radian1/PI*180); //-15
LCD_SetXY(10,6);
LCD_WriteInt(radian2/PI*180); //-46
OSTimeDly(1);
#if WalkGroundEnable
for(;;)
{
if(Button7_On)
{
RouteForm=PointLine;
LCD_SetXY(0,7);
LCD_WriteString("out break!");
break;
}
OSTimeDly(1);
}
#endif
}
if(Point_NowNum>=Point_EndNum-1)
{
RouteFinish=0;
Route_Num++;
OSMboxPost(ArmMbox,&Route_Num);
LCD_SetXY(0,0);
LCD_WriteString("route3 done");
}
OSTimeDly(1);
}
}
void LCD_WriteIntXY(int data,int x,int y)
{
LCD_SetXY(x,y);
LCD_WriteInt(data);
}
void Input_IntValue(int *address,char *name)//整型输入
{
u32 key_value;
char str[20]={'\0'};
int temp;
int i,j;
str[0]=' ';
i=1;
for(;;)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString(name);
LCD_SetXY(0,1);
LCD_WriteString("old:");
LCD_WriteInt(*address);
LCD_SetXY(0,2);
LCD_WriteString("new:");
LCD_WriteString(str);
if(i<=1)
{
LCD_SetXY(5,2);
LCD_WriteString("Value=0!!!");
}
Delay_ms(5);
if(rt_mb_recv(&Mb_Key, (rt_uint32_t*)&key_value, 0)==RT_EOK)
{
switch(key_value)
{
case 1://1
if(i<14)
str[i++]='1';
break;
case 2://2
if(i<14)
str[i++]='2';
break;
case 3://3
if(i<14)
str[i++]='3';
break;
case 4://4
if(i<14)
str[i++]='4';
break;
case 7://5
if(i<14)
str[i++]='5';
break;
case 8://6
if(i<14)
str[i++]='6';
break;
case 9://7
if(i<14)
str[i++]='7';
break;
case 10://8
if(i<14)
str[i++]='8';
break;
case 13://9
if(i<14)
str[i++]='9';
break;
case 14://0
if(i<14)
str[i++]='0';
break;
case 16://-
if(str[0] == ' ')
str[0] = '-';
else
str[0] = ' ';
break;
case 17://del
if(i>1)
{
str[--i]='\0';
}
break;
case 18://ok
temp=0;
for(j=1;j<i;j++)//计算输入值
{
temp=temp*10+str[j]-'0';
}
if(str[0]=='-')
temp=-temp;
*address=temp;
return;
case 5://cancel
return;
}
}
}
}
/*
* 函数名: Ductedfan_test
* 描 述: 调试涵道的方向
* 输 入: 任意一个涵道
* 输 出: 无
* 调 用: 外部调用
*/
void Ductedfan_test(int *CH)
{
rt_uint32_t key_value;
double speed_pwm = 0;
/*->*/fan_test: while (1)
{
LCD_Clear();
LCD_SetXY(0, 0);
LCD_WriteString("======Ductedfan=======");
LCD_SetXY(0, 1);
LCD_WriteString("1.Speed");
LCD_SetXY(0, 2);
LCD_WriteString("2.ChangePort now:");
LCD_WriteInt(*CH);
if (rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch (key_value)
{
case key1:
goto fan_speed;
case key2:
Input_IntValue(CH, "Port");
break;
case keyback:
return;
}
}
Delay_ms(10);
}
/*->*/fan_speed: while (1)
{
LCD_Clear();
LCD_SetXY(0, 0);
LCD_WriteString("======Ductedfan=======");
LCD_SetXY(0, 1);
LCD_WriteString("Speed_PWM:");
LCD_WriteDouble(get_fan_duty(*CH));
LCD_SetXY(0, 2);
LCD_WriteString("1.SpeedUp");
LCD_SetXY(0, 3);
LCD_WriteString("2.SpeedDown");
LCD_SetXY(0, 4);
LCD_WriteString("3.Stop");
if (rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch (key_value)
{
case key1:
if (speed_pwm < 100)
speed_pwm += 1;
break;
case key2:
if (speed_pwm > 0)
speed_pwm -= 1;
break;
case key3:
speed_pwm = 0;
break;
case keyback:
goto fan_test;
}
if (key_value == keyback)
{
Fan_Stop();
break;
}
Fan_Duty(*CH, speed_pwm);
}
Delay_ms(10);
}
}