/* * 函数名: 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); } }