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);
}
}
