|
|
@@ -64,38 +64,191 @@ uint8_t ctr_Fan_IN(uint8_t status_ws)
|
|
|
return 0;
|
|
|
}
|
|
|
/****************************定制色诱*************************************/
|
|
|
-void ctr_Color_Key(uint8_t key_num)
|
|
|
+/**/
|
|
|
+uint8_t Read_Color_Key(rt_base_t key_pin)
|
|
|
{
|
|
|
- if(key_num==0)
|
|
|
+ rt_uint32_t sta_s = 0;
|
|
|
+ rt_pin_mode(key_pin, PIN_MODE_INPUT);
|
|
|
+ while(rt_pin_read(key_pin)==1)
|
|
|
{
|
|
|
- rt_pin_mode(LIMIT_CLOSE, PIN_MODE_INPUT);
|
|
|
- while(rt_pin_read(LIMIT_CLOSE)==1)
|
|
|
+ sta_s++;
|
|
|
+ /*持续的*/
|
|
|
+ rt_thread_delay(50);
|
|
|
+ }
|
|
|
+ while(rt_pin_read(key_pin)==0)
|
|
|
+ {
|
|
|
+ rt_thread_delay(50);
|
|
|
+ if(sta_s>0) break;
|
|
|
+ }
|
|
|
+ LOG_W("检测到IO0翻转!!!");
|
|
|
+ return 1;
|
|
|
+
|
|
|
+}
|
|
|
+/*读限位开关状态*/
|
|
|
+uint8_t Read_Color_Limit(rt_base_t pin_limit)
|
|
|
+{
|
|
|
+ uint8_t l_sta = 0;
|
|
|
+ rt_pin_mode(pin_limit, PIN_MODE_INPUT);
|
|
|
+ l_sta = rt_pin_read(pin_limit);
|
|
|
+ rt_thread_delay(20);
|
|
|
+ l_sta = rt_pin_read(pin_limit);
|
|
|
+ return l_sta;
|
|
|
+}
|
|
|
+/*色诱板*/
|
|
|
+int ctr_Color_board(uint8_t forward,uint16_t time_board)
|
|
|
+{
|
|
|
+ if(forward==CLOSE) //归位
|
|
|
+ {
|
|
|
+ if(device_status.cul_time>7) device_status.cul_time=7;
|
|
|
+ if(device_status.cul_time!=7)
|
|
|
{
|
|
|
- /*持续的*/
|
|
|
- rt_thread_delay(50);
|
|
|
+ uint8_t ct = device_status.cul_time;
|
|
|
+ while(1)
|
|
|
+ {
|
|
|
+ Beep_Tip(100);
|
|
|
+ rt_thread_delay(100);
|
|
|
+ ct--;
|
|
|
+ if(ct==0) break;
|
|
|
+ }
|
|
|
+ //return device_status.cul_time;
|
|
|
}
|
|
|
- LOG_W("检测到IO0翻转为0!!!");
|
|
|
- while(rt_pin_read(LIMIT_CLOSE)==0)
|
|
|
+ if(Read_Color_Limit(LIMIT_RESET)==0)
|
|
|
{
|
|
|
- rt_thread_delay(50);
|
|
|
+ LOG_W("已在初始位置!");
|
|
|
+ Beep_Tip(1000);
|
|
|
+ return 0;
|
|
|
+ //黄板已经在复位状态,直接返回0;
|
|
|
+ }
|
|
|
+ ctr_cangmen_up(CLOSE);
|
|
|
+ rt_thread_delay(10000);
|
|
|
+ ctr_cangmen_up(OFF);
|
|
|
+ if(device_status.cul_time==7&&Read_Color_Limit(LIMIT_BOARD)==0)
|
|
|
+ {
|
|
|
+ LOG_W("第七天!!!");
|
|
|
+ ctr_cangmen_down(CLOSE);
|
|
|
+ rt_thread_delay(500);
|
|
|
+ }
|
|
|
+ LOG_W("先打开运行到限位的右侧!");
|
|
|
+ while(Read_Color_Limit(LIMIT_BOARD)==0)
|
|
|
+ {
|
|
|
+ LOG_W("归位:到达限位中间");
|
|
|
+ ctr_cangmen_down(CLOSE);
|
|
|
+ if(Read_Color_Limit(LIMIT_RESET)==0)
|
|
|
+ {
|
|
|
+ ctr_cangmen_down(OFF);
|
|
|
+ break;
|
|
|
+ }
|
|
|
}
|
|
|
- LOG_W("检测到IO0翻转为1!!!");
|
|
|
+ //先打开运行到限位的右侧
|
|
|
+ while(Read_Color_Limit(LIMIT_BOARD)==1)
|
|
|
+ {
|
|
|
+ LOG_W("归位:运行!");
|
|
|
+ ctr_cangmen_down(CLOSE);
|
|
|
+ if(Read_Color_Limit(LIMIT_RESET)==0)
|
|
|
+ {
|
|
|
+ ctr_cangmen_down(OFF);
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ while(Read_Color_Limit(LIMIT_BOARD)==0) //到达限位中间 继续运行
|
|
|
+ {
|
|
|
+ LOG_W("归位:到达限位中间");
|
|
|
+ ctr_cangmen_down(CLOSE);
|
|
|
+ if(Read_Color_Limit(LIMIT_RESET)==0)
|
|
|
+ {
|
|
|
+ ctr_cangmen_down(OFF);
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if(Read_Color_Limit(LIMIT_RESET)==0) return 0;
|
|
|
+ LOG_W("到达限位左侧!");
|
|
|
+ //停止电机
|
|
|
+ ctr_cangmen_down(OFF);
|
|
|
+ rt_thread_delay(2000);
|
|
|
+ //将推杆电机归位
|
|
|
+ LOG_D("*推杆电机归位*");
|
|
|
+ /*推杆电机伸出*/
|
|
|
+ ctr_cangmen_up(OPEN);
|
|
|
+ rt_thread_delay(10000);
|
|
|
+ ctr_cangmen_up(OFF);
|
|
|
+ if(device_status.cul_time>1)
|
|
|
+ device_status.cul_time--;
|
|
|
+ ef_set_env_blob("cul_time",&device_status.cul_time,sizeof(device_status.cul_time)); //采集时间
|
|
|
+ return 2;
|
|
|
}
|
|
|
- else {
|
|
|
- rt_pin_mode(LIMIT_OPEN, PIN_MODE_INPUT);
|
|
|
- while(rt_pin_read(LIMIT_OPEN)==1)
|
|
|
+ else if(forward==OPEN)
|
|
|
+ {
|
|
|
+ if(device_status.cul_time==7)
|
|
|
+ {
|
|
|
+ Beep_Tip(5000);
|
|
|
+ return 7;
|
|
|
+ }
|
|
|
+ LOG_W("打开推杆电机!");
|
|
|
+ if(Read_Color_Limit(LIMIT_BOARD)==0)
|
|
|
{
|
|
|
- /*持续的*/
|
|
|
+ LOG_D("*推杆电机归位*");
|
|
|
+ /*推杆电机归位*/
|
|
|
+ ctr_cangmen_up(CLOSE);
|
|
|
+ rt_thread_delay(10000);
|
|
|
+ ctr_cangmen_up(OFF);
|
|
|
+ }
|
|
|
+ while(Read_Color_Limit(LIMIT_BOARD)==0)
|
|
|
+ {
|
|
|
+ ctr_cangmen_down(forward);
|
|
|
rt_thread_delay(50);
|
|
|
}
|
|
|
- LOG_W("检测到IO1翻转为0!!!");
|
|
|
- while(rt_pin_read(LIMIT_OPEN)==0)
|
|
|
+ while(Read_Color_Limit(LIMIT_BOARD)==1)
|
|
|
{
|
|
|
+ time_board--;
|
|
|
+ if(time_board==0) break;
|
|
|
+ ctr_cangmen_down(forward);
|
|
|
rt_thread_delay(50);
|
|
|
}
|
|
|
- LOG_W("检测到IO1翻转为1!!!");
|
|
|
- }
|
|
|
+ ctr_cangmen_down(OFF);
|
|
|
+ rt_thread_delay(1000);
|
|
|
|
|
|
+ ctr_cangmen_up(OPEN);
|
|
|
+ rt_thread_delay(10000);
|
|
|
+ ctr_cangmen_up(OFF);
|
|
|
+ if(device_status.cul_time<7)
|
|
|
+ device_status.cul_time++;
|
|
|
+ ef_set_env_blob("cul_time",&device_status.cul_time,sizeof(device_status.cul_time)); //采集时间
|
|
|
+ return OPEN;
|
|
|
+ }
|
|
|
+ else if (forward==RESET) {
|
|
|
+ if(Read_Color_Limit(LIMIT_RESET)==0)
|
|
|
+ {
|
|
|
+ device_status.cul_time = 0;
|
|
|
+ //ef_set_env_blob("cul_time",&device_status.cul_time,sizeof(device_status.cul_time)); //采集时间
|
|
|
+ LOG_W("已在初始位置!");
|
|
|
+ Beep_Tip(1000);
|
|
|
+ ctr_cangmen_up(CLOSE);
|
|
|
+ rt_thread_delay(10000);
|
|
|
+ ctr_cangmen_up(OFF);
|
|
|
+ return 0;
|
|
|
+ //黄板已经在复位状态,直接返回0;
|
|
|
+ }
|
|
|
+ //if(Read_Color_Limit(LIMIT_BOARD)==0)
|
|
|
+ {
|
|
|
+ LOG_D("*推杆电机归位*");
|
|
|
+ /*推杆电机归位*/
|
|
|
+ ctr_cangmen_up(CLOSE);
|
|
|
+ rt_thread_delay(10000);
|
|
|
+ ctr_cangmen_up(OFF);
|
|
|
+ }
|
|
|
+ while(Read_Color_Limit(LIMIT_RESET)==1)
|
|
|
+ {
|
|
|
+ time_board--;
|
|
|
+ if(time_board==0) break;
|
|
|
+ ctr_cangmen_down(CLOSE);
|
|
|
+ rt_thread_delay(20);
|
|
|
+ }
|
|
|
+ ctr_cangmen_down(OFF);
|
|
|
+ }
|
|
|
+ device_status.cul_time = 0;
|
|
|
+ ef_set_env_blob("cul_time",&device_status.cul_time,sizeof(device_status.cul_time)); //采集时间
|
|
|
+ return RESET; //复位返回0
|
|
|
}
|
|
|
/**************************机器人********************************/
|
|
|
void ctr_Motor_Robot(uint8_t status_ws)
|
|
|
@@ -221,65 +374,29 @@ void ctr_JD_Clear(uint8_t shake_sw)
|
|
|
{
|
|
|
rt_uint16_t turn_timeout = 0;
|
|
|
rt_pin_mode(MOTOR_C_LIMIT,PIN_MODE_INPUT);
|
|
|
-
|
|
|
+ if(shake_sw==ON)
|
|
|
+ {
|
|
|
+ Motor_shake(ON, 0);
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ Motor_shake(OFF, 0);
|
|
|
+ }
|
|
|
while(rt_pin_read(MOTOR_C_LIMIT)==0)
|
|
|
- {
|
|
|
-
|
|
|
- rt_thread_delay(20);
|
|
|
- while(rt_pin_read(MOTOR_C_LIMIT)==0)
|
|
|
- {
|
|
|
-
|
|
|
- Motor_Juandai(ON);
|
|
|
- if(shake_sw==ON)
|
|
|
- {
|
|
|
- Motor_shake(ON, 1);
|
|
|
- }
|
|
|
- else {
|
|
|
- rt_thread_delay(50);
|
|
|
- }
|
|
|
- turn_timeout++;
|
|
|
- if(turn_timeout==1000) break;
|
|
|
- }
|
|
|
- break;
|
|
|
- }
|
|
|
- while(rt_pin_read(MOTOR_C_LIMIT)==1)
|
|
|
- {
|
|
|
- rt_thread_delay(20);
|
|
|
- while(rt_pin_read(MOTOR_C_LIMIT)==1)
|
|
|
- {
|
|
|
- if(shake_sw==ON)
|
|
|
- {
|
|
|
- Motor_shake(ON, 1);
|
|
|
- }
|
|
|
- else {
|
|
|
- rt_thread_delay(50);
|
|
|
- }
|
|
|
- Motor_Juandai(ON);
|
|
|
- turn_timeout++;
|
|
|
- if(turn_timeout==1000) break;
|
|
|
- }
|
|
|
- break;
|
|
|
- }
|
|
|
- while(rt_pin_read(MOTOR_C_LIMIT)==0)
|
|
|
- {
|
|
|
-
|
|
|
- if(shake_sw==ON)
|
|
|
- {
|
|
|
- Motor_shake(ON, 1);
|
|
|
- }
|
|
|
- else {
|
|
|
- rt_thread_delay(20);
|
|
|
- }
|
|
|
- if(rt_pin_read(MOTOR_C_LIMIT)==0)
|
|
|
- {
|
|
|
- Motor_Juandai(OFF);
|
|
|
- break;
|
|
|
- }
|
|
|
- else {
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- rt_pin_write(SHAKE_SW, OFF);
|
|
|
+ {
|
|
|
+ Motor_Juandai(ON);
|
|
|
+ rt_thread_delay(50);
|
|
|
+ turn_timeout++;
|
|
|
+ if(turn_timeout==1000) break;
|
|
|
+ }
|
|
|
+ turn_timeout = 0;
|
|
|
+ while(rt_pin_read(MOTOR_C_LIMIT)==1)
|
|
|
+ {
|
|
|
+ Motor_Juandai(ON);
|
|
|
+ turn_timeout++;
|
|
|
+ rt_thread_delay(50);
|
|
|
+ if(turn_timeout==1000) break;
|
|
|
+ }
|
|
|
+ Motor_shake(OFF, 1);
|
|
|
Motor_Juandai(OFF);
|
|
|
}
|
|
|
//工作任务流程
|
|
|
@@ -297,7 +414,7 @@ void WorkTask_XCT(void)
|
|
|
{
|
|
|
photo_fre --;
|
|
|
rt_thread_delay(1000);
|
|
|
- rt_kprintf("收集中,等待开始拍照:%d\n",photo_fre);
|
|
|
+ LOG_I("收集中,等待开始拍照:%d",photo_fre);
|
|
|
}
|
|
|
//卷带
|
|
|
ctr_JD_Clear(OFF);
|
|
|
@@ -311,12 +428,12 @@ void WorkTask_XCT(void)
|
|
|
//卷带
|
|
|
ctr_JD_Clear(ON);
|
|
|
//震动
|
|
|
- Motor_shake(ON, device_status.shake_sec*20);
|
|
|
+ Motor_shake(ON, device_status.shake_sec);
|
|
|
}
|
|
|
//关闭风机
|
|
|
ctr_Motor_xichong(OFF);
|
|
|
ctr_Motor_xifu(OFF);
|
|
|
- rt_kprintf("不在工作时间,未工作!!!\n");
|
|
|
+ LOG_I("不在工作时间,未工作!!!");
|
|
|
}
|
|
|
/**********************************************************************/
|
|
|
/*光控*/
|
|
|
@@ -1292,17 +1409,27 @@ void HardWare_Init(uint8_t boot)
|
|
|
void HK_TakePhoto(void)
|
|
|
{
|
|
|
//打开补光灯
|
|
|
- ctr_led(ON);
|
|
|
DTU_Write("takephoto.mp3");
|
|
|
- //延时
|
|
|
- rt_thread_delay(1000*15);
|
|
|
- rt_kprintf("\n测试拍照!\n");
|
|
|
+ rt_thread_delay(1000);
|
|
|
char *takephotojson = makeJson_takephoto();
|
|
|
DTU_Write("%s",takephotojson);
|
|
|
rt_free(takephotojson);
|
|
|
+ //吸虫塔
|
|
|
+ if(device_status.version_type!=6)
|
|
|
+ {
|
|
|
+ ctr_led(ON);
|
|
|
+ rt_thread_delay(1000*15);
|
|
|
+ }
|
|
|
//触发海康相机拍照
|
|
|
Hk_Take_io();
|
|
|
- rt_thread_delay(1000*45);
|
|
|
+ if(device_status.version_type!=6)
|
|
|
+ {
|
|
|
+ rt_thread_delay(1000*45);
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ ctr_led(ON);
|
|
|
+ rt_thread_delay(1000*19);
|
|
|
+ }
|
|
|
ctr_led(OFF);
|
|
|
}
|
|
|
//光控测试任务
|
|
|
@@ -1621,13 +1748,6 @@ void WorkCheck(void)
|
|
|
}
|
|
|
void WorkTask_BZY(void)
|
|
|
{
|
|
|
- rt_uint8_t wait_cnt = 0;
|
|
|
- while(1)
|
|
|
- {
|
|
|
- wait_cnt++;
|
|
|
- if(wait_cnt==30) break;
|
|
|
- rt_thread_delay(1000);
|
|
|
- }
|
|
|
if(device_status.ds==1)
|
|
|
{
|
|
|
//时间段判断
|
|
|
@@ -2101,6 +2221,7 @@ static void Work_Loop_thread_entry(void *parameter)
|
|
|
/*入口函数清扫*/
|
|
|
static void Control_Clear_thread_entry(void *parameter)
|
|
|
{
|
|
|
+
|
|
|
while(1)
|
|
|
{
|
|
|
#if ROBOT_EN ==1
|
|
|
@@ -2108,7 +2229,12 @@ static void Control_Clear_thread_entry(void *parameter)
|
|
|
rt_thread_delay(100);
|
|
|
#else
|
|
|
#if Color_EN ==1
|
|
|
- ctr_Color_Key(0);
|
|
|
+ /*复位*/
|
|
|
+ if(Read_Color_Key(KEY_RESET)==1)
|
|
|
+ {
|
|
|
+ ctr_Color_board(RESET, 800);
|
|
|
+ rt_thread_delay(5000);
|
|
|
+ }
|
|
|
#else
|
|
|
if(device_status.clear_status==1)
|
|
|
{
|
|
|
@@ -2127,11 +2253,13 @@ static void Control_Heat_thread_entry(void *parameter)
|
|
|
#if ROBOT_EN == 1
|
|
|
static uint8_t key_on = 0;
|
|
|
#else
|
|
|
+ #if Color_EN == 0
|
|
|
while(device_status.work_status==0)
|
|
|
{
|
|
|
rt_thread_delay(1000);
|
|
|
}
|
|
|
#endif
|
|
|
+ #endif
|
|
|
while(1)
|
|
|
{
|
|
|
#if ROBOT_EN == 1
|
|
|
@@ -2170,6 +2298,15 @@ static void Control_Heat_thread_entry(void *parameter)
|
|
|
rt_pin_write(EMS_Dir, 1);
|
|
|
}
|
|
|
#else
|
|
|
+ #if Color_EN == 1
|
|
|
+ /*黄板归位*/
|
|
|
+ if(Read_Color_Key(KEY_BACK)==1)
|
|
|
+ {
|
|
|
+ LOG_W("黄板归位!");
|
|
|
+ ctr_Color_board(CLOSE, 300);
|
|
|
+ rt_thread_delay(5000);
|
|
|
+ }
|
|
|
+ #else
|
|
|
if(device_status.work_status==1&&device_status.heat_sw==1)
|
|
|
{
|
|
|
ctr_lamp(ON, 1);
|
|
|
@@ -2180,8 +2317,9 @@ static void Control_Heat_thread_entry(void *parameter)
|
|
|
{
|
|
|
LOG_I("Not calorstat!");
|
|
|
}
|
|
|
- #endif
|
|
|
rt_thread_delay(1000);
|
|
|
+ #endif
|
|
|
+ #endif
|
|
|
}
|
|
|
}
|
|
|
/*清扫线程*/
|
|
|
@@ -2239,10 +2377,12 @@ int Work_Loop_Task(void)
|
|
|
WORK_LOOP_THREAD_STACK_SIZE, //2048
|
|
|
WORK_LOOP_THREAD_PRIORITY, //12
|
|
|
WORK_LOOP_THREAD_TIMESLICE); //5
|
|
|
- if(tid_workloop!=RT_NULL&&Color_EN==0)
|
|
|
+ #if Color_EN == 0
|
|
|
+ if(tid_workloop!=RT_NULL)
|
|
|
{
|
|
|
rt_thread_startup(tid_workloop);
|
|
|
}
|
|
|
+ #endif
|
|
|
return 0;
|
|
|
}
|
|
|
MSH_CMD_EXPORT(HardWare_Check,HardWare_Check<ON|OFF>);
|