/*
* 函数名: RecKin
* 描 述: 测试 Kinect 接受
* 输 入: 无
* 输 出: 无
* 调 用: 外部调用
*/
void RecKin_Print(void)
{
rt_uint32_t key_value;
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("========Receiving========");
LCD_SetXY(0,1);
LCD_Printf("x = %d",(int)uart_data.x);
LCD_SetXY(0,2);
LCD_Printf("y = %d",(int)uart_data.y);
LCD_SetXY(0,3);
LCD_Printf("z = %d",(int)uart_data.z);
LCD_SetXY(0,4);
LCD_Printf("Now Angle:%g %g %g",Motor_Pos[W_MOTOR1_OLD_ID-W_MOTOR1_OLD_ID]*360,\
Motor_Pos[W_MOTOR2_OLD_ID-W_MOTOR1_OLD_ID]*360,Motor_Pos[W_MOTOR3_OLD_ID-W_MOTOR1_OLD_ID]*360);
LCD_SetXY(0,5);
LCD_Printf("1.STOP %d %d %d",(int)(Motor_RealAim[W_MOTOR1_OLD_ID-W_MOTOR1_OLD_ID]*360/1000),\
(int)(Motor_RealAim[W_MOTOR2_OLD_ID-W_MOTOR1_OLD_ID]*360/1000),(int)(Motor_RealAim[W_MOTOR3_OLD_ID-W_MOTOR1_OLD_ID]*360/1000));
LCD_SetXY(0,7);
LCD_WriteString("1.Set_XYZ");
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1:
Input_DoubleValue(&uart_data.x,"x");
Input_DoubleValue(&uart_data.y,"y");
Input_DoubleValue(&uart_data.z,"z");
Input_IntValue(&pos_speed.v_A,"v_A");
Input_IntValue(&pos_speed.v_B,"v_B");
// Input_IntValue(&pos_speed.v_C,"v_C");
Data_Processing();
W_MOTOR_OLD_FUNC(W_MOTOR2_OLD_ID, (float)pos_speed.pos_A , (int16_t)pos_speed.v_A , CMD_SET_PSG);
W_MOTOR_OLD_FUNC(W_MOTOR2_OLD_ID, (float)pos_speed.pos_B , (int16_t)pos_speed.v_B , CMD_SET_PSG);
// W_MOTOR_OLD_FUNC(W_MOTOR3_OLD_ID, (float)pos_speed.pos_C , (int16_t)pos_speed.v_C , CMD_SET_PSG);
break;
case keyback:
return;
}
}
Delay_ms(10);
}
}
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 sys_sonic(void)
{
rt_uint32_t key_value;
while(1)
{
LCD_Clear();
LCD_SetXY(0,0); LCD_WriteString("====pole_test====");
LCD_SetXY(0, 1); LCD_WriteString("1.step1");
LCD_SetXY(0, 2); LCD_WriteString("2.xypid");
LCD_SetXY(0, 3); LCD_WriteString("3.pw1");
LCD_SetXY(0, 4); LCD_WriteString("4.dis");
LCD_SetXY(10, 1); LCD_WriteString("5.sd save");
LCD_SetXY(10, 2); LCD_WriteString("6.");
LCD_SetXY(10, 3); LCD_WriteString("7.");
LCD_SetXY(10, 4); LCD_WriteString("8.");
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1: _step1(); break;
case key2: pw1_pid(); break;
case key3: sonic_pw1(); break;
case key4:
Input_DoubleValue(&std_fdis, "f dis");
Input_DoubleValue(&std_sdis, "s dis");
case key5:
{
int _sonicx = 0 ;
Input_IntValue(&_sonicx, "_sonicx");
sonic_SDsave(_sonicx, "sonic_data", 500);
}
break;
case keyback: return;
}
}
Delay_ms(10);
}
}
/*
* 函数名: 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);
}
}
/*
* 函数名: Text_Catch
* 描 述: 测试电机和数据使用
* 输 入: 无
* 输 出: 无
* 调 用: 外部调用
*/
void Text_Catch(void)
{
rt_uint32_t key_value;
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_Printf("=======Catch=======");
LCD_SetXY(0,1);
LCD_Printf("1.STOP %d %d %d",(int)(Motor_RealAim[W_MOTOR1_OLD_ID-W_MOTOR1_OLD_ID]*360/1000),\
(int)(Motor_RealAim[W_MOTOR2_OLD_ID-W_MOTOR1_OLD_ID]*360/1000),(int)(Motor_RealAim[W_MOTOR3_OLD_ID-W_MOTOR1_OLD_ID]*360/1000));
LCD_SetXY(0,2);
LCD_WriteString("2.Set Angle");
LCD_SetXY(0,3);
LCD_WriteString("3.Set Speed");
LCD_SetXY(0,4);
LCD_WriteString("4.Set Angle and Speed");
LCD_SetXY(0,5);
LCD_WriteString("5.Clear");
LCD_SetXY(12,5);
LCD_WriteString("6.Can");
LCD_SetXY(0,6);
LCD_Printf("Now Angle:%g %g %g",Motor_Pos[W_MOTOR1_OLD_ID-W_MOTOR1_OLD_ID]*360,\
Motor_Pos[W_MOTOR2_OLD_ID-W_MOTOR1_OLD_ID]*360,Motor_Pos[W_MOTOR3_OLD_ID-W_MOTOR1_OLD_ID]*360);
LCD_SetXY(0,7);
LCD_Printf("IOA:%d%d%d%d",M_IO_READ(W_MOTOR1_OLD_ID,1),M_IO_READ(W_MOTOR1_OLD_ID,2),\
M_IO_READ(W_MOTOR1_OLD_ID,3),M_IO_READ(W_MOTOR1_OLD_ID,4));
LCD_SetXY(9,7);
LCD_Printf("IOB:%d%d%d%d",M_IO_READ(W_MOTOR2_OLD_ID,1),M_IO_READ(W_MOTOR2_OLD_ID,2),\
M_IO_READ(W_MOTOR2_OLD_ID,3),M_IO_READ(W_MOTOR2_OLD_ID,4));
// LCD_SetXY(0,8);
// LCD_Printf("IOC:%d%d%d%d",M_IO_READ(W_MOTOR3_OLD_ID,1),M_IO_READ(W_MOTOR3_OLD_ID,2),\
// M_IO_READ(W_MOTOR3_OLD_ID,3),M_IO_READ(W_MOTOR3_OLD_ID,4));
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1:
Moto_Stop(W_MOTOR1_OLD_ID);
Moto_Stop(W_MOTOR2_OLD_ID);
// Moto_Stop(W_MOTOR3_OLD_ID);
break;
case key2:
Input_DoubleValue(&catch_data.angle_A,"Goal angle_A");
Input_DoubleValue(&catch_data.angle_B,"Goal angle_B");
// Input_DoubleValue(&catch_data.angle_C,"Goal angle_C");
Data_Pro_Text();
Moto_Set_GPos(W_MOTOR1_OLD_ID,(float)pos_speed.pos_A);
Moto_Set_GPos(W_MOTOR2_OLD_ID,(float)pos_speed.pos_B);
// Moto_Set_GPos(W_MOTOR3_OLD_ID,(float)pos_speed.pos_C);
break;
case key3:
Input_IntValue(&pos_speed.v_A,"Goal Speed_A");
Input_IntValue(&pos_speed.v_B,"Goal Speed_B");
// Input_IntValue(&pos_speed.v_C,"Goal Speed_C");
Moto_Set_GSpeed(W_MOTOR1_OLD_ID,(int16_t)pos_speed.v_A);
Moto_Set_GSpeed(W_MOTOR2_OLD_ID,(int16_t)pos_speed.v_B);
// Moto_Set_GSpeed(W_MOTOR3_OLD_ID,(int16_t)pos_speed.v_C);
break;
case key4:
Input_DoubleValue(&catch_data.angle_A,"Goal angle_A");
Input_IntValue(&pos_speed.v_A,"Goal Speed");
Input_DoubleValue(&catch_data.angle_B,"Goal angle_B");
Input_IntValue(&pos_speed.v_B,"Goal Speed");
// Input_DoubleValue(&catch_data.angle_C,"Goal angle_C");
// Input_IntValue(&pos_speed.v_C,"Goal Speed");
Data_Pro_Text();
W_MOTOR_OLD_FUNC(W_MOTOR1_OLD_ID, (float)pos_speed.pos_A , (int16_t)pos_speed.v_A , CMD_SET_PSG);
W_MOTOR_OLD_FUNC(W_MOTOR2_OLD_ID, (float)pos_speed.pos_B , (int16_t)pos_speed.v_B , CMD_SET_PSG);
// W_MOTOR_OLD_FUNC(W_MOTOR3_OLD_ID, (float)pos_speed.pos_C , (int16_t)pos_speed.v_C , CMD_SET_PSG);
break;
case key5:
Moto_Clear_NPos(W_MOTOR1_OLD_ID);
Moto_Clear_NPos(W_MOTOR2_OLD_ID);
// Moto_Clear_NPos(W_MOTOR3_OLD_ID);
break;
case key6:
W_MOTOR_OLD_FUNC(W_MOTOR1_OLD_ID, 0 , 0 , CMD_INIT_CAN);
W_MOTOR_OLD_FUNC(W_MOTOR2_OLD_ID, 0 , 0 , CMD_INIT_CAN);
// W_MOTOR_OLD_FUNC(W_MOTOR3_OLD_ID, 0 , 0 , CMD_INIT_CAN);
break;
case keyback:
return;
}
}
Delay_ms(10);
}
}
/*
* 函数名: 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);
}
}
