/*
* 函数名: 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 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);
}
}
void Input_List(struct Input_ListValue *list,int num)
{
rt_uint32_t key_value;
int i,index;
int allpage=(num-1)/3;
int nowpage=0;
fp64 out;
while(1)
{
LCD_Clear();
for(i=0;i<3;i++)
{
index=nowpage*3+i;
if(index<num)
{
LCD_SetXY(0,i);
LCD_Printf("%d.",i+1);
LCD_WriteString(list[index].name);
LCD_WriteString(":");
switch(list[index].type)
{
case TYPE_INT8:
LCD_WriteNum(*(int8_t*)(list[index].value));
break;
case TYPE_UINT8:
LCD_WriteNum(*(uint8_t*)(list[index].value));
break;
case TYPE_INT16:
LCD_WriteNum(*(int16_t*)(list[index].value));
break;
case TYPE_UINT16:
LCD_WriteNum(*(uint16_t*)(list[index].value));
break;
case TYPE_INT32:
LCD_WriteNum(*(int32_t*)(list[index].value));
break;
case TYPE_UINT32:
LCD_WriteNum(*(uint32_t*)(list[index].value));
break;
case TYPE_INT64:
LCD_WriteNum(*(int64_t*)(list[index].value));
break;
case TYPE_UINT64:
LCD_WriteNum(*(uint64_t*)(list[index].value));
break;
case TYPE_FP32:
LCD_WriteNum(*(fp32*)(list[index].value));
break;
case TYPE_FP64:
LCD_WriteNum(*(fp64*)(list[index].value));
break;
}
}
else
{
LCD_SetXY(0,i);
LCD_WriteString(" ");
}
LCD_SetXY(0,3);
LCD_Printf("5.save 6.go %d/%d ",nowpage+1,allpage+1);
}
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case pageup:
if(nowpage>0)
nowpage--;
else
nowpage=allpage;
break;
case pagedown:
if(nowpage<allpage)
nowpage++;
else
nowpage=0;
break;
case 1:
case 2:
case 3:
index=key_value+nowpage*3-1;
switch(list[index].type)
{
case TYPE_INT8:
out=(*(int8_t*)(list[index].value));
break;
case TYPE_UINT8:
out=(*(uint8_t*)(list[index].value));
break;
case TYPE_INT16:
out=(*(int16_t*)(list[index].value));
break;
case TYPE_UINT16:
out=(*(uint16_t*)(list[index].value));
break;
case TYPE_INT32:
out=(*(int32_t*)(list[index].value));
break;
case TYPE_UINT32:
out=(*(uint32_t*)(list[index].value));
break;
case TYPE_INT64:
out=(*(int64_t*)(list[index].value));
break;
case TYPE_UINT64:
out=(*(uint64_t*)(list[index].value));
break;
case TYPE_FP32:
out=(*(fp32*)(list[index].value));
break;
case TYPE_FP64:
out=(*(fp64*)(list[index].value));
break;
}
Input_DoubleValue(&out,list[index].name);
switch(list[index].type)
{
case TYPE_INT8:
(*(int8_t*)(list[index].value))=out;
break;
case TYPE_UINT8:
(*(uint8_t*)(list[index].value))=out;
break;
case TYPE_INT16:
(*(int16_t*)(list[index].value))=out;
break;
case TYPE_UINT16:
(*(uint16_t*)(list[index].value))=out;
break;
case TYPE_INT32:
(*(int32_t*)(list[index].value))=out;
break;
case TYPE_UINT32:
(*(uint32_t*)(list[index].value))=out;
break;
case TYPE_INT64:
(*(int64_t*)(list[index].value))=out;
break;
case TYPE_UINT64:
(*(uint64_t*)(list[index].value))=out;
break;
case TYPE_FP32:
(*(fp32*)(list[index].value))=out;
break;
case TYPE_FP64:
(*(fp64*)(list[index].value))=out;
break;
}
break;
case key6:
return;
}
}
Delay_ms(5);
}
}
/*
* 函数名: owenTest
* 描 述: 测试函数
* 输 入: 无
* 输 出: 无
* 调 用: 外部调用
*/
void owenTest(void)
{
rt_uint32_t key_value;
static FIL fil_owen;
extern int Handle_Off_Count;
extern int Handle_Off_Count2;
double speed;
/*->*/owen_test:
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("1.Mecanum");
LCD_SetXY(0,1);
LCD_WriteString("2.close f_pid");
LCD_SetXY(0,2);
LCD_WriteString("3.SD_Record");
LCD_SetXY(0,3);
LCD_Printf("4.Handle off: %d",Handle_Off_Count);
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case key1:
goto owen_mecanum;
case key2:
SD_Printf_MQ(&fil_pid,"THE END");
break;
case key3:
goto owen_SD_Record;
case keyback:
return;
}
}
Delay_ms(10);
}
/*->*/owen_mecanum:
Input_DoubleValue(&speed,"Speed_Y");
Wait_Button8_Change();
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("go ahead");
Speed_Y = speed;
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
switch(key_value)
{
case keyback:
Speed_Y = 0;
goto owen_test;
}
}
Delay_ms(10);
}
/*->*/owen_SD_Record:
f_open (&fil_owen, "owen.txt", FA_OPEN_ALWAYS | FA_READ | FA_WRITE);
while(1)
{
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("Recording...");
SD_Printf_MQ(&fil_owen,"%f,%f,%f,%f,%f;\r\n",GPS.position.x,GPS.position.y,GPS.radian,Encoder[0].dis,Encoder[1].dis);
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK)
{
SD_Printf_MQ(&fil_owen,"THE END");
LCD_Clear();
LCD_SetXY(0,0);
LCD_WriteString("file have closeed");
if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_FOREVER) == RT_EOK)
goto owen_test;
}
Delay_ms(10);
}
}
