mini7_V2/DriverInterface/usr_motor.c

/*
 * usr_gpio.c
 *
 *	Created on: 2022年4月11日
 *		Author: amos
 */
#include "usr_motor.h"

#define BOUNCE_THRESHOLD 2 // 800ms长按阈值
#define LONG_PRESS_THRESHOLD 80 // 800ms长按阈值
#define CONTINUOUS_INTERVAL 40 // 300ms内算连续按键
#define USER_STOP_TIMER 200 // 长按2s不放，停止混匀

#define FIND_OPTIC_SPEED 50 // 找光开速度
#define FIND_OPTIC_BACK_SPEED 43 // 过光开回退速度
#define FIND_OPTIC_BACK_GAP 2 // 过光开回退速度

#define MAX_RUNCNT 4 // 找光开速度

typedef enum swing_state {
  SWING_IDLE,
  SWING_START,
  SWING_LOOP,
  SWING_END,
  LOOP_START,
  LOOP_END,
  EMERGENCY_END,
  FIND_GAPNUM,
  FIND_HOMEGAP,
  SWING_ERR,
} swing_state_def;

typedef enum {
  KEY_IDLE,     // 空闲状态
  KEY_SHORT,    // 短按
  KEY_CONTINUE, // 连续按
  KEY_LONG,     // 空闲状态，长按抬起，复位设备
  KEY_END       //混匀状态，长按不释放，停止混匀
} KeyState;

volatile

    struct {
  KeyState state;
  uint32_t press_tick;        // 按下时刻（10ms单位）
  uint32_t release_tick;      // 当前释放时刻
  uint32_t last_release_tick; // 新增：上一次释放时刻
  uint32_t click_count;       // 连续按键计数
  uint32_t last_level;
  uint32_t key_valid_flag; // 连续按键标志
} key_ctrl;

swing_state_def sw_st, last_st;
unsigned int keyState = 0;
unsigned int MotorOptState = 0;

extern uint32_t conter100ms;
uint32_t conter10ms;

int32_t runLoopTime = 600;
int32_t runLoopSpeed = 80;

uint32_t curtimeinner;
uint32_t timeinner = 6000;
uint32_t swingtime = 100;
uint32_t swingtimeRight = 40;
uint32_t swingtimePause = 100;
int32_t backOptFlag = 0;
int32_t startLoopConter = 0;
int32_t swingspeed = 60;
int32_t stateFistin;
int32_t REVORFWD;
//extern uint8_t error_set(uint8_t error);
uint32_t timerBuf[16];
uint32_t timerconter;
uint32_t intoGap;
uint32_t OutGap;
uint32_t lastGap;
uint32_t judgeGap;
uint32_t curGap;
uint32_t checkPWM;
MOTOR_DIR findhomedir;
uint32_t intoGapFlag;
uint32_t resetFromSwingFlag;

uint32_t swingLoopHigtTime = 0;
uint32_t swingHigStopflage = 0;
uint32_t swingStoptime = 0;
uint32_t startFindHometime = 0;
uint32_t findHome = 0;



extern void DelayMS(uint32_t ms);


//冒泡法排序，排序结果有小到大
void Bubble_Sort(unsigned int *Array,unsigned int n)
{ 
	unsigned char i, j;
	unsigned int temp;
	for(i=0;i<n-1;i++)
	{
		for(j=i+1;j<n;j++)
		{
			if(Array[i] > Array[j])
			{
				temp     = Array[i];
				Array[i] = Array[j];
				Array[j] = temp;
			}
		}
	}
 }


uint32_t Read_Motor(void) {
  return (HAL_GPIO_ReadPin(MINI7_V2_REF_SW2_GPIO_Port,MINI7_V2_REF_SW2_Pin)) ? 1 : 0; // 检查PF5引脚值
}


uint32_t Read_key(void) {
  return (HAL_GPIO_ReadPin(MINI7_V2_KEY_GPIO_Port,MINI7_V2_KEY_Pin)) ? 1 : 0; // 检查PF5引脚值
}


uint32_t TIMER_GetCounter(void) {
  return htim3.Instance->CNT;
}

