int main(void)
{
#ifndef HOST_VERSION
uint8_t flags, flags2;
IRQ_LOCK(flags);
IRQ_UNLOCK(flags);
IRQ_LOCK(flags);
IRQ_LOCK(flags2);
IRQ_UNLOCK(flags2);
IRQ_UNLOCK(flags);
#endif
return 0;
}
/** Alloc an event, and fill its field, then mark it as active.
* return its index in the table, or -1 if no evt is available */
int8_t
scheduler_add_event(uint8_t unicity, void (*f)(void *),
void * data, uint16_t period,
uint8_t priority) {
int8_t i;
uint8_t flags;
i = scheduler_alloc_event();
if ( i == -1 )
return -1;
if(!unicity)
g_tab_event[i].period = period ;
else
g_tab_event[i].period = 0 ;
g_tab_event[i].current_time = period ;
g_tab_event[i].priority = priority ;
g_tab_event[i].f = f;
g_tab_event[i].data = data;
IRQ_LOCK(flags);
g_tab_event[i].state = SCHEDULER_EVENT_ACTIVE;
IRQ_UNLOCK(flags);
return i;
}
void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y)
{
uint8_t flags;
int32_t local_x;
int32_t local_y;
int32_t x_opponent;
int32_t y_opponent;
int32_t local_robot_angle;
IRQ_LOCK(flags);
local_x = beacon.robot_x;
local_y = beacon.robot_y;
local_robot_angle = beacon.robot_angle;
IRQ_UNLOCK(flags);
if (local_robot_angle < 0)
local_robot_angle += 360;
x_opponent = cos((local_robot_angle + angle)* 2 * M_PI / 360)* dist;
y_opponent = sin((local_robot_angle + angle)* 2 * M_PI / 360)* dist;
//BEACON_DEBUG("x_op= %ld\t",x_opponent);
//BEACON_DEBUG("y_op= %ld\r\n",y_opponent);
//BEACON_NOTICE("robot_x= %ld\t",local_x);
//BEACON_NOTICE("robot_y= %ld\t",local_y);
//BEACON_NOTICE("robot_angle= %ld\r\n",local_robot_angle);
*x = local_x + x_opponent;
*y = local_y + y_opponent;
}
/** init module, give the robot system to use as a parameter */
void bd_init(struct blocking_detection * bd)
{
uint8_t flags;
IRQ_LOCK(flags);
memset(bd, 0, sizeof(*bd));
IRQ_UNLOCK(flags);
}
void quadramp_init(struct quadramp_filter * q)
{
uint8_t flags;
IRQ_LOCK(flags);
memset(q, 0, sizeof(*q));
IRQ_UNLOCK(flags);
}
/* executed at each byte transmission. */
void mf2_server_register_tx_event(void (*f)(char))
{
u08 flags;
IRQ_LOCK(flags);
tx_event = f;
IRQ_UNLOCK(flags);
}
/** reset current blocking */
void bd_reset(struct blocking_detection * bd)
{
uint8_t flags;
IRQ_LOCK(flags);
bd->cpt = 0;
IRQ_UNLOCK(flags);
}
/** All fcts pointers to NULL */
void error_init(void)
{
uint8_t flags;
IRQ_LOCK(flags);
memset(&g_error_fct, 0, sizeof(g_error_fct));
IRQ_UNLOCK(flags);
}
/** Register log function for WARNING level */
void error_register_warning(void (*f)(struct error *, ...))
{
uint8_t flags;
IRQ_LOCK(flags);
g_error_fct.warning = f;
IRQ_UNLOCK(flags);
}
/* executed at each byte reception. */
void mf2_client_register_rx_event(void (*f)(char))
{
u08 flags;
IRQ_LOCK(flags);
rx_event = f;
IRQ_UNLOCK(flags);
}
/** Register log function for NOTICE level */
void error_register_notice(void (*f)(struct error *, ...))
{
uint8_t flags;
IRQ_LOCK(flags);
g_error_fct.notice = f;
IRQ_UNLOCK(flags);
}
/* speed threshold */
void bd_set_speed_threshold(struct blocking_detection * bd,
uint16_t speed)
{
uint8_t flags;
IRQ_LOCK(flags);
bd->speed_thres = speed;
IRQ_UNLOCK(flags);
}
void pwm_init_3C(void)
{
uint8_t flags;
IRQ_LOCK(flags);
pwm_timer_16bits_init(3,C,PWM3C_MODE,TIMER3_PRESCALE, TIMER3_MODE);
IRQ_UNLOCK(flags);
}
/** get value of blocking detection */
uint8_t bd_get(struct blocking_detection * bd)
{
uint8_t ret, flags;
IRQ_LOCK(flags);
ret = (bd->cpt_thres && (bd->cpt == bd->cpt_thres));
IRQ_UNLOCK(flags);
return ret;
}
void pwm_set_2(int16_t value)
{
uint8_t flags;
IRQ_LOCK(flags);
pwm_timer_8bits_set(2,value,PWM2_MODE);
IRQ_UNLOCK(flags);
}
void pwm_set_1B(int16_t value)
{
uint8_t flags;
IRQ_LOCK(flags);
pwm_timer_16bits_set(1,B,value,PWM1B_MODE);
IRQ_UNLOCK(flags);
}
void pwm_init_1C(void)
{
uint8_t flags;
IRQ_LOCK(flags);
pwm_timer_16bits_init(1,C,PWM1C_MODE,TIMER1_PRESCALE, TIMER1_MODE);
IRQ_UNLOCK(flags);
}
void pwm_set_3C(int16_t value)
{
uint8_t flags;
IRQ_LOCK(flags);
pwm_timer_16bits_set(3,C,value,PWM3C_MODE);
IRQ_UNLOCK(flags);
}
void pwm_init_0(void)
{
uint8_t flags;
IRQ_LOCK(flags);
pwm_timer_8bits_init(0,PWM0_MODE,TIMER0_PRESCALE, TIMER0_MODE);
IRQ_UNLOCK(flags);
}
void pwm_init_2(void)
{
uint8_t flags;
IRQ_LOCK(flags);
pwm_timer_8bits_init(2,PWM2_MODE,TIMER2_PRESCALE, TIMER2_MODE);
IRQ_UNLOCK(flags);
}
void robot_cs_set_a_consign( robot_cs_t* rcs,
int32_t angle)
{
uint8_t flags;
IRQ_LOCK(flags);
cs_set_consign(&csm_angle,angle);
IRQ_UNLOCK(flags);
}
void biquad_set_series_son(struct biquad_filter *p, struct biquad_filter *son)
{
uint8_t flags;
IRQ_LOCK(flags);
p->son = son;
IRQ_UNLOCK(flags);
}
void biquad_set_deniminator_coeffs(struct biquad_filter *p, int16_t a1, int16_t a2)
{
uint8_t flags;
IRQ_LOCK(flags);
p->a1 = -a1;
p->a2 = -a2;
IRQ_UNLOCK(flags);
}
void quadramp_set_1st_order_vars(struct quadramp_filter * q,
uint32_t var_1st_ord_pos,
uint32_t var_1st_ord_neg)
{
uint8_t flags;
IRQ_LOCK(flags);
q->var_1st_ord_pos = var_1st_ord_pos;
q->var_1st_ord_neg = var_1st_ord_neg;
IRQ_UNLOCK(flags);
}
/* return the current time reference (the variable is incremented in the
* interuption */
static uint16_t get_time_ms(void)
{
uint8_t flags;
uint16_t ret;
IRQ_LOCK(flags);
ret = time_ref;
IRQ_UNLOCK(flags);
return ret;
}
void biquad_set_divisor_shifts(struct biquad_filter *p, uint8_t recursive_shift, uint8_t out_shift)
{
uint8_t flags;
IRQ_LOCK(flags);
p-> out_shift = out_shift;
p-> recursive_shift = recursive_shift;
IRQ_UNLOCK(flags);
}
/** accessors to coefficients */
void biquad_set_numerator_coeffs(struct biquad_filter *p, int16_t b0, int16_t b1, int16_t b2)
{
uint8_t flags;
IRQ_LOCK(flags);
p->b0 = b0;
p->b1 = b1;
p->b2 = b2;
IRQ_UNLOCK(flags);
}
/* get the xy pos of the opponent robot */
int8_t get_opponent_xy(int16_t *x, int16_t *y)
{
uint8_t flags;
IRQ_LOCK(flags);
*x = ballboard.opponent_x;
*y = ballboard.opponent_y;
IRQ_UNLOCK(flags);
if (*x == I2C_OPPONENT_NOT_THERE)
return -1;
return 0;
}
/** Extraction d'une valeur de codeur */
encoders encoders_get_value(uint8_t number)
{
encoders value;
uint8_t flags;
IRQ_LOCK(flags);
value = get_encoder(number);
IRQ_UNLOCK(flags);
return value;
}
void robot_cs_set_xy_consigns( robot_cs_t* rcs,
int32_t x,
int32_t y)
{
uint8_t flags;
IRQ_LOCK(flags);
cs_set_consign(&csm_x,x);
cs_set_consign(&csm_y,y);
IRQ_UNLOCK(flags);
}