checker_slave/source/task/moter.c

#include "moter.h"
#include "dev_flash.h"
#include "debug.h"
#include "commend.h"
#include "mymisc.h"
#include "board.h"


// <20><><EFBFBD><EFBFBD>״̬
typedef enum{
  STOP=0,
  INITING=1,
  DOWNING=2,
  UPING=3,
}moter_stat;


typedef struct{
  int inited;
  pwm_def *pwm;
  rt_event_t event;
  int run;
  int want_count;
  int count;
  int max_count;
  moter_stat stat;
  void (*fun_in_end)(void *t);
  void *t;
}self_def;


self_def g_self;



#define MOTER_MAX_COUNT 22000

#define MOTER_END_EVENT 0x1






static void end_irq(void *t)
{
  self_def *s=t;
  if(s->inited)
    rt_event_send(s->event,MOTER_END_EVENT);
}



static void moter_run(void *t)
{
  self_def *s=t;
  uint32_t ev;
  while(s->run)
  {
    rt_event_recv(s->event,0xffffffff,RT_EVENT_FLAG_OR|RT_EVENT_FLAG_CLEAR,RT_WAITING_FOREVER,&ev);
    if(ev&MOTER_END_EVENT)
    {
      if(s->stat==INITING)
      {
        s->count=0;
      }else if(s->stat==DOWNING)
      {
        s->count+=s->want_count;
      }else if(s->stat==UPING)
      {
        // <20><><EFBFBD><EFBFBD>ʱ s->want_count Ϊ<><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȼ<EFBFBD>Ǽ<EFBFBD>
        s->count+=s->want_count;
        if(s->count<0) s->count=0;
      }
      s->stat=STOP;
      DBG_LOG("moter stoped.");
      emit moter_end_signal(s);
      if(s->fun_in_end){
        s->fun_in_end(s->t);
        s->fun_in_end=0;
        s->t=0;
      }
    }
    ev=0;
  }
}




// <20><><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><C2BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ص<EFBFBD>
void moter_down(void (*fun)(void *t),void *t)
{
  self_def *s=&g_self;
  if(fun){
    s->fun_in_end=fun;
    s->t=t;
  }
  moter_start(0,s->max_count-s->count);
}

// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ص<EFBFBD>
void moter_up(void (*fun)(void *t),void *t)
{
  self_def *s=&g_self;
  if(fun){
    s->fun_in_end=fun;
    s->t=t;
  }
  moter_start(0,-s->count-100);
}


void moter_start(int fre,int step_count)
{
  self_def *s=&g_self;
  if(s->inited==0)
  {
    DBG_WARN("moter object is not inited.");
    return;
  }
  moter_start_slot(s,fre,step_count);
}

void moter_start_slot(void *t,int fre,int step_count)
{
  self_def *s=t;
  if(s->stat!=STOP)
  {
    DBG_WARN("moter is running!");
    return ;
  }
  if(s->count+step_count>s->max_count)
  {
    DBG_WARN("steps count exceeded limit!,max=%d",s->max_count);
    end_irq(s);
    return ;
  }
  else{
    DBG_LOG("s->count=%d,step_count=%d,s->max_count=%d",s->count,step_count,s->max_count);
  }
  if(step_count>0){
    s->stat=DOWNING;
  }else if(step_count<0){
    s->stat=UPING;
  }else{
    DBG_WARN("step_count can not == 0");
    return ;
  }
  s->want_count=step_count;
  pwm_def *pwm=s->pwm;
  pwm->set_fre(pwm,fre);
  pwm->start(pwm,step_count);
}


void moter_init(void *t)
{
  self_def *s=t;
  DBG_LOG("moter init to zero.");
  pwm_def *pwm=s->pwm;
  pwm->set_fre(pwm,2000);
  pwm->start(pwm,-22000);
}

int init_moter(void)
{
  self_def *s=&g_self;
  const sys_param_def *par=sys_param();
  if(strcmp(par->device_type,"checker")==0){
    pwm_def *pwm=dev_get("pwm1");
    if(pwm==0){
      cmd_print("can not find device \"pwm1\"");
      return -1;
    }
    pwm->init(pwm);
    pwm->set_irq_fun(pwm,end_irq,s);
    s->pwm=pwm;
    s->stat=INITING;
    if((par->moter_max_count>25000)||(par->moter_max_count<1000))
    {
      s->max_count=MOTER_MAX_COUNT;
    }else{
      s->max_count=par->moter_max_count;
    }
    s->count=s->max_count;
    s->event=rt_event_create("moter_e",RT_IPC_FLAG_FIFO);
    s->inited=1;
    s->run=1;
    rt_thread_t rt_t=rt_thread_create("moter_t",moter_run,s,2048,3,20);
    rt_thread_startup(rt_t);
    later_execute(moter_init,s,500);
    app_variable("moter",s,0);
  }
  return 0;
}
app_init_export(init_moter)