/**
 * @brief	   SetMotorPwm
 *
 * @param		value between 0-100
 *
 * @return		None
 *
 * @details
 */
void SetMotorPwm(int dir, int32_t value) {
  /* Start PWM counter */
  unsigned int DutyCycle;
  if (value > 100) {
    value = 100;
  } else if (value < 0) {
    value = 0;
    DutyCycle = 0;
  } 
	DutyCycle = value*10;

  if (dir) {    
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, 0);
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, DutyCycle);
  } else {
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, DutyCycle);
		__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_3, 0);
  }
}

/**
 * @brief		GPIO PA/PB/PG/PH IRQ
 *
 * @param		None
 *
 * @return		None
 *
 * @details 	The PA/PB/PG/PH default IRQ, declared in startup_M031Series.s.
 */
void KYE_IRQHandler(void) {
  volatile uint32_t temp;
	conter10ms = sysinf.conter10ms;

  /* To check if PB.2 interrupt occurred */
   {
    uint8_t curr_level = Read_key();

    if (curr_level != key_ctrl.last_level) {
      key_ctrl.last_level = curr_level;

      if (curr_level == 0) { // 下降沿（按下）
        key_ctrl.press_tick = conter10ms;
        if (key_ctrl.state == KEY_SHORT) {
          key_ctrl.last_release_tick =
              key_ctrl.release_tick; // 更新上次释放时间
        } else {
          key_ctrl.last_release_tick = 0;
        }

      } else { // 上升沿（释放）
        key_ctrl.release_tick = conter10ms;
        uint32_t press_duration = 0;
				if(key_ctrl.press_tick){
					press_duration = key_ctrl.release_tick - key_ctrl.press_tick;
				}
		
        key_ctrl.press_tick = 0;
        // 长按判定
        if (press_duration <= BOUNCE_THRESHOLD) {
          key_ctrl.state = KEY_IDLE;
          key_ctrl.last_release_tick = 0;
        } else if (press_duration >= LONG_PRESS_THRESHOLD) {
          key_ctrl.state = KEY_LONG;
          key_ctrl.last_release_tick = 0; // 更新上次释放时间
          key_ctrl.key_valid_flag = 1;
        }

        // 短按判定
        else {
          key_ctrl.click_count = 1;
          key_ctrl.state = KEY_SHORT;
          if ((key_ctrl.release_tick - key_ctrl.last_release_tick) <=
              CONTINUOUS_INTERVAL) {
            key_ctrl.key_valid_flag = 1;
            key_ctrl.click_count++;
            key_ctrl.state = KEY_CONTINUE;
          }
        }
      }
    }
  }
}

void check_keyShortValid(void) {
  volatile uint32_t temp;

  /* To check if PB.2 interrupt occurred */
  if (key_ctrl.state == KEY_SHORT) {
    if ((conter10ms - key_ctrl.release_tick) > CONTINUOUS_INTERVAL * 2) {
      key_ctrl.key_valid_flag = 1;
    }
  }
  if (sw_st != SWING_IDLE && key_ctrl.press_tick) {
    if ((conter10ms - key_ctrl.press_tick) > USER_STOP_TIMER) {
      key_ctrl.state = KEY_END;
      key_ctrl.key_valid_flag = 1;
	  key_ctrl.press_tick = 0;
    }
  }
}

/**
 * @brief		GPIO PC/PD/PE/PF IRQ
 *
 * @param		None
 *
 * @return		None
 *
 * @details 	The PC/PD/PE/PF default IRQ, declared in startup_M031Series.s.
 */
