📄 app._c
字号:
motor_l_msg=0; //1 正 0 反 2 停
motor_r_msg=0;
motor_m_msg=0;
}
else if(rx_chr[1][6][1]&rx_chr[2][6][1]) //dawn x
{
motor_l_msg=1;
motor_r_msg=1;
motor_m_msg=1;
}
else if(rx_chr[1][4][1]&rx_chr[2][6][1]) //up x
{
motor_l_msg=2;
motor_r_msg=2;
motor_m_msg=2;
}
else if(rx_chr[1][6][1]&rx_chr[2][4][1]) //dawn 3
{
motor_l_msg=3;
motor_r_msg=3;
motor_m_msg=3;
}
//单轮--------------------------------------------------
else if(rx_chr[1][4][1]&(~rx_chr[2][4][0])&(~rx_chr[2][4][1])&(~rx_chr[2][5][1])) //探测本次和上次对方键值，保持直线 //up
{
motor_l_msg=4;
motor_r_msg=4;
motor_m_msg=4;
}
else if(rx_chr[1][6][1]&(~rx_chr[2][7][0])&(~rx_chr[2][7][1])&(~rx_chr[2][5][1])) //dawn //探测本次和上次对方键值，保持直线
{
motor_l_msg=5;
motor_r_msg=5;
motor_m_msg=5;
}
else if(rx_chr[2][4][1]&(~rx_chr[1][4][0])&(~rx_chr[1][4][1])) //3 //探测本次和上次对方键值，保持直线
{
motor_l_msg=6;
motor_r_msg=6;
motor_m_msg=6;
}
else if(rx_chr[2][6][1]&(~rx_chr[1][6][0])&(~rx_chr[1][6][1])) // x //探测本次和上次对方键值，保持直线
{
motor_l_msg=7;
motor_r_msg=7;
motor_m_msg=7;
}
/*
else if(rx_chr[1][7][1]) // 左平移
{
motor_l_msg=8;
motor_r_msg=8;
motor_m_msg=8;
}
else if(rx_chr[2][5][1]) // 右平移
{
motor_l_msg=9;
motor_r_msg=9;
motor_m_msg=9;
}
*/
else //stop
{
motor_l_msg=10;
motor_r_msg=10;
motor_m_msg=10;
}
OSMboxPost(motor_l_Box,&motor_l_msg); //发送左电机驱动邮件
OSMboxPost(motor_r_Box,&motor_r_msg); //发送右电机驱动邮件
OSMboxPost(motor_m_Box,&motor_m_msg); //发送中电机驱动邮件
//提升部分--------------------------------------------------
///自动上升下降次数累加
if(rx_chr[1][5][1])
{
t_model=1; //运动类型 手动提升
auto_s_2=0; // 撤销自动上升次数累加
auto_x_2=0; //撤销自动下降累加
}
else if(rx_chr[2][7][1])
{
t_model=2; //运动类型 手动提升
auto_s_2=0; // 撤销自动上升次数累加
auto_x_2=0; //撤销自动下降累加
}
else if(rx_chr[2][5][1]&rx_chr[1][4][1]&(~rx_chr[1][4][0]))//自动上升 按下一次
{
t_model=3; //运动类型 自动上升 （中间停）
auto_s_2++; // 自动上升次数累加
auto_x_2=0; //撤销自动下降累加
}
else if(rx_chr[2][5][1]&rx_chr[1][6][1]&(~rx_chr[1][6][0]))//自动下降 按下一次
{
t_model=4; //运动类型 自动下降（中间停）
auto_s_2=0; // 自动下降累加
auto_x_2++; // 撤销上升次数累加
}
else if(rx_chr[2][5][1]&rx_chr[2][2][1]&(~rx_chr[2][2][0]))//自动上升下降到底
{
t_model=5; //运动类型 自动上升到底
auto_s_2=0; // 撤销自动下降累加
auto_x_2=0; // 撤销上升次数累加
}
else if(rx_chr[2][5][1]&rx_chr[2][0][1]&(~rx_chr[2][0][0]))
{
t_model=6; //运动类型 自动下降到底
auto_s_2=0; // 撤销自动下降累加
auto_x_2=0; // 撤销上升次数累加
}
///自动提升完成程度判断
switch(t_model)
{
case 1: if(!contact_t[2][1]) t_model=0;break;
case 2: if(!contact_t[0][1]) t_model=0;break; //下降完全到底
case 3: if((!contact_t[3][1])&contact_t[3][0]&(auto_s_2>0)) auto_s_2 --;if((auto_s_2==0)||(!contact_t[2][1])) t_model=0;break; //自动上升 中间停 完成一次任务
case 4: if((!contact_t[3][1])&contact_t[3][0]&(auto_x_2>0)) auto_x_2 --;if((auto_x_2==0)||(!contact_t[0][1])) t_model=0;break; //自动下降 中间停 完成一次任务
case 5: if(!contact_t[2][1]) t_model=0;break; //自动上升完全到顶
case 6: if(!contact_t[0][1]) t_model=0;break; //自动下降完全到底
}
//运动产生
if(t_model==1||t_model==3||t_model==5) motor_t_msg=11; //上升
else if(t_model==2||t_model==4||t_model==6) motor_t_msg=12; //下降
else motor_t_msg=10; //停
OSMboxPost(motor_t_Box,&motor_t_msg);
if((t_model==1)||(t_model==2))t_model=0; //手动完成任务 变为停
//手抓控制部分-------------------------------------------------
if((~rx_chr[2][3][0])&rx_chr[2][3][1]&(~rx_chr[2][5][1]))
{
switch(hand_whe[0])
{
case 0: hand_cy[0]=hand_st_2[0];hand_whe[0]=1;break;
case 1: hand_cy[0]=hand_st_3[0];hand_whe[0]=2;break;
case 2: hand_cy[0]=hand_st_2[0];hand_whe[0]=3;break;
case 3: hand_cy[0]=hand_st_1[0];hand_whe[0]=0;break;
}
}
if((~rx_chr[2][1][0])&rx_chr[2][1][1])
{
switch(hand_whe[1])
{
case 0: hand_cy[1]=hand_st_2[1];hand_whe[1]=1;break;
case 1: hand_cy[1]=hand_st_3[1];hand_whe[1]=2;break;
case 2: hand_cy[1]=hand_st_2[1];hand_whe[1]=3;break;
case 3: hand_cy[1]=hand_st_1[1];hand_whe[1]=0;break;
}
}
if((~rx_chr[2][2][0])&rx_chr[2][2][1]&(~rx_chr[2][5][1]))
{
switch(hand_whe[2])
{
case 0: hand_cy[2]=hand_st_2[2];hand_whe[2]=1;break;
case 1: hand_cy[2]=hand_st_3[2];hand_whe[2]=2;break;
case 2: hand_cy[2]=hand_st_2[2];hand_whe[2]=3;break;
case 3: hand_cy[2]=hand_st_1[2];hand_whe[2]=0;break;
}
}
if((~rx_chr[2][0][0])&rx_chr[2][0][1]&(~rx_chr[2][5][1]))
{
switch(hand_whe[3])
{
case 0: hand_cy[3]=hand_st_2[3];hand_whe[3]=1;break;
case 1: hand_cy[3]=hand_st_3[3];hand_whe[3]=2;break;
case 2: hand_cy[3]=hand_st_2[3];hand_whe[3]=3;break;
case 3: hand_cy[3]=hand_st_1[3];hand_whe[3]=0;break;
}
}
if((~rx_chr[2][3][0])&rx_chr[2][3][1]&rx_chr[2][5][1]) //复合键 手爪全部闭合
{
hand_cy[0]=hand_st_3[0];hand_whe[0]=2;
hand_cy[1]=hand_st_3[1];hand_whe[1]=2;
hand_cy[2]=hand_st_3[2];hand_whe[2]=2;
hand_cy[3]=hand_st_3[3];hand_whe[3]=2;
}
//给自动机器人发送信号-------------
if(rx_chr[1][3][1])PORTC|=0x30;
else PORTC&=0xcf;
}
/*
*********************************************************************************************************
* 复位
*********************************************************************************************************
*/
static void reset_action(void)
{
robot_model_key++;
//数码管显示---------------------------
LED_send_16b(0x0100|(robot_model_key/3));
LED_send_16b(0x0200|(robot_model_key/3));
LED_send_16b(0x0300|(robot_model_key/3));
LED_send_16b(0x0400|(robot_model_key/3));
LED_send_16b(0x0500|(robot_model_key/3));
LED_send_16b(0x0600|(robot_model_key/3));
LED_send_16b(0x0700|(robot_model_key/3));
LED_send_16b(0x0800|(robot_model_key/3));
//行进电机--------------
switch(robot_model_key)
{
case 2: motor_l_msg=0;motor_r_msg=0;break;
case 7: motor_l_msg=1;motor_r_msg=1;break;
case 12:motor_l_msg=10;motor_r_msg=10;break;
}
OSMboxPost(motor_l_Box,&motor_l_msg); //发送左电机驱动邮件
OSMboxPost(motor_r_Box,&motor_r_msg); //发送右电机驱动邮件
//手爪----------------------------
switch(robot_model_key)
{
case 2:hand_cy[0]=hand_st_3[0];hand_cy[1]=hand_st_3[1];hand_cy[2]=hand_st_3[2];hand_cy[3]=hand_st_3[3];break;
case 12:hand_cy[0]=hand_st_2[0];hand_cy[1]=hand_st_2[1];hand_cy[2]=hand_st_2[2];hand_cy[3]=hand_st_2[3];break;
case 22:hand_cy[0]=hand_st_1[0];hand_cy[1]=hand_st_1[1];hand_cy[2]=hand_st_1[2];hand_cy[3]=hand_st_1[3];hand_whe[0]=0;hand_whe[1]=0;hand_whe[2]=0;hand_whe[3]=0;break;
}
//提升-----------------------
switch(robot_model_tmp)
{
case 0: motor_t_msg=12; if(!contact_t[0][1]) {robot_model_tmp=1;motor_t_msg=11;}break;
case 1: motor_t_msg=11; if((!contact_t[3][1])&contact_t[3][0]) {robot_model_tmp=2;motor_t_msg=10; }break;
case 2: motor_t_msg=10; robot_model_dly++; if(robot_model_dly==5) robot_model_tmp=3;break;
case 3: motor_t_msg=11; if((!contact_t[3][1])&contact_t[3][0]) {robot_model_tmp=4;motor_t_msg=10;}break;
case 4: motor_t_msg=10;robot_model_dly++; if(robot_model_dly==10) robot_model_tmp=5;break;
case 5: motor_t_msg=11;if(!contact_t[2][1]) {robot_model_tmp=6;motor_t_msg=12;}break;
case 6: motor_t_msg=12;if((!contact_t[3][1])&contact_t[3][0]) {robot_model_tmp=7;motor_t_msg=10;}break;
case 7: motor_t_msg=10;robot_model_tmp=8;robot_model_dly=0;break;
}
OSMboxPost(motor_t_Box,&motor_t_msg);
//结束------------
if(robot_model_tmp==8)//robot_model_key==30
{
robot_model_key=0;
speed_model=1;
robot_model_tmp=0;
OSTaskDel(2);
OSTaskCreate(Task_time_show, (void *)0, (OS_STK *)&Task_time_show_Stk[OS_TASK_STK_SIZE - 1], 2); //创建时间显示任务 优先级2
}
}
/*
*********************************************************************************************************
* 时间显示任务
*********************************************************************************************************
*/
static void Task_time_show(void *p_arg)
{
INT16U time_count_start,time_left=180; //时间起始值 时间剩余值 180秒
INT16U time_led_100,time_led_10,time_led_1; //时间显示的 100位 10位 和 1位
p_arg = p_arg;
LED_send_16b(0x0400); //第四位清零
LED_send_16b(0x0300); //第三位清零
LED_send_16b(0x0203); //第二位3
time_key=OSMboxPend(time_Box,0,&err); //请求触发邮件
time_count_start=OSTimeGet();
//开始计时后 手抓到达1位置---------
hand_cy[0]=hand_st_1[0];
hand_cy[1]=hand_st_1[1];
hand_cy[2]=hand_st_1[2];
hand_cy[3]=hand_st_1[3];
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -