//打印在显示器上
void sys_Timer(void)
{
rt_uint32_t key_value;
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("======Timer=======");
LCD_SetXY(0,1);
LCD_WriteString("M_TIME:");
LCD_WriteDouble(M_TIME);
LCD_SetXY(0,2);
LCD_WriteString("S_TIME:");
LCD_WriteDouble(S_TIME);
LCD_SetXY(0,6);
LCD_WriteString("1.Init_Timer");
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1:
Init_Timer();
break;
case keyback:
return;
}
}
Delay_ms(1);
}
}
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 GoCircle(void)
{
//struct Point pos;
double radian;
double error_dis,error_angle;
double Vout_D,Vout_A;
radian = GetLineAngle(GPS.position,Center);
//得到理论坐标与理论角度
//pos.x = Center.x + R*cos(radian);
//pos.y = Center.y + R*sin(radian);
//角度误差和距离误差
error_dis = GetLength(GPS.position,Center) - R;
error_angle = radian - GPS.radian;
if(error_angle>pi)
error_angle-=2*pi;
if(error_angle<-pi)
error_angle+=2*pi;
//PID调整
Vout_D = DistancePID(error_dis,Pid_List[1]);
Vout_A = 1000*AnglePID(error_angle,Pid_List[0]);
Speed_Y = Speed;
Speed_X = Vout_D;
Speed_Rotation = Vout_A;
SetSpeed(Speed_X,Speed_Y,Speed_Rotation);
//LCD显示
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("A:");
LCD_WriteDouble(GPS.radian);
LCD_SetXY(0,1);
LCD_WriteString("X:");
LCD_WriteDouble(GPS.position.x);
LCD_SetXY(9,1);
LCD_WriteString(" Y:");
LCD_WriteDouble(GPS.position.y);
LCD_SetXY(0,2);
LCD_WriteString("error_angle:");
LCD_WriteDouble(error_angle);
LCD_SetXY(0,3);
LCD_WriteString("error_dis:");
LCD_WriteDouble(error_dis);
LCD_SetXY(0,4);
LCD_WriteString("Vout_A:");
LCD_WriteDouble(Vout_A);
LCD_SetXY(0,5);
LCD_WriteString("Vout_D:");
LCD_WriteDouble(Vout_D);
LCD_SetXY(0,6);
LCD_WriteDouble(radian);
}
void LCD_WriteDoubleXY(double data,int m,int x,int y)
{
LCD_SetXY(x,y);
LCD_WriteDouble(data, m);
}
/*
* 函数名: 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);
}
}