void OPTO_IRQHandler(void) {
  //
  // volatile unsigned int temp;

  /* To check if PB.2 interrupt occurred */
  {
	conter10ms = sysinf.conter10ms;
	switch(sw_st){
		case FIND_GAPNUM:{
			if (Read_Motor()){
					intoGap = TIMER_GetCounter();		
				}else{
					OutGap = TIMER_GetCounter();
					if((conter10ms - startFindHometime) > MOTOR_LOOP_TIM){
						if(MAX_RUNCNT > timerconter){			
							if(OutGap > intoGap){
								timerBuf[timerconter++] = OutGap - intoGap;
							}else{
								timerBuf[timerconter++] = OutGap + 0x10000 - intoGap;
							}
							if(MAX_RUNCNT == timerconter){
								Bubble_Sort(timerBuf,MAX_RUNCNT);
								judgeGap = 0;
								if(timerBuf[0]*1.25<timerBuf[1]){
									judgeGap = timerBuf[0]+timerBuf[0];
								}else{
									judgeGap = timerBuf[1]+timerBuf[0];
								}
	//							for(unsigned int i =0;i<(MAX_RUNCNT/2);i++){
	//								judgeGap += timerBuf[i];
	//							}
								judgeGap =judgeGap*0.625;
								timerBuf[timerconter++] = judgeGap ;
								/*判决门限为最低两个值均值的1.5倍*/
							}
						}else{
							if(OutGap > intoGap){
								curGap = OutGap - intoGap;
							}else{
								curGap = OutGap + 0x10000 - intoGap;
							}
							timerBuf[timerconter++] = curGap ;							
							if(curGap > judgeGap){						
							   sw_st = FIND_HOMEGAP;	
							   SetMotorPwm(MOTOR_REV, 0);
							   SetMotorPwm(MOTOR_FWD, 0);	
							   stateFistin = 1;
							   checkPWM = checkPWM*0.8; /*降速便于复位*/		
							   findhomedir = !findhomedir;
							}
							if(timerconter==sizeof(timerBuf)){
							   SetMotorPwm(MOTOR_REV, 0);
							   SetMotorPwm(MOTOR_FWD, 0);	
							   timerconter = 0;
							}
						}

					}
					
				}
		}
		break;
		case FIND_HOMEGAP:{
		/*找远点阶段，光开亮停止*/
		if (Read_Motor()){			
			SetMotorPwm(MOTOR_REV, 0);
			SetMotorPwm(MOTOR_FWD, 0);	
		    intoGapFlag = 1;
			intoGap = TIMER_GetCounter();
			}
		}
		break;
		case SWING_LOOP:{
		/*找远点阶段，光开亮停止*/		
		if(startLoopConter > 0){
			startLoopConter --;
		}else{
			if (Read_Motor()){			
				SetMotorPwm(MOTOR_REV, 0);
				SetMotorPwm(MOTOR_FWD, 0);	
				intoGapFlag = 1;
				intoGap = TIMER_GetCounter();
			}
		}
		}break;
		default:{	
				SetMotorPwm(MOTOR_REV, 0);
				SetMotorPwm(MOTOR_FWD, 0);	
//			  sw_st = FIND_GAPNUM;
//		      stateFistin = 1;	  
		}break;	
		
	}
	  
  }
}



/**
 * @brief	   Motor_Init
 *
 * @param		init key and optic input
 *
 * @return		None
 *
 * @details
 */
void Motor_Init()

{ /* Configure PB.2 as Input mode and enable interrupt by
                                                   rising edge trigger */
 

//  sw_st = SWING_END;
  key_ctrl.last_level = 1;
/*
  timeinner = g_cfg.timeinner;
  swingspeed = g_cfg.swingspeed;
	swingtime =g_cfg.swingtime;
	swingtimeRight =g_cfg.swingtimeRight;
	swingtimePause =g_cfg.swingtimePause;
	runLoopTime = g_cfg.runLoopTime;
	runLoopSpeed =g_cfg.runLoopSpeed;
*/
  
}

/**
 * @brief	   Motor_mix
 *
 * @param		init key and optic input
 *
 * @return		0正常，1没离开光开，2，没回到光开
 *
 * @details    设置夹爪转动参数。
 */
uint32_t Motor_mix(uint32_t time, uint32_t speed, uint32_t swing,
                   uint32_t timeRight, uint32_t timePause) {

  timeinner = time;
  swingtime = swing;
  swingspeed = speed;
  swingtimeRight = timeRight;
  swingtimePause = timePause;
//  timeinner = time - time % (swing + timeRight);
//  curtimeinner = timeinner;
  if (speed == 0) {
    swingspeed = MOTOR_CHECK_SPEED;
  }
  
  curtimeinner = sysinf.conter10ms;

  sw_st = SWING_LOOP;
  stateFistin = 1;
	return 0;
}

