|
|
@@ -68,235 +68,10 @@ uint8_t ctr_Fan_IN(uint8_t status_ws)
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
|
-/****************************5.0***********************************/
|
|
|
-void ctr_turnv5(void)
|
|
|
-{
|
|
|
- rt_pin_mode(CLEAR_R, PIN_MODE_OUTPUT);
|
|
|
- rt_pin_mode(CLEAR_L, PIN_MODE_OUTPUT);
|
|
|
- // 转出
|
|
|
- rt_pin_write(CLEAR_R,ON);
|
|
|
- rt_pin_write(CLEAR_L,OFF);
|
|
|
- rt_thread_mdelay(300);
|
|
|
- //停止
|
|
|
- rt_pin_write(CLEAR_R,OFF);
|
|
|
- rt_pin_write(CLEAR_L,OFF);
|
|
|
- rt_thread_mdelay(300);
|
|
|
- //转入
|
|
|
- rt_pin_write(CLEAR_R,OFF);
|
|
|
- rt_pin_write(CLEAR_L,ON);
|
|
|
- rt_thread_mdelay(300);
|
|
|
- //停止
|
|
|
- rt_pin_write(CLEAR_R,OFF);
|
|
|
- rt_pin_write(CLEAR_L,OFF);
|
|
|
- rt_thread_mdelay(300);
|
|
|
-}
|
|
|
-/**************************机器人********************************/
|
|
|
-void ctr_Motor_Robot(uint8_t status_ws)
|
|
|
-{
|
|
|
- unsigned int close_timeout = 0;
|
|
|
- unsigned int open_timeout = 0;
|
|
|
- rt_pin_mode(LIMIT_OPEN, PIN_MODE_INPUT);
|
|
|
- rt_pin_mode(LIMIT_CLOSE, PIN_MODE_INPUT);
|
|
|
- rt_pin_mode(Motor_Power, PIN_MODE_OUTPUT);
|
|
|
- rt_pin_mode(Motor_Direction_Button, PIN_MODE_INPUT);
|
|
|
- rt_pin_mode(Motor_Direction_Ctr, PIN_MODE_OUTPUT);
|
|
|
- rt_pin_write(Motor_Power,OFF);
|
|
|
- rt_pin_write(Motor_Direction_Ctr,ON);
|
|
|
- rt_pin_mode(CLEAR_R, PIN_MODE_OUTPUT);
|
|
|
- rt_pin_mode(CLEAR_L, PIN_MODE_OUTPUT);
|
|
|
- if(status_ws == ON)
|
|
|
- {
|
|
|
- //检测按键状态
|
|
|
- while(rt_pin_read(Motor_Direction_Button)==1)
|
|
|
- {
|
|
|
- if(rt_pin_read(LIMIT_CLOSE)==1)
|
|
|
- {
|
|
|
- rt_pin_write(Motor_Direction_Ctr,ON);
|
|
|
- close_timeout++;
|
|
|
- //打开电机驱动器电源,默认关闭
|
|
|
- rt_pin_write(Motor_Power,ON);
|
|
|
- if(close_timeout==2)
|
|
|
- {
|
|
|
- rt_pin_write(CLEAR_R,ON);
|
|
|
- rt_pin_write(CLEAR_L,OFF);
|
|
|
- rt_pin_write(Motor_Power,OFF);
|
|
|
- rt_thread_delay(20);
|
|
|
- rt_pin_write(Motor_Power,ON);
|
|
|
- }
|
|
|
- rt_thread_delay(20);
|
|
|
- if(close_timeout>500)
|
|
|
- {
|
|
|
- close_timeout = 600;
|
|
|
- rt_pin_write(Motor_Power,OFF);
|
|
|
- rt_pin_write(CLEAR_R,OFF);
|
|
|
- rt_pin_write(CLEAR_L,OFF);
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- close_timeout = 0;
|
|
|
- rt_pin_write(Motor_Power,OFF); //关闭驱动器电源
|
|
|
- rt_pin_write(CLEAR_R,OFF);
|
|
|
- rt_pin_write(CLEAR_L,OFF);
|
|
|
- }
|
|
|
- rt_thread_delay(20);
|
|
|
- }
|
|
|
- rt_pin_write(Motor_Power,OFF);
|
|
|
- rt_thread_delay(1000*2);
|
|
|
- while(rt_pin_read(Motor_Direction_Button)==0)
|
|
|
- {
|
|
|
- if(rt_pin_read(LIMIT_OPEN)==1)
|
|
|
- {
|
|
|
- open_timeout ++;
|
|
|
- //打开电机驱动器电源,默认关闭
|
|
|
- if(open_timeout==2){
|
|
|
- rt_pin_write(CLEAR_R,OFF);
|
|
|
- rt_pin_write(CLEAR_L,ON);
|
|
|
- }
|
|
|
- rt_pin_write(Motor_Power,ON);
|
|
|
- rt_pin_write(Motor_Direction_Ctr,OFF);
|
|
|
- if(open_timeout>500)
|
|
|
- {
|
|
|
- open_timeout = 600;
|
|
|
- rt_pin_write(Motor_Power,OFF);
|
|
|
- rt_pin_write(CLEAR_R,OFF);
|
|
|
- rt_pin_write(CLEAR_L,OFF);
|
|
|
- }
|
|
|
- rt_thread_delay(20);
|
|
|
- }
|
|
|
- if(rt_pin_read(LIMIT_OPEN)==0)
|
|
|
- {
|
|
|
- open_timeout = 0;
|
|
|
- rt_pin_write(Motor_Power,OFF);
|
|
|
- rt_pin_write(CLEAR_R,OFF);
|
|
|
- rt_pin_write(CLEAR_L,OFF);
|
|
|
- }
|
|
|
|
|
|
- rt_thread_delay(20);
|
|
|
- close_timeout = 0;
|
|
|
- }
|
|
|
- open_timeout = 0;
|
|
|
- close_timeout = 0;
|
|
|
- }
|
|
|
- else {
|
|
|
- rt_pin_write(Motor_Power,OFF);
|
|
|
- }
|
|
|
-}
|
|
|
/*************************吸虫塔*********************************/
|
|
|
-//吸虫电机
|
|
|
-uint8_t ctr_Motor_xichong(uint8_t status_ws)
|
|
|
-{
|
|
|
- rt_pin_mode(MOTOR1_CTR_PIN, PIN_MODE_OUTPUT);
|
|
|
- if(status_ws==ON)
|
|
|
- {
|
|
|
- rt_pin_write(MOTOR1_CTR_PIN,ON);
|
|
|
- }
|
|
|
- else {
|
|
|
- rt_pin_write(MOTOR1_CTR_PIN, OFF);
|
|
|
- }
|
|
|
- return 0;
|
|
|
-}
|
|
|
|
|
|
-uint8_t ctr_Camera_Power(uint8_t status_ws)
|
|
|
-{
|
|
|
- rt_pin_mode(MOTOR1_CTR_PIN, PIN_MODE_OUTPUT);
|
|
|
- if(status_ws==ON)
|
|
|
- {
|
|
|
- rt_pin_write(MOTOR1_CTR_PIN,ON);
|
|
|
- }
|
|
|
- else {
|
|
|
- rt_pin_write(MOTOR1_CTR_PIN, OFF);
|
|
|
- }
|
|
|
- return 0;
|
|
|
-}
|
|
|
-//吸附电机
|
|
|
-uint8_t ctr_Motor_xifu(uint8_t status_ws)
|
|
|
-{
|
|
|
- rt_pin_mode(MOTOR2_CTR_PIN, PIN_MODE_OUTPUT);
|
|
|
- if(status_ws==ON)
|
|
|
- {
|
|
|
- rt_pin_write(MOTOR2_CTR_PIN,ON);
|
|
|
- }
|
|
|
- else {
|
|
|
- rt_pin_write(MOTOR2_CTR_PIN, OFF);
|
|
|
- }
|
|
|
- return 0;
|
|
|
-}
|
|
|
-//清扫-卷带电机
|
|
|
-void ctr_JD_Clear(uint8_t shake_sw)
|
|
|
-{
|
|
|
- rt_uint16_t turn_timeout = 0;
|
|
|
- rt_pin_mode(MOTOR_C_LIMIT,PIN_MODE_INPUT);
|
|
|
- ctr_cangmen_up(OFF); //清扫传送带的电机
|
|
|
- if(shake_sw==ON)
|
|
|
- {
|
|
|
- ctr_Heat(ON); //清洁风机
|
|
|
- Motor_shake(ON, 0);
|
|
|
- ctr_cangmen_up(OPEN); //打开清扫
|
|
|
- }
|
|
|
- else {
|
|
|
- Motor_shake(OFF, 0);
|
|
|
- ctr_cangmen_up(OPEN); //打开清扫
|
|
|
- }
|
|
|
- while(rt_pin_read(MOTOR_C_LIMIT)==0)
|
|
|
- {
|
|
|
- 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);
|
|
|
- ctr_cangmen_up(OFF); //关闭清扫
|
|
|
- ctr_Heat(OFF);
|
|
|
-}
|
|
|
-//工作任务流程
|
|
|
-void WorkTask_XCT(void)
|
|
|
-{
|
|
|
- while(device_status.work_status==1&&device_status.ds==1)
|
|
|
- {
|
|
|
|
|
|
- ctr_Motor_xifu(ON);
|
|
|
- //打开相机供电
|
|
|
- ctr_Camera_Power(ON);
|
|
|
- //ctr_Heat(ON); //清洁风机
|
|
|
- uint32_t photo_fre = 0;
|
|
|
- photo_fre = device_status.dattim * 60;
|
|
|
- while(photo_fre!=0)
|
|
|
- {
|
|
|
- photo_fre --;
|
|
|
- rt_thread_delay(1000);
|
|
|
- LOG_I("收集中,等待开始拍照:%d",photo_fre);
|
|
|
- }
|
|
|
- //卷带
|
|
|
- ctr_JD_Clear(OFF);
|
|
|
- //拍照
|
|
|
- HK_TakePhoto();
|
|
|
- //关闭吸附风机
|
|
|
- ctr_Motor_xifu(OFF);
|
|
|
- //关闭相机供电
|
|
|
- //ctr_Heat(OFF);
|
|
|
- ctr_Camera_Power(OFF);
|
|
|
- //卷带
|
|
|
- ctr_JD_Clear(ON);
|
|
|
-// rt_thread_delay(1000);
|
|
|
-// ctr_JD_Clear(ON);
|
|
|
- //震动
|
|
|
- Motor_shake(ON, device_status.shake_sec);
|
|
|
- }
|
|
|
- //关闭风机
|
|
|
- ctr_Motor_xichong(OFF);
|
|
|
- ctr_Motor_xifu(OFF);
|
|
|
- LOG_I("不在工作时间,未工作!!!");
|
|
|
-}
|
|
|
/**********************************************************************/
|
|
|
/*光控*/
|
|
|
uint8_t In_Light_status(void)
|
|
|
@@ -2122,22 +1897,6 @@ void WorkTask(void)
|
|
|
//ctr_shake(ON, 100);
|
|
|
}
|
|
|
}
|
|
|
- else {
|
|
|
- //打开高压
|
|
|
- ctr_Heat(ON);
|
|
|
- //打开补光
|
|
|
- ctr_led(ON);
|
|
|
- LOG_I("打开补光!!");
|
|
|
- rt_pin_mode(CLEAR_L_LIMIT,PIN_MODE_INPUT);
|
|
|
- //2.翻转清虫
|
|
|
- while(rt_pin_read(CLEAR_L_LIMIT)==1) rt_thread_delay(20);
|
|
|
- rt_thread_delay(500);
|
|
|
- Hk_Take_io(); //触发海康相机拍照
|
|
|
- rt_thread_delay(500);
|
|
|
- ctr_turnv5(); //摆动清虫
|
|
|
- LOG_I("摆动清虫!");
|
|
|
-
|
|
|
- }
|
|
|
}
|
|
|
if(device_status.bat_status == 1 || device_status.temp_status == 1 || device_status.rain_status==1 || device_status.ds==0)
|
|
|
{
|
|
|
@@ -2335,40 +2094,10 @@ static void Work_Loop_thread_entry(void *parameter)
|
|
|
{
|
|
|
WorkTask();
|
|
|
}
|
|
|
- if(device_status.version_type==6)
|
|
|
- {
|
|
|
- WorkTask_XCT();
|
|
|
- }
|
|
|
rt_thread_delay(1000);
|
|
|
}
|
|
|
}
|
|
|
-/*入口函数清扫*/
|
|
|
-static void Control_Clear_thread_entry(void *parameter)
|
|
|
-{
|
|
|
- while(1)
|
|
|
- {
|
|
|
- #if ROBOT_EN ==1
|
|
|
- ctr_Motor_Robot(ON);
|
|
|
- rt_thread_delay(100);
|
|
|
- #else
|
|
|
- #if Color_EN ==1
|
|
|
- /*复位*/
|
|
|
- if(Read_Color_Key(KEY_RESET)==1)
|
|
|
- {
|
|
|
- ctr_Color_board(RESET, 1200);
|
|
|
- rt_thread_delay(5000);
|
|
|
- }
|
|
|
- #else
|
|
|
- if(device_status.clear_status==1)
|
|
|
- {
|
|
|
- if(ctr_clear(1200)==0)
|
|
|
- ctr_TG_Clear(400);
|
|
|
- }
|
|
|
- rt_thread_delay(100);
|
|
|
- #endif
|
|
|
- #endif
|
|
|
- }
|
|
|
-}
|
|
|
+
|
|
|
|
|
|
void slidelock();
|
|
|
//滑台垂直方向开关
|
|
|
@@ -2410,12 +2139,11 @@ uint32_t ctr_move(uint8_t DIR_,uint8_t dir_,uint32_t dis_,uint8_t pulse)
|
|
|
}
|
|
|
|
|
|
if(dis_ >=1000){
|
|
|
- beep_set(500000, 250000); //PWM使能
|
|
|
+ beep_set(500000, 150000); //PWM使能
|
|
|
}else{
|
|
|
beep_set(2000000, 1000000); //PWM使能
|
|
|
}
|
|
|
|
|
|
-
|
|
|
switch(DIR_){
|
|
|
case X_:
|
|
|
//判断限位
|
|
|
@@ -2427,7 +2155,7 @@ uint32_t ctr_move(uint8_t DIR_,uint8_t dir_,uint32_t dis_,uint8_t pulse)
|
|
|
}
|
|
|
|
|
|
ctr_lamp(1, ON); //X轴电源开关:开
|
|
|
- rt_thread_delay(100);
|
|
|
+ rt_thread_delay(10);
|
|
|
|
|
|
for(j=0;j<dis_;j++){
|
|
|
if(slide_para.stop_flag == 0){
|
|
|
@@ -2450,23 +2178,27 @@ uint32_t ctr_move(uint8_t DIR_,uint8_t dir_,uint32_t dis_,uint8_t pulse)
|
|
|
|
|
|
break;
|
|
|
case Y_:
|
|
|
+ ctr_Collect_Fan(ON); //Y轴电源开关:开
|
|
|
+ rt_thread_delay(10);
|
|
|
//判断限位
|
|
|
- if(dir_== 0 && rt_pin_read(Limit_Motor_Y)==0){
|
|
|
- for(int z =0;z < 3; z++){
|
|
|
+ if(dir_== 1 && rt_pin_read(Limit_Motor_Y)==1)
|
|
|
+ {
|
|
|
+ for(int z =0;z < 3; z++)
|
|
|
+ {
|
|
|
Beep_Tip(200);
|
|
|
}
|
|
|
+ ctr_Collect_Fan(OFF); //Y轴电源开关:关
|
|
|
return 0;
|
|
|
}
|
|
|
- ctr_Collect_Fan(ON); //Y轴电源开关:开
|
|
|
- rt_thread_delay(100);
|
|
|
-
|
|
|
- for(j=0;j<dis_;j++){
|
|
|
- if(slide_para.stop_flag == 0){
|
|
|
+ for(j=0;j<dis_;j++)
|
|
|
+ {
|
|
|
+ if(slide_para.stop_flag == 0)
|
|
|
+ {
|
|
|
rt_thread_delay(pulse);
|
|
|
- if(dir_==0&&rt_pin_read(Limit_Motor_Y)==0){
|
|
|
+ if(dir_==1&&rt_pin_read(Limit_Motor_Y)==1){
|
|
|
rt_thread_delay(10);
|
|
|
- if(rt_pin_read(Limit_Motor_Y)==0){
|
|
|
- j = dis_;
|
|
|
+ if(rt_pin_read(Limit_Motor_Y)==1){
|
|
|
+ break;
|
|
|
}
|
|
|
}
|
|
|
}else{ //急停
|
|
|
@@ -2479,7 +2211,8 @@ uint32_t ctr_move(uint8_t DIR_,uint8_t dir_,uint32_t dis_,uint8_t pulse)
|
|
|
|
|
|
break;
|
|
|
case Z_:
|
|
|
-
|
|
|
+ ctr_swz(ON); //X轴电源开关:开
|
|
|
+ rt_thread_delay(100); //
|
|
|
//判断限位
|
|
|
if(dir_== 1 && rt_pin_read(Limit_Motor_Z)==0){
|
|
|
for(int z =0;z < 3; z++){
|
|
|
@@ -2488,9 +2221,6 @@ uint32_t ctr_move(uint8_t DIR_,uint8_t dir_,uint32_t dis_,uint8_t pulse)
|
|
|
return 0;
|
|
|
}
|
|
|
|
|
|
- ctr_swz(ON); //X轴电源开关:开
|
|
|
- rt_thread_delay(100); //
|
|
|
-
|
|
|
for(j=0;j<dis_;j++){
|
|
|
if(slide_para.stop_flag == 0){
|
|
|
rt_thread_delay(pulse);
|
|
|
@@ -2525,7 +2255,7 @@ uint32_t ctr_move(uint8_t DIR_,uint8_t dir_,uint32_t dis_,uint8_t pulse)
|
|
|
ctr_Collect_Fan(ON); //Y轴开关:开
|
|
|
break;
|
|
|
case Z_:
|
|
|
- ctr_swz(ON); //Y轴开关:关
|
|
|
+ ctr_swz(ON); //Z轴开关:开
|
|
|
ctr_lamp(OFF,1); //X轴开关:关
|
|
|
ctr_Collect_Fan(OFF); //Y轴开关:关
|
|
|
break;
|
|
|
@@ -2684,7 +2414,7 @@ void slide_thread_entry(void *parameter)
|
|
|
ctr_move(Y_,slide_para.ydir,slide_para.ydis,slide_para.pulse_period);
|
|
|
rt_kprintf("The slide's y movement was done ok\n");
|
|
|
rt_thread_delay(1000);
|
|
|
- if(slide_para.ydis==2||slide_para.ydis==50){
|
|
|
+ if(slide_para.ydis==4||slide_para.ydis==100){
|
|
|
char *photo = makeJson_Warn("movy","done");
|
|
|
LOG_I("%s",photo);
|
|
|
DTU_Write("%s",photo);
|
|
|
@@ -2697,7 +2427,7 @@ void slide_thread_entry(void *parameter)
|
|
|
break;
|
|
|
}
|
|
|
case 7:{ //Y轴复位
|
|
|
- slide_para.ydir = 0;
|
|
|
+ slide_para.ydir = 1;
|
|
|
slide_para.ydis = 100000;
|
|
|
ctr_move(Y_,slide_para.ydir,slide_para.ydis,slide_para.pulse_period);
|
|
|
rt_kprintf("The slide's movement of back to y_original point was done ok\n");
|
|
|
@@ -2716,21 +2446,7 @@ void slide_thread_entry(void *parameter)
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
-/*清扫线程*/
|
|
|
-int Control_clear(void)
|
|
|
-{
|
|
|
- /* 创建线程*/
|
|
|
- tid_ctr = rt_thread_create("Control_Clear_thread_entry",
|
|
|
- Control_Clear_thread_entry, RT_NULL,
|
|
|
- 2048, //2048
|
|
|
- 24, //23
|
|
|
- WORK_CHECK_THREAD_TIMESLICE); //5
|
|
|
- if(tid_ctr!=RT_NULL)
|
|
|
- {
|
|
|
- rt_thread_startup(tid_ctr);
|
|
|
- }
|
|
|
- return 0;
|
|
|
-}
|
|
|
+
|
|
|
|
|
|
/* 创建线程 -工作状态检测*/
|
|
|
int Workstatus_check(void)
|