204 lines
3.5 KiB
C
204 lines
3.5 KiB
C
#include "moter.h"
|
|
#include "dev_flash.h"
|
|
#include "debug.h"
|
|
#include "commend.h"
|
|
#include "mymisc.h"
|
|
#include "board.h"
|
|
|
|
|
|
// 运行状态
|
|
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)
|
|
{
|
|
// 向上时 s->want_count 为负数，所以依然是加
|
|
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;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
// 电机下降，完成时回调
|
|
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);
|
|
}
|
|
|
|
// 电机上升，完成时回调
|
|
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)
|
|
|
|
|
|
|
|
|
|
|
|
|