/**
 * @brief	  Motor_deal_key
 *
 * @param	   按键处理，短按，左右摇；连续点击，转圈摇；长按，复位；
 *
 * @return	   0正常，1没离开光开，2，没回到光开
 *
 * @details	  夹爪根据。
 */
uint32_t Motor_deal_key(swing_state_def st, KeyState key) {
  if (st == SWING_IDLE) {
    switch (key) {
    case KEY_SHORT:
      curtimeinner = conter10ms;
      sw_st = SWING_LOOP;
	  startLoopConter = 2;
      stateFistin = 1;	  		
	  backOptFlag = 1;
	  findhomedir = MOTOR_REV;
      break;
//    case KEY_CONTINUE:
//      curtimeinner = conter10ms;
//      sw_st = LOOP_START;
//      stateFistin = 1;
//      break;
    case KEY_LONG:
      curtimeinner = conter10ms;
	  sw_st = FIND_GAPNUM;
	  stateFistin = 1;
      break;
    default:
      break;
    }

  } 
if (key == KEY_LONG || key == KEY_END ) {
	  sw_st = FIND_GAPNUM;
      stateFistin = 1;	  
	  resetFromSwingFlag = 0;
   }
	return 0;
}

/**
 * @brief	   Motor_mix_work
 *
 * @param		init key and optic input
 *
 * @return		0正常，1没离开光开，2，没回到光开
 *
 * @details    夹爪根据。
 */
