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