void _step1(void) { double dis = 0; struct Point goal_point; gps_t gps_save = GPS; SONICx_ENABLE(0X03); Speed_Y = rbyCoef * -600; while((double)(sonic_data[F_SONIC-1].data)>600 || (double)(sonic_data[F_SONIC-1].data)<150) { LCD_Clear(); LCD_SetXY(0, 0); LCD_Printf("_step2: not see pole"); LCD_SetXY(0, 1); LCD_Printf("angle:%f", GPS.radian); Delay_ms(5); } /* check again */ Delay_ms(30); dis = sonic_data[F_SONIC-1].data; if(dis<500 && dis>150) { /* be sure seeing the first pole */ double l_disx = (dis + 220 - std_overDesign); /* local delta distance - x_direction */ double l_disy = -450*rbyCoef; /* local delta distance - y_direction */ double theta = GPS.radian; Speed_Y = 0; msg("see pole, dis:%f", dis); /* local to global */ goal_point = GPS.position; goal_point.x += l_disx*cos(theta) - l_disy*sin(theta); goal_point.y += l_disx*sin(theta) + l_disy*cos(theta); _set_keep(goal_point, gps_save.radian, 1000, 150, 5); } }
static void pw1_pid(void) { rt_uint32_t key_value; // static double coefy2a = 0.8; while(1) { LCD_Clear(); LCD_SetXY(0,0); LCD_WriteString("=====sonic_test====="); LCD_SetXY(0, 1); LCD_WriteString("1.front pid"); LCD_SetXY(0, 2); LCD_WriteString("2.f->pid"); LCD_SetXY(0, 3); LCD_WriteString("3.side pid"); LCD_SetXY(0, 4); LCD_WriteString("4.s->pid"); LCD_SetXY(10, 1); LCD_WriteString("5.fs pid"); if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK) { switch(key_value) { case key1: pole_sonic(0x01); break; case key2: _input_pid(&sPos_Pid[eF_sonic]); break; case key3: pole_sonic(0x02); break; case key4: _input_pid(&sPos_Pid[eS_sonic]); break; case key5: pole_sonic(0x03); break; case keyback: return; } } Delay_ms(10); } }
/* * 函数名: LBTest * 描 述: 灯板测试函数 * 输 入: 无 * 输 出: 无 * 调 用: 外部调用 */ void LBTest(void) { rt_uint32_t key_value; while(1) { LCD_Clear(); LCD_SetXY(0,0); LCD_WriteString("=========LB========="); LCD_SetXY(0,1); LCD_WriteString("LB1:"); LCD_WriteInt(LB_Count(1)); LCD_SetXY(10,1); LCD_WriteString("LB2:"); LCD_WriteInt(LB_Count(2)); LCD_SetXY(0,2); LCD_WriteString("LB1:"); LCD_Printf("%s",itob(LB_recv[0].v)); LCD_SetXY(0,4); LCD_WriteString("LB2:"); LCD_Printf("%s",itob(LB_recv[1].v)); LCD_SetXY(0,6); LCD_WriteString("1.LB_Init"); LCD_SetXY(10,6); LCD_WriteString("2.LB_State"); LCD_SetXY(0,7); LCD_WriteString("3.LB_Test"); if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK) { switch(key_value) { case key1: LB_Init(); break; case key2: LB_State(); break; case key3: LB_Test(); break; case keyback: return; } } Delay_ms(10); } }
void LookIt(void) { rt_uint32_t key_value; // int i; while(1) { LCD_Clear(); LCD_SetXY(0,0);LCD_Printf("1.AngleSwing %d",AngleSwing); LCD_SetXY(0,1);LCD_Printf("2.AngleHit %d",AngleHit); LCD_SetXY(0,2);LCD_Printf("3.pos_A %f",pos_speed.pos_A); LCD_SetXY(0,3);LCD_Printf("4.pos_B %f",pos_speed.pos_B); if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK) { if(key_value == 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); } }
static void _can_senddata(CAN_TypeDef *canx, CanTxMsg *tx) { u8 wait_time = 0; while(CAN_Transmit(canx, tx)==CAN_NO_MB && wait_time<6) { wait_time ++; Delay_ms(5); } /* delay 30ms */ if(wait_time>=6) { extern struct rt_thread *rt_current_thread; int i; LCD_Clear(); LCD_SetXY(2, 3); LCD_Printf("can send timeout!"); f_open (&can_send_file, "can_errInfo.txt", FA_OPEN_ALWAYS | FA_READ | FA_WRITE); f_lseek(&can_send_file, can_send_file.fsize); SD_Printf2(&can_send_file, ""); SD_Printf2(&can_send_file, "-----------------------------------------------"); SD_Printf2(&can_send_file, "-->date:%s", __DATE__); SD_Printf2(&can_send_file, "-->time:%s", __TIME__); SD_Printf2(&can_send_file, "-->tick:%d", rt_tick_get()); SD_Printf2(&can_send_file, "-->thread:%s", rt_current_thread->name); SD_Printf2(&can_send_file, "-->GPS:"); SD_Printf2(&can_send_file, " >x:%f", GPS.position.x); SD_Printf2(&can_send_file, " >y:%f", GPS.position.y); SD_Printf2(&can_send_file, " >a:%f", GPS.radian); SD_Printf2(&can_send_file, "-->txMsg:"); SD_Printf2(&can_send_file, " >ID :%d", tx->StdId); SD_Printf2(&can_send_file, " >len :%d", tx->DLC); SD_Printf2(&can_send_file, " >data", tx->DLC); for(i=0; i<tx->DLC; i++) SD_Printf(&can_send_file, " %x", tx->Data[i]); SD_Printf2(&can_send_file, ""); SD_Printf2(&can_send_file, "-----------------------------------------------"); SD_Printf2(&can_send_file, ""); f_close(&can_send_file); Delay_ms(500); } }
/* * 函数名: 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 GoRoute7(void) { Route_Num=7; // SetPointPath(Route[6]+1,Route[7],PointRoute[Route[7]].selfangle,PointRoute[Route[7]].position); while(RouteFinish) { if(Point_NowNum>=Point_EndNum-1) { RouteFinish=0; RouteForm=RouteStop; LCD_SetXY(0,0); LCD_WriteString("route7 done"); } OSTimeDly(1); } }
static void _kb_draw_text(const char *str) { u16 w, h; LCD_SetFont(Display.keyboard.font); LCD_GetCharDimensions('A', &w, &h); LCD_FillRoundRect(TEXTBOX_X_OFFSET, TEXTBOX_Y_OFFSET, LCD_WIDTH - 2 * TEXTBOX_X_OFFSET, TEXTBOX_HEIGHT, TEXTBOX_ROUND, TEXTBOX_BG_COLOR); // clear the backgroup firstly LCD_DrawRoundRect(TEXTBOX_X_OFFSET, TEXTBOX_Y_OFFSET, LCD_WIDTH - 2 * TEXTBOX_X_OFFSET, TEXTBOX_HEIGHT, TEXTBOX_ROUND, TEXTBOX_OUTLINE); LCD_SetXY(TEXTBOX_X_OFFSET + 2, (TEXTBOX_HEIGHT - h) / 2 + TEXTBOX_Y_OFFSET); LCD_SetFontColor(TEXTBOX_COLOR); LCD_PrintString(str); }
void sonic_SDsave(int sonicx, const char *f_name, int ndata) { FRESULT res; u8 _sonicx = 0; char *_f_name = f_namecrt(f_name); extern struct rt_thread *rt_current_thread; /* enable the sonic */ _sonicx = 0x01<<(sonicx-1); SONICx_ENABLE(_sonicx); res = f_open (&sonic_file[sonicx-1], _f_name, FA_OPEN_ALWAYS | FA_READ | FA_WRITE); f_lseek(&sonic_file[sonicx-1], sonic_file[sonicx-1].fsize); if(res != FR_OK) {LCD_ERROR(); anyKey(0, "file open err:%d", res); return;} SD_Printf2(&sonic_file[sonicx-1], "-------------------------------------------"); SD_Printf2(&sonic_file[sonicx-1], "-->date:%s", __DATE__); SD_Printf2(&sonic_file[sonicx-1], "-->time:%s", __TIME__); SD_Printf2(&sonic_file[sonicx-1], "-->thread:%s", rt_current_thread->name); SD_Printf2(&sonic_file[sonicx-1], "sonic%d data", sonicx); while(ndata-- >0) { SD_Printf2(&sonic_file[sonicx-1], "%d,", sonic_data[sonicx-1].data); Delay_ms(5*6); LCD_Clear(); LCD_SetXY(0, 0); LCD_Printf("sonic data saving : %d", sonic_data[sonicx-1].data); } SD_Printf2(&sonic_file[sonicx-1], "-------------------------------------------"); SD_Printf2(&sonic_file[sonicx-1], ""); SD_Printf2(&sonic_file[sonicx-1], ""); f_close(&sonic_file[sonicx-1]); f_timestamp(_f_name); SONICx_DISABLE(_sonicx); }
void GoRoute6(void)//给自动车树叶 { //PID_flag=0; Route_Num=6; RouteFinish=1; OSTimeDly(400); // SetPointPath(Route[5]+1,Route[6],PointRoute[Route[6]].selfangle,PointRoute[Route[6]].position); while(RouteFinish) { if(Point_NowNum>=Point_EndNum-1) { rGPJDAT&=~0x1000; //关蜂鸣器 RouteFinish=0; //PID_flag=1; RouteForm=RouteStop; LCD_SetXY(0,0); LCD_WriteString("route6 done"); } OSTimeDly(1); } }
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_PrintStringXY(unsigned int x, unsigned int y, const char *str) { LCD_SetXY(x, y); LCD_PrintString(str); }
void LCD_WriteDoubleXY(double data,int m,int x,int y) { LCD_SetXY(x,y); LCD_WriteDouble(data, m); }
void LCD_WriteNumXY(double data,int x,int y) { LCD_SetXY(x,y); LCD_WriteNum(data); }
/* * 函数名: 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); } }
/* * 函数名: 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); } }
void TaskAuto (void *pdata) { //static struct Pointfp64 position; //static fp64 angle; uint8 err; uint8 *msg; pdata=pdata; for(;;) { msg = OSMboxPend(AutoMbox,0,&err); switch (msg[0]) { case 1: LCD_Clear(); GPS_Clear(); RouteForm=RouteStop; PID_Clear(); GoRoute1(); case 2: PID_Clear(); GoRoute2(); break; case 3: PID_Clear(); GoRoute3(); case 4: PID_Clear(); GoRoute4(); case 5: PID_Clear(); GoRoute5(); case 6: rGPJDAT&=~0x1000; //¹Ø·äÃùÆ÷ LCD_Clear(); PID_Clear(); //GoRoute6(); Route_Num=6; SetKeep(PointRoute[Route[6]].position,0.0); while(RouteFinish) { if(((Fp64Abs(Aim_Angle-Gps_List[0].angle)<1)&& (Getlength(AimPoint,Gps_List[0].position)<10))||(Route_Num==7)) { RouteFinish=0; RouteForm=RouteStop; LCD_SetXY(0,2); LCD_WriteString("route6 done"); } OSTimeDly(1); } break; case 7: GPS_Init(PointRoute[Route[6]].position,0.0); LCD_Clear(); PID_Clear(); GoRoute7(); break; case 8: GPS_Init(PointRoute[Route[7]+1].position,Gps_List[0].angle); LCD_Clear(); PID_Clear(); GoRoute8(); Shootest(); break; default:; break; } OSTimeDly(1); } }
void LCD_WriteCharXY(char data,int x,int y) { LCD_SetXY(x,y); LCD_PushByte(data); }
/* * 函数名: 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); } }
/* only in red field */ void pole_sonic(u32 data) { double ferr = 0; double serr = 0; static double Vout[2] = {6.0, 7.0}; rt_uint32_t key_value = 0; SONICx_ENABLE(data); while(1) { double fdis = (double)(sonic_data[F_SONIC-1].data); /* front distance */ double sdis = (double)(sonic_data[S_SONIC-1].data); /* side distance */ //front supersonic adjust if(data&0x01 && sonic_data[F_SONIC-1].data<1000) { double v_max =2400; double basev = 250; double coef = (v_max-basev)/2; buz_msg(0,0,0,200); ferr = fdis - std_fdis; Vout[0] = Pos_Pid(ferr, 1, eF_sonic, 1800.0); //Moto_Set_GSpeed(W_MOTOR1_OLD_ID, (int16_t)Vout[0]); //Vout[0] = Pos_Pid(ferr, 0.998, eF_sonic, 50.0); if(Vout[0]*coef+basev > v_max) W_MOTOR_OLD_FUNC(W_MOTOR1_OLD_ID, Motor_Pos[0]+Vout[0], v_max, CMD_SET_PSG);//CMD_SET_PSG else if(Vout[0]*coef+basev >0) W_MOTOR_OLD_FUNC(W_MOTOR1_OLD_ID, Motor_Pos[0]+Vout[0], basev+Vout[0]*coef, CMD_SET_PSG);//CMD_SET_PSG else if(Vout[0]*coef+basev >-v_max) W_MOTOR_OLD_FUNC(W_MOTOR1_OLD_ID, Motor_Pos[0]+Vout[0], -basev+Vout[0]*coef, CMD_SET_PSG);//CMD_SET_PSG else W_MOTOR_OLD_FUNC(W_MOTOR1_OLD_ID, Motor_Pos[0]+Vout[0], -v_max, CMD_SET_PSG);//CMD_SET_PSG } else if(data&0x01) { Moto_Set_GSpeed(W_MOTOR1_OLD_ID, 0); //anyKey(500, "front sonic can't see"); //buz_msg(3,2,1,200); } else {Moto_Set_GSpeed(W_MOTOR1_OLD_ID, 0);} //side supersonic adjust if(data&0x02 && sonic_data[S_SONIC-1].data<1000) { //buz_msg(0,0,0,200); serr = sdis - std_sdis; Vout[1] = Pos_Pid(serr, 0.999, eS_sonic, 500.0); Speed_Y = rbyCoef * Vout[1]; } else if(data&0x02) { SPEED_STOP; LCD_Clear(); //anyKey(500, " side sonic can't see "); //buz_msg(1,2,3,200); //break; } else {SPEED_STOP;} /* show infomation */ LCD_Clear(); LCD_SetXY(0, 0); LCD_Printf("vout0:%f", Vout[0]); LCD_SetXY(0, 1); LCD_Printf("vout1:%f", Vout[1]); LCD_SetXY(0, 2); LCD_Printf("ferr:%f", ferr); LCD_SetXY(0, 3); LCD_Printf("serr:%f", serr); LCD_SetXY(0, 4); LCD_Printf("fdis:%f", fdis); LCD_SetXY(0, 5); LCD_Printf("sdis:%f", sdis); /* wait for key event */ if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK) { SONICx_DISABLE(0x03); Vout[0] = Vout[1] = 0.0; Speed_Y = 0; Moto_Set_GSpeed(W_MOTOR1_OLD_ID, 0); //buz_msg(0,0,0,200); break; } {/* wait for task finish */ double ferr, serr; ferr = fdis-std_fdis; serr = sdis-std_sdis; if(ferr<=derr && fdis>=-derr && serr<=derr && serr>=-derr) { msg("reach the handover position"); Speed_Y = 0; Moto_Set_GSpeed(W_MOTOR1_OLD_ID, 0); //buz_msg(0,0,0,200); break; } } /* period : 30ms */ Delay_ms(5*6); }//while(1) }
void Input_FloatValue(float * address,char *name)//浮点输入 { u32 key_value; u32 point=0; char str[20]={'\0'}; float temp; int i,j; str[0]=' '; i=1; point=0; for(;;) { LCD_Clear(); LCD_SetXY(0,0); LCD_WriteString(name); LCD_SetXY(0,1); LCD_WriteString("old:"); LCD_WriteNum(*address); LCD_SetXY(0,2); LCD_WriteString("new:"); LCD_WriteString(str); if(i<=1) { LCD_SetXY(5,2); LCD_WriteString("Value=0!!!"); } Delay_ms(20); if(rt_mb_recv(&Mb_Key, (rt_uint32_t*)&key_value, 0)==RT_EOK) { switch(key_value) { case 1://1 if(i<14) str[i++]='1'; break; case 2://2 if(i<14) str[i++]='2'; break; case 3://3 if(i<14) str[i++]='3'; break; case 4://4 if(i<14) str[i++]='4'; break; case 7://5 if(i<14) str[i++]='5'; break; case 8://6 if(i<14) str[i++]='6'; break; case 9://7 if(i<14) str[i++]='7'; break; case 10://8 if(i<14) str[i++]='8'; break; case 13://9 if(i<14) str[i++]='9'; break; case 14://0 if(i<14) str[i++]='0'; break; case 15://. if(point==0&&i<14) { str[i++]='.'; point=1; } break; case 16://- if(str[0] == ' ') str[0] = '-'; else str[0] = ' '; break; case 17://del if(i>1) { if(str[i-1]=='.') point=0; str[--i]='\0'; } break; case 18://ok temp=0; point=0; for(j=1;j<i;j++)// { if(str[j]=='.') { point=10; } else if(point==0) { temp=temp*10+str[j]-'0'; } else { temp+=(str[j]-'0')/(float)point; point=point*10; } } if(str[0]=='-') temp=-temp; *address=temp; return; case 5://cancel return; } } } }
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); } }
void Input_IntValue(int *address,char *name)//整型输入 { u32 key_value; char str[20]={'\0'}; int temp; int i,j; str[0]=' '; i=1; for(;;) { LCD_Clear(); LCD_SetXY(0,0); LCD_WriteString(name); LCD_SetXY(0,1); LCD_WriteString("old:"); LCD_WriteInt(*address); LCD_SetXY(0,2); LCD_WriteString("new:"); LCD_WriteString(str); if(i<=1) { LCD_SetXY(5,2); LCD_WriteString("Value=0!!!"); } Delay_ms(5); if(rt_mb_recv(&Mb_Key, (rt_uint32_t*)&key_value, 0)==RT_EOK) { switch(key_value) { case 1://1 if(i<14) str[i++]='1'; break; case 2://2 if(i<14) str[i++]='2'; break; case 3://3 if(i<14) str[i++]='3'; break; case 4://4 if(i<14) str[i++]='4'; break; case 7://5 if(i<14) str[i++]='5'; break; case 8://6 if(i<14) str[i++]='6'; break; case 9://7 if(i<14) str[i++]='7'; break; case 10://8 if(i<14) str[i++]='8'; break; case 13://9 if(i<14) str[i++]='9'; break; case 14://0 if(i<14) str[i++]='0'; break; case 16://- if(str[0] == ' ') str[0] = '-'; else str[0] = ' '; break; case 17://del if(i>1) { str[--i]='\0'; } break; case 18://ok temp=0; for(j=1;j<i;j++)//计算输入值 { temp=temp*10+str[j]-'0'; } if(str[0]=='-') temp=-temp; *address=temp; return; case 5://cancel return; } } } }
void CameraTest(void) { rt_uint32_t key_value; while(1) { LCD_Clear(); LCD_SetXY(0,0); LCD_Printf("1.ShowInfo"); LCD_SetXY(0,1); LCD_Printf("2.Handover"); LCD_SetXY(0,2); LCD_Printf("3.Pole"); LCD_SetXY(0,3); LCD_Printf("4.Circle_sta"); LCD_SetXY(0,4); LCD_Printf("5.WhiteTape"); LCD_SetXY(0,5); LCD_Printf("6.Recode"); LCD_SetXY(0,6); LCD_Printf("7.Circle_dyn"); LCD_SetXY(0,7); LCD_Printf("8.TestData"); if(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) == RT_EOK) { switch(key_value) { case key1: ShowInfo(); break; case key2: Handover(TEST,NONE); break; case key3: while(rt_mb_recv(&Mb_Key, &key_value, RT_WAITING_NO) != RT_EOK) { LCD_Clear(); LCD_SetXY(0,0); LCD_Printf("1.NEW"); LCD_SetXY(0,1); LCD_Printf("2.OLD"); Delay_ms(20); } switch(key_value) { case key1: PolePutTest(); break; case key2: PolePut(TEST); break; default: break; } break; case key4: CamCircle_Static(TEST,NONE); break; case key5: CamWhiteTape(TEST); break; case key6: CamRecode(NONE,NONE,TEST); case key7: CamCircle_Dynamic(TEST); break; case key8: ShowData(); break; case keyback: return; } } Delay_ms(20); } }
void LCD_WriteStringXY(char *data,int x,int y) { LCD_SetXY(x,y); LCD_PushString(data); }
void GoRoute3(void)//放最远的树叶 { struct Pointfp64 pos_scan1,pos_scan2,pos_tmp; struct Pointfp64 pos_real1,pos_real2; struct Pointfp64 pos_ring1= C_Ring1,pos_ring2=C_Ring2; struct Pointfp64 pos_robot; uint8 flag1=0,flag2=0,i; fp64 dist1,dist2,radian1,radian2,robot_radian; struct Pointfp64 error1,error2; int flaglidar; struct Pointfp64 temp; Route_Num=3; // SetPointPath(Route[2]+1,Route[3],PointRoute[Route[3]].selfangle,PointRoute[Route[3]].position); flaglidar=1; while(RouteFinish) { if((Point_NowNum>=Point_EndNum-250)&&flaglidar==1) { pos_robot=Gps_List[0].position; dist1=MATH_Distance(pos_robot,pos_ring1); dist2=MATH_Distance(pos_robot,pos_ring2); radian1=atan((pos_robot.y-pos_ring1.y)/(pos_robot.x-pos_ring1.x))-Gps_List[0].radian; radian2=atan((pos_robot.y-pos_ring2.y)/(pos_robot.x-pos_ring2.x))-Gps_List[0].radian; pos_real1.x=dist1*sin(-radian1); pos_real1.y=dist1*cos(-radian1)-335; pos_real2.x=dist2*sin(-radian2); pos_real2.y=dist2*cos(-radian2)-335; Lidar_Stat=Lidar_Route_3;//第三段的修正方式 flaglidar=0; robot_radian=Gps_List[0].radian; #if WalkGroundEnable RouteForm=RouteStop; #endif } if(Lidar_Stat==Lidar_Route_3)//在雷达传输完成前,液晶屏显示"waiting" { LCD_SetXY(0,0); LCD_WriteString("Waiting1"); } else if (flaglidar==0) { //雷达任务会给出 Lidar_Num_Result 个圆环中心 for (i=0;i<Lidar_Num_Result;i++)//处理雷达数据,将其他环的圆心剔除 { pos_tmp.x=Lidar_ResultX[i]-Ring1_Shift.x; pos_tmp.y=Lidar_ResultY[i]-Ring1_Shift.y; dist1=MATH_Distance(pos_tmp,pos_real1); dist2=MATH_Distance(pos_tmp,pos_real2); if (dist1<500)//若圆心相对车体距离误差小于500,取之 { flag1=i+1; pos_scan1=pos_tmp; error1.x=pos_tmp.x-pos_real1.x; error1.y=pos_tmp.y-pos_real1.y; } if (dist2<200) { flag2=i+1; pos_scan2=pos_tmp; error2.x=pos_tmp.x-pos_real2.x; error2.y=pos_tmp.y-pos_real2.y; } } error2=error1; if (flag1!=0)//修改坐标 { temp.y=(error1.y+error2.y)/2; temp.x=(error1.x+error2.x)/2; Gps_List[0].position.x-=temp.y*cos(-robot_radian)-temp.x*sin(-robot_radian); Gps_List[0].position.y+=temp.y*sin(-robot_radian)+temp.x*cos(-robot_radian); fix_2.x=temp.y*cos(-robot_radian)-temp.x*sin(-robot_radian); fix_2.y=-(temp.y*sin(-robot_radian)+temp.x*cos(-robot_radian)); #if WalkGroundEnable rGPJDAT|=0x1000; //开蜂鸣器 #endif flaglidar=-1; } else { flaglidar=-2; rGPJDAT&=~0x1000; //关蜂鸣器 } LCD_Clear(); LCD_SetXY(10,0); LCD_WriteInt(flaglidar); //-3 LCD_SetXY(0,3); LCD_WriteInt(pos_real1.x); //495 LCD_SetXY(10,3); LCD_WriteInt(pos_real1.y); //1448 LCD_SetXY(0,4); LCD_WriteInt(pos_scan1.x); // LCD_SetXY(10,4); LCD_WriteInt(pos_scan1.y); // LCD_SetXY(0,5); LCD_WriteInt(temp.y*cos(-robot_radian)-temp.x*sin(-robot_radian)); // LCD_SetXY(10,5); LCD_WriteInt(-(temp.y*sin(-robot_radian)+temp.x*cos(-robot_radian))); // LCD_SetXY(0,6); LCD_WriteNum(radian1/PI*180); //-15 LCD_SetXY(10,6); LCD_WriteInt(radian2/PI*180); //-46 OSTimeDly(1); #if WalkGroundEnable for(;;) { if(Button7_On) { RouteForm=PointLine; LCD_SetXY(0,7); LCD_WriteString("out break!"); break; } OSTimeDly(1); } #endif } if(Point_NowNum>=Point_EndNum-1) { RouteFinish=0; Route_Num++; OSMboxPost(ArmMbox,&Route_Num); LCD_SetXY(0,0); LCD_WriteString("route3 done"); } OSTimeDly(1); } }
void LCD_WriteIntXY(int data,int x,int y) { LCD_SetXY(x,y); LCD_WriteInt(data); }
/* * 函数名: rt_thread_entry_handle * 描 述: 手柄线程的入口函数 * 输 入: 无 * 输 出: 无 * 调 用: 内部调用 * 说 明: */ static void rt_thread_entry_handle(void* parameter) { double Handle_Speed_X; double Handle_Speed_Y; double Handle_Speed_Rotation; struct Point temp_point; struct Point now_point; u32 temp,temp2; u8 delay_flag = 1; u8 hall_flag = 0; u8 time_flag = 1,time_flag2 = 1; u8 handleoff_flag = 1,handleoff_flag2 = 1; u8 flag[6] = {0}; //对应电机的运行状态 int16_t Speed_M = 500; float pos_openfan = 4; extern float Motor2_Aim_Pos[]; extern u8 Motor2_Time_Flag; u8 x_con_flag = 0; //X方向闭环 u8 XY_Only = 0; Handle_Speed_X = 0; Handle_Speed_Y = 0; HandleData_X.lr= 12288; HandleData_Y.lr= 12288; TIM2->CNT = 0; while(1) { temp = *handle_count; temp2 = *handle_count2; while(*handle_count == temp) { time_flag++; Delay_ms(5); if(time_flag > 60)//超时 { time_flag = 0; handle_timeout_flag = 1; if(handleoff_flag) { Handle_Off_Count++; handleoff_flag = 0; } break; } } while(*handle_count2 == temp2) { time_flag2++; Delay_ms(5); if(time_flag2 > 60)//超时 { time_flag2 = 0; handle_timeout_flag2 = 1; if(handleoff_flag2) { Handle_Off_Count2++; handleoff_flag2 = 0; } break; } } if(time_flag||time_flag2)//手柄工作正常 { time_flag = 1; time_flag2 = 1; handle_timeout_flag = 0; handle_timeout_flag2 = 0; handleoff_flag = 1; handleoff_flag2 = 1; Handle_Speed_X = HandleData_X.lr-12288; Handle_Speed_Y = HandleData_Y.lr-12288; Handle_Speed_Rotation = 0; //Handle_Speed_Y = HandleData_Y.ud-12288; //Handle_Speed_Rotation = (HandleData_X.turn-12288)/10.0; if(Button10_On && Button7_Off) { Handle_Speed_X /= 10.0; Handle_Speed_Y /= 10.0; Handle_Speed_Rotation /= 10.0; Speed_M = 150; } else Speed_M = 500; if(Button9_On) { // HandoverFlag[0]=1; // HandoverFlag[1]=1; // HandoverFlag[2]=0; // PoleFlag = 1; // SwingFlag = 1; } if(Button9_Off) { // HandoverFlag[0]=0; // HandoverFlag[1]=0; // HandoverFlag[2]=0; // PoleFlag = 0; // SwingFlag = 0; } if(XY_Only) { // Handle_Speed_X=0; Handle_Speed_Rotation=0; } if(GROUND==RED_GROUND) { Handle_Speed_X = -Handle_Speed_X; Handle_Speed_Y = -Handle_Speed_Y; } if(x_con_flag) { temp_point.x = GPS.position.x + Handle_Speed_X*cos(GPS.radian); temp_point.y = GPS.position.y + Handle_Speed_X*sin(GPS.radian); SetSpeed(Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation); EasyLine(temp_point,GPS.radian,Abs(Handle_Speed_X)); } else SetSpeed(Speed_X+Handle_Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation); //SetSpeed(Speed_X,Speed_Y,Speed_Rotation+Handle_Speed_Rotation); //手柄前后出现问题,先无视之。。。 /*********************************上层动作控制***********************************/ //------------单键控制电机------------// if(Button1_Up && Button7_Off && flag[1]==0) { flag[1]=1; Moto_Set_GSpeed(W_MOTOR1_OLD_ID, 4*Speed_M); } if(Button1_Down && Button7_Off && flag[1]==0) { flag[1]=1; Moto_Set_GSpeed(W_MOTOR1_OLD_ID, -4*Speed_M); } if(Button1_Off && (flag[1]==1)) { flag[1]=0; Moto_Stop(W_MOTOR1_OLD_ID); } if(Button2_Up && Button7_Off && flag[2]==0) { flag[2]=1; Moto_Set_GSpeed(W_MOTOR2_OLD_ID, -3*Speed_M); } if(Button2_Down && Button7_Off && flag[2]==0) { flag[2]=1; Moto_Set_GSpeed(W_MOTOR2_OLD_ID, 3*Speed_M); } if(Button2_Off && (flag[2]==1)) { Moto_Stop(W_MOTOR2_OLD_ID); flag[2]=0; } if(Button3_Up && Button7_Off && flag[3]==0) { flag[3]=1; Moto_Set_GSpeed(W_MOTOR3_OLD_ID, -30*Speed_M); } if(Button3_Down && Button7_Off && flag[3]==0) { flag[3]=1; Moto_Set_GSpeed(W_MOTOR3_OLD_ID, 30*Speed_M); } if(Button3_Off && (flag[3]==1)) { flag[3]=0; Moto_Stop(W_MOTOR3_OLD_ID); } //------------涵道------------// if(Button4_Up && Button7_Off && Button10_Off) { Fan_Duty(L_CH,60.0); Fan_Duty(R_CH,60.0); } if(Button4_Down && Button7_Off && Button10_Off) { Fan_Stop(); } //------------霍尔------------// if(Button5_Up && Button7_Off && Button10_Off && hall_flag == 0) { hall_flag = 1; if(Hall_Count > 6) Hall_Count=1; Hall_Send(Hall_Count); } if(Button5_Down && Button7_Off && Button10_Off && hall_flag == 0) { hall_flag = 1; if(Hall_Count > 1) Hall_Count -= 2; Hall_Send(Hall_Count); } if(Button5_Off && hall_flag == 1) { hall_flag = 0; } //------------双键组合用来控制电磁阀,7键为第二功能键------------// if(Button6_Up && Button7_Off && Button10_Off){ HandPush(); } if(Button6_Down && Button7_Off && Button10_Off){ HandPushOff(); } //缓冲装置 if(Button1_Up && Button7_On && Button10_Off){ BufferOn(); } if(Button1_Down && Button7_On && Button10_Off){ BufferOff(); } //旋转调姿 if(Button2_Up && Button7_On && Button10_Off){ HandTurnRight(); } if(Button2_Down && Button7_On && Button10_Off){ HandTurnLeft(); } //俯仰调姿 if(Button3_Up && Button7_On && Button10_Off){ HandRaise(); } if(Button3_Down && Button7_On && Button10_Off){ HandFall(); } //放置大炮 if(Button4_Up && Button7_On && Button10_Off){ GunOn(); } if(Button4_Down && Button7_On && Button10_Off){ GunOff(); } //开炮 if(Button5_Up && Button7_On && Button10_Off){ FireOn(); } if(Button5_Down && Button7_On && Button10_Off){ FireOff(); } //交接爪 if(Button6_Up && Button7_On && Button10_Off){ HandOpen(); } if(Button6_Down && Button7_On && Button10_Off){ HandClose(); } //------------双键组合用来控制上下电机的位置,10键为第二功能键------------// if(Button4_Up && Button7_Off && Button10_On){ Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[0]); } if(Button4_Down && Button7_Off && Button10_On){ Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[2]); } if(Button5_Up && Button7_Off && Button10_On){ Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[4]); } if(Button5_Down && Button7_Off && Button10_On){ Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[6]); } if(Button6_Up && Button7_Off && Button10_On){ HandUD(0,NO); Delay_ms(500); HandClose();//关爪子 HandFB(0,NO,3500); FireOn();//开炮 Delay_ms(100); now_point.x = GPS.position.x-700*cos(GPS.radian); now_point.y = GPS.position.y-700*sin(GPS.radian); SetLine(now_point,GPS.radian,100,1200,0); while(1) { GoLine(); SetSpeed(Speed_X+Handle_Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation); if(delay_flag) delay_flag++; if(delay_flag>100) { delay_flag = 0; FireOff(); } if(GetLength(GPS.position,now_point) < 10) { SPEED_STOP; break; } Delay_ms(5); } delay_flag = 1; FireClear(); Delay_ms(50); BufferOn();//打开缓冲装置 Delay_ms(500); BufferOff();//重新复位缓冲装置 } if(Button6_Down && Button7_Off && Button10_On){ now_point.x = GPS.position.x+680*cos(GPS.radian); now_point.y = GPS.position.y+680*sin(GPS.radian); SetLine(now_point,GPS.radian,300,1200,0); HandFB(Motor1_Aim_Pos[4],NO,3500);//抓取自动机器人 while(1) { GoLine(); SetSpeed(Speed_X+Handle_Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation); if(GetLength(GPS.position,now_point) < 10) { SPEED_STOP; break; } Delay_ms(5); } SetSpeed(Speed_X,Speed_Y,Speed_Rotation); HandUD(0.5,NO); HandOpen();//松开爪子 Delay_ms(300); FireOff(); BufferOn();//打开缓冲装置 } //三键组合用来重复任务 if(Button1_Up && Button7_On && Button10_On){ Retry = 1; RouteFinish=0; Hall_Send(1);//翘翘板初始化 rt_mb_send(&Mb_Emergency, 7); rt_mb_send (&Mb_Arm, 1); } if(Button2_Up && Button7_On && Button10_On){ Retry = 1; RouteFinish=0; Hall_Send(2);//梅花桩初始化 rt_mb_send(&Mb_Emergency, 7); rt_mb_send (&Mb_Arm, 4); } if(Button3_Up && Button7_On && Button10_On){ Retry = 1; RouteFinish=0; Hall_Send(4);//秋千初始化 rt_mb_send(&Mb_Emergency, 7); rt_mb_send (&Mb_Arm, 6); } if(Button4_Up && Button7_On && Button10_On){ Retry = 1; RouteFinish=0; Hall_Send(6);//楼梯初始化 rt_mb_send(&Mb_Emergency, 7); rt_mb_send (&Mb_Arm, 7); } if(Button5_Up && Button7_On && Button10_On){ rt_mb_send(&Mb_Emergency, 6); } if(Button6_Up && Button7_On && Button10_On){ Retry = 1; RouteFinish=0; rt_mb_send(&Mb_Emergency, 7); //Motor_Init(); FireOff(); BufferOn(); ChooseGround_Pole(GROUND); W_MOTOR_OLD_FUNC(W_MOTOR1_OLD_ID, 0.5 , 1000 , CMD_SET_PSG); Moto_Set_GPos(W_MOTOR2_OLD_ID,2.55); LCD_Clear(); LCD_SetXY(0,1); LCD_Printf("Ready to go!"); Wait_Button8_Change(); rt_mb_send (&Mb_Auto, 1); } } else { SPEED_STOP; //Acc_Limit_Enable = 0; SetSpeed(0,0,0); //Acc_Limit_Enable = 1; Moto_Stop(W_MOTOR1_OLD_ID); Moto_Stop(W_MOTOR2_OLD_ID); //Moto_Stop(W_MOTOR3_OLD_ID); //Com_Printf(5,"v0\r"); } if(Abs(Motor_Pos[0])>pos_openfan && CHILD_ON && !fan_flag) { fan_flag = 1; Fan_Duty(L_CH,60.0); Fan_Duty(R_CH,60.0); } if((Abs(Motor_Pos[0])<pos_openfan || CHILD_OFF) && fan_flag) { fan_flag = 0; Fan_Stop(); } if(Motor2_Time_Flag>10) { //W_MOTOR_OLD_FUNC(W_MOTOR2_OLD_ID, 0 , 0 , CMD_INIT_CAN); Motor2_Time_Flag = 0; } 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); } }