uint32_t Motor_mix_work(void) {
  static uint32_t last10ms;
  static uint32_t firstINGap;
  static uint32_t powerOnFlag = 1;
  check_keyShortValid();
  if(powerOnFlag){
	powerOnFlag = 0;
//	Motor_Check(0);  
	resetFromSwingFlag = 0;
	sw_st = FIND_GAPNUM;
	stateFistin = 1;
  }
	conter10ms = sysinf.conter10ms;
  if (key_ctrl.key_valid_flag) {
    key_ctrl.key_valid_flag = 0;
    Motor_deal_key(sw_st, key_ctrl.state);
    key_ctrl.state = KEY_IDLE;
  }
  if(sw_st == SWING_ERR){
  		/*电机故障，不执行操作*/
		return 0;
  }
  switch (sw_st) {
  case SWING_IDLE: {
    SetMotorPwm(MOTOR_REV, 0);
    SetMotorPwm(MOTOR_FWD, 0);
  } break;
  case FIND_GAPNUM: {
    if (stateFistin) {
      timerconter = 0;
	  intoGap = 0;
	  OutGap = 0;
	  lastGap = 0;
	  judgeGap = 0;
	  HAL_TIM_Base_Stop(&htim4);
	  HAL_TIM_Base_Start(&htim4);
      last10ms = conter10ms;
      stateFistin = 0;    
	  startFindHometime = conter10ms;  
	  findhomedir = MOTOR_REV;
    checkPWM = swingspeed;
	  SetMotorPwm(findhomedir, checkPWM);
    } else  {
  	  if (((conter10ms - last10ms) > MOTOR_LOOP_TIM*4 )&&(timerconter < MAX_RUNCNT)) {
  		/*电机不动，返回报错*/
  		SetMotorPwm(MOTOR_REV, 0);
  		sw_st = SWING_ERR;
//  		error_set(ERROR_MOTOR_FIALED);
  		return 1;
  	  }
    } 
  } break;
  case FIND_HOMEGAP: {
    if (stateFistin) {
      last10ms = conter10ms;
      stateFistin = 0;
	  intoGapFlag = 0;
	  firstINGap = 0;
	  SetMotorPwm(findhomedir, checkPWM);
    } else  {
  	  if (!intoGapFlag && (conter10ms - last10ms) > MOTOR_LOOP_TIM) {
  		  /*电机来回不动，返回报错*/
		  stateFistin = 1;
		  checkPWM = checkPWM/0.85; /*降速便于复位*/
  	  }
	   if(intoGapFlag){
	   		if(!firstINGap){
				/*进入缝隙，置位，延时判读*/
				firstINGap = 1;
				last10ms = conter10ms;
			}else{
				if((conter10ms - last10ms) > MOTOR_MOVEFORM_OPTIC_TIM){
					if(Read_Motor()){							
					   sw_st = SWING_IDLE;
					   stateFistin = 1;
					   findHome = 1;
					  
					}
					else{
						sw_st = FIND_HOMEGAP;
					   stateFistin = 1;
				   	   findhomedir = !findhomedir;
					   checkPWM = checkPWM*0.9; /*降速便于复位*/		  
					}
				}
			}   	    
	   }
    } 
  } break;
  

  case LOOP_START: {
    if (stateFistin) {
      SetMotorPwm(MOTOR_REV, runLoopSpeed);
      last10ms = conter10ms;
      stateFistin = 0;
    } else {
      if ((conter10ms - last10ms) == (runLoopTime)) {
        SetMotorPwm(MOTOR_REV, 0);
        SetMotorPwm(MOTOR_FWD, 0);
      }
      if ((conter10ms - last10ms) >= (runLoopTime + swingtimePause)) {
        sw_st = LOOP_END;
        stateFistin = 1;
        REVORFWD = 0;
      }
    }
  } break;

  case LOOP_END: {
	sw_st = FIND_GAPNUM;
	stateFistin = 1;
    REVORFWD = 1;
  } break;


  case SWING_LOOP: { 
    if (stateFistin ) {
	  	/**/
	 	if(backOptFlag){
			if(swingtime< (conter10ms - swingStoptime)){
			 checkPWM = swingspeed;		
			 backOptFlag = 0;
			 SetMotorPwm(findhomedir, checkPWM);
			 stateFistin = 0;
			 last10ms = conter10ms;
			 swingHigStopflage = 0;
			}			
		}else{
			 SetMotorPwm(findhomedir, checkPWM);
			 stateFistin = 0;
			 last10ms = conter10ms;
			 swingHigStopflage = 0;
		}   
		intoGapFlag = 0;
		firstINGap = 0;
    } else{
  	  if (!intoGapFlag && (conter10ms - last10ms) > MOTOR_LOOP_TIM) {
  		/*速度过底，电机回不来*/	  
		  backOptFlag = 1;		  
		  swingStoptime = conter10ms;
		  findhomedir = MOTOR_REV;
		  stateFistin = 1;

  	  }
	   if(intoGapFlag){
	   		if(!firstINGap){
				/*进入缝隙，置位，延时判读*/
				firstINGap = 1;
				last10ms = conter10ms;
			}else{
			if((conter10ms - last10ms) > MOTOR_MOVEFORM_OPTIC_TIM){
				 stateFistin = 1;
				if(Read_Motor()){		
					backOptFlag = 1;		
					swingStoptime = conter10ms;
	  				findhomedir = MOTOR_REV;
				}
				else{
			   	   findhomedir = !findhomedir;
				   checkPWM = checkPWM*0.9; /*降速便于复位*/		  
				}
			}	
			}   
	   }
	   if ((conter10ms - curtimeinner) > (timeinner)){
		  resetFromSwingFlag = 0;  		  
			__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, 0);		 
			__HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_2, 0);
			timerconter = 0;
		  memset(timerBuf,0,sizeof(timerBuf));
	      stateFistin = 1;	
		  sw_st = FIND_GAPNUM;  
	  }
    } 
  } break;

  default:
  	sw_st = SWING_ERR;
    break;
  }
	return 0;
}


/**
 * @brief	   Motor_mix
 *
 * @param		init key and optic input
 *
 * @return		空闲返回1 其他返回0
 *
 * @details    设置夹爪转动参数。
 */
uint32_t Motor_idle() {
	return(sw_st == SWING_IDLE);
 
}
uint32_t print_home_time() {
	if(findHome){
		findHome = 0;
		Van_Device_Printf(VAN_LOG_NOTICE,"gap:%d  ",timerconter);
	    if(timerconter > sizeof(timerBuf)){
			timerconter = sizeof(timerBuf);
		}
		for(unsigned int i=0;i<timerconter;i++){
			Van_Device_Printf(VAN_LOG_NOTICE," %d  ",timerBuf[i]);
		}
		Van_Device_Printf(VAN_LOG_NOTICE,"\n");
	}
	
	return 0;
}