void TERM_TASK_motor_x(void *p_arg)
{
OS_ERR err;
OS_MSG_SIZE size;
CPU_TS ts;
CPU_INT16U x_input, prev_x_input = TERM_MOTOR_MIDDLE;
CPU_INT16U pulse;
while (DEF_ON) {
//sensor의 값을 기다린다.
x_input = (CPU_INT16U)OSTaskQPend(0,
(OS_OPT )OS_OPT_PEND_BLOCKING,
(OS_MSG_SIZE *)&size,
(CPU_TS *)&ts,
(OS_ERR *)&err);
pulse = TERM_sensorXtoMotorX(x_input);
//sensor의 값을 안정화
if(x_input - prev_x_input < 20
&& x_input - prev_x_input > -20)
x_input = prev_x_input;
prev_x_input = x_input;
//모터의 각도 설정
TERM_PWM_PULSE_X = TERM_MOTOR_MAX + TERM_MOTOR_MIN - pulse;
BSP_Ser_Printf("\n receive x : %d, pulse : %d ccr1 : %d\n\n", x_input, pulse, TERM_PWM_PULSE_X);
//BSP_Ser_Printf("\n BIAS : %d", Bias_X);
}
}
void tasklet_action(void *p_arg)
{
OS_ERR err;
OS_MSG_SIZE msg_size;
struct tasklet_struct *t;
CPU_SR cpu_sr;
while(1)
{
t = OSTaskQPend(0, OS_OPT_PEND_BLOCKING, &msg_size, 0,&err);
if(err != OS_ERR_NONE)
{
USBH_DBG("tasklet_schedule OSTaskQPend Failed %d\r\n",err);
continue;
}
if(!t){
USBH_DBG("tasklet_schedule t == NULL\r\n");
continue;
}
// OSSchedLock(&err);
CPU_CRITICAL_ENTER();
if(t->state == TASKLET_STATE_UNSCHED)
USBH_DBG("tasklet_action Error at %d\r\n",__LINE__);
t->state = TASKLET_STATE_UNSCHED;
CPU_CRITICAL_EXIT();
t->func(t->data);
// OSSchedUnlock(&err);
}
}
bool poll_task_event_queue(void)
{
OS_ERR osErr;
OS_MSG_SIZE size;
uint32_t event;
event = (uint32_t)OSTaskQPend(0, OS_OPT_PEND_NON_BLOCKING, &size, NULL, &osErr);
if(osErr == OS_ERR_PEND_WOULD_BLOCK)
{
return false;
}
if(osErr != OS_ERR_NONE)
{
NABTO_LOG_ERROR(("Error in receiver task: %i", osErr));
return false;
}
NABTO_LOG_TRACE(("Received event: %u", event));
if (event & EVENT_RECV_DATA)
{
uint8_t index = event & EVENT_MASK;
if(sockets[index].isOpen)
{
OSSemPost(&sockets[index].receiverSemaphore, OS_OPT_POST_NO_SCHED, &osErr);
if(osErr != OS_ERR_NONE)
{
NABTO_LOG_ERROR(("Unable to post on socket receive semaphore!"));
}
NABTO_LOG_TRACE(("Received packet on socket %u", index));
}
else
{
NABTO_LOG_TRACE(("Data on non-open socket."));
}
}
else
{
NABTO_LOG_TRACE(("Dropped unexpected event."));
}
return true;
}
//显示消息队列中的消息
void msgdis_task(void *p_arg)
{
u8 *p;
OS_MSG_SIZE size;
OS_ERR err;
while(1)
{
//请求消息
p=OSTaskQPend((OS_TICK )0,
(OS_OPT )OS_OPT_PEND_BLOCKING,
(OS_MSG_SIZE* )&size,
(CPU_TS* )0,
(OS_ERR* )&err );
LCD_ShowString(40,270,100,16,16,p);
myfree(SRAMIN,p); //释放内存
OSTimeDlyHMSM(0,0,1,0,OS_OPT_TIME_PERIODIC,&err); //延时1s
}
}
//浮点测试任务
void float_task(void *p_arg)
{
OS_ERR err;
OS_MSG_SIZE size;
char *p;
//CPU_SR_ALLOC();
while(1)
{
p=OSTaskQPend((OS_TICK )0,
(OS_OPT )OS_OPT_PEND_BLOCKING,
(OS_MSG_SIZE* )&size,
(CPU_TS* )0,
(OS_ERR* )&err );
printf("float task get que . \n\r");
printf("que: %s. \n\r",p);
OSTimeDlyHMSM(0,0,0,800,OS_OPT_TIME_HMSM_STRICT,&err); //延时800ms
}
}
void TERM_TASK_motor_y(void *p_arg)
{
OS_ERR err;
OS_MSG_SIZE size;
CPU_TS ts;
CPU_INT16U y_input, prev_y_input = TERM_MOTOR_MIDDLE;
CPU_INT16U pulse;
CPU_INT16U bias = 0;
while (DEF_ON) {
y_input = (CPU_INT16U)OSTaskQPend(0,
(OS_OPT )OS_OPT_PEND_BLOCKING,
(OS_MSG_SIZE *)&size,
(CPU_TS *)&ts,
(OS_ERR *)&err);
if(y_input - prev_y_input < 20
&& y_input - prev_y_input > -20)
y_input = prev_y_input;
prev_y_input = y_input;
pulse = TERM_sensorYtoMotorY(y_input);
//bias의 값을 가져온다.
OSSemPend(&SEM_bias_y, 10, OS_OPT_PEND_NON_BLOCKING, &ts, &err);
switch(err){
case OS_ERR_NONE:
bias = bias_y;
OSSemPost(&SEM_bias_y, OS_OPT_POST_1, &err);
break;
}
TERM_PWM_PULSE_Y = bias + TERM_MOTOR_MAX + TERM_MOTOR_MIN - pulse;
BSP_Ser_Printf("\n receive y : %d, pulse : %d ccr2 : %d\n\n", y_input, pulse, TERM_PWM_PULSE_Y);
BSP_Ser_Printf("\n BIAS : %d", bias);
}
}
void ts_serv_task(void *p_arg)
{
void *p_msg;
OS_MSG_SIZE msg_size;
CPU_TS ts;
OS_ERR err;
(void)&p_arg;
while (1)
{
p_msg = OSTaskQPend(0,
OS_OPT_PEND_BLOCKING,
&msg_size,
&ts,
&err);
if (err == OS_ERR_NONE)
{
//ts_send_byte_str((uint8_t *)p_msg, msg_size);
ts_recv_serv((uint8_t *)p_msg, (uint32_t)msg_size);
}
}
}
