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);
}
/** get a free event, mark it as allocated and return its index, or -1
* if not found. */
static inline int8_t
scheduler_alloc_event(void)
{
uint8_t i;
uint8_t flags;
for (i=0 ; i<SCHEDULER_NB_MAX_EVENT ; i++) {
IRQ_LOCK(flags);
if( g_tab_event[i].state == SCHEDULER_EVENT_FREE ) {
g_tab_event[i].state = SCHEDULER_EVENT_ALLOCATED;
IRQ_UNLOCK(flags);
return i;
}
IRQ_UNLOCK(flags);
}
return -1;
}
s08 mf2_client_send(char c)
{
u16 mask, cpt=0;
u08 flags;
IRQ_LOCK(flags);
/* we don't preempt the remote device, even if the
protocol allow it */
if (!mf2_client_ready()) {
IRQ_UNLOCK(flags);
return -1;
}
state=MF2_CLIENT_STATE_XMIT;
current_bitnum = 1;
disable_intr();
/* set clk to 0 */
clk_0();
IRQ_UNLOCK(flags);
tx_buf = c;
tx_c = c;
tx_buf <<= 1;
tx_buf |= STOP_BIT;
for (mask = START_BIT ; mask != STOP_BIT; mask<<=1 ) {
if (tx_buf & mask)
cpt++;
}
if (!(cpt % 2))
tx_buf |= PARITY_BIT;
#if CONFIG_MODULE_MF2_CLIENT_USE_SCHEDULER
scheduler_add_single_event(start_sending, NULL, 1000L/SCHEDULER_UNIT);
#else
wait_ms(1);
start_sending(NULL);
#endif
return 0;
}
/** 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;
}
/* 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 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 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);
}
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);
}
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);
}
/* 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;
}
/** 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;
}
/** 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;
}
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);
}
/** init sets an unity filter, as usual */
void biquad_init (struct biquad_filter * p)
{
uint8_t flags;
IRQ_LOCK(flags);
/* set all structure to 0 */
memset(p, 0, sizeof(struct biquad_filter));
/* unity filter */
p-> b0=1;
IRQ_UNLOCK(flags);
}
/* thresholds */
void bd_set_current_thresholds(struct blocking_detection * bd,
int32_t k1, int32_t k2,
uint32_t i_thres, uint16_t cpt_thres)
{
uint8_t flags;
IRQ_LOCK(flags);
bd->k1 = k1;
bd->k2 = k2;
bd->i_thres = i_thres;
bd->cpt_thres = cpt_thres;
bd->cpt = 0;
IRQ_UNLOCK(flags);
}
/* get the da pos of the opponent robot */
int8_t get_opponent_da(int16_t *d, int16_t *a)
{
uint8_t flags;
int16_t x_tmp;
IRQ_LOCK(flags);
x_tmp = ballboard.opponent_x;
*d = ballboard.opponent_d;
*a = ballboard.opponent_a;
IRQ_UNLOCK(flags);
if (x_tmp == I2C_OPPONENT_NOT_THERE)
return -1;
return 0;
}
/** this is useful for cleaning the filter memories before a new data
* set. With this you avoid having old data in the filter memories
* when beginning to filter a new stream. Can also be used after
* changing the coefficients, to avoid jumps of the output value.
*/
void biquad_flush_memories(struct biquad_filter *p)
{
uint8_t flags;
IRQ_LOCK(flags);
/* empty mem cells */
p->mem_in_1 = 0;
p->mem_in_2 = 0;
p->mem_out_1 = 0;
p->mem_out_2 = 0;
IRQ_UNLOCK(flags);
}
int8_t mf2_server_send(char c)
{
uint8_t flags;
IRQ_LOCK(flags);
if (!mf2_server_ready()) {
IRQ_UNLOCK(flags);
// recv();
return -1;
}
mf2_state = MF2_SERVER_STATE_SEND;
mf2_step = 1;
mf2_data_send = c;
mf2_parity_cpt = 0;
timer1A_register_OC_intr_at_tics(mf2_server_timer_cb,
timer1_get()+MF2_SERVER_CLK_HALF_PERIOD);
clk_Z();
data_Z();
IRQ_UNLOCK(flags);
return 0;
}
int16_t sensor_get_adc(uint8_t i)
{
#ifdef HOST_VERSION
return 0;
#else
int16_t tmp;
uint8_t flags;
IRQ_LOCK(flags);
tmp = adc_infos[i].value;
IRQ_UNLOCK(flags);
return tmp;
#endif
}
void robot_cs_activate(robot_cs_t* rcs, uint8_t active)
{
uint8_t flags;
// must be performed on a interruption free environnement
IRQ_LOCK(flags);
if(!active)
hrobot_set_motors(rcs->hrs, 0, 0, 0);
else
rcs->reactivated = 1;
rcs->active = active;
IRQ_UNLOCK(flags);
}
void scheduler_del_event(int8_t i)
{
uint8_t flags;
/* if scheduled, it will be deleted after execution.
* if active, free it.
* else do nothing. */
IRQ_LOCK(flags);
if (g_tab_event[i].state == SCHEDULER_EVENT_SCHEDULED) {
g_tab_event[i].state = SCHEDULER_EVENT_DELETING;
}
else if (g_tab_event[i].state == SCHEDULER_EVENT_ACTIVE) {
g_tab_event[i].state = SCHEDULER_EVENT_FREE;
}
IRQ_UNLOCK(flags);
}
// Initialize
void i2cm_init(void)
{
uint8_t flags;
IRQ_LOCK(flags);
TWBR = I2C_BITRATE;
TWCR = (1<<TWEN) | (1<<TWINT);
if(I2C_PRESCALER & 1)
sbi(TWSR, TWPS0);
if(I2C_PRESCALER & 2)
sbi(TWSR, TWPS1);
IRQ_UNLOCK(flags);
}
void mf2_client_init(void)
{
u08 flags;
IRQ_LOCK(flags);
/* ports are inputs, and values are 0 */
data_Z();
clk_Z();
cbi(MF2_CLIENT_DATA_PORT, MF2_CLIENT_DATA_BIT);
cbi(MF2_CLIENT_CLK_PORT, MF2_CLIENT_CLK_BIT);
set_falling_edge();
state = MF2_CLIENT_STATE_IDLE;
current_bitnum = 0;
IRQ_UNLOCK(flags);
}
int32_t encoders_spi_update_beacon_speed(void * number)
{
int32_t ret;
uint8_t flags;
IRQ_LOCK(flags);
ret = encoders_spi_get_value_beacon(number);
beacon_speed = ret - beacon_pos;
beacon_pos = ret;
beacon_prev_save_count = beacon_save_count;
beacon_save_count = TCNT3;
IRQ_UNLOCK(flags);
beacon_coeff = COEFF_TIMER * COEFF_MULT;//beacon_speed * COEFF_MULT / ((beacon_prev_save_count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER);
return beacon_speed;
}
void i2cs_init(uint8_t slave_address)
{
uint8_t flags;
IRQ_LOCK(flags);
TWBR = I2C_BITRATE;
/* enable, enable int, respond to own adress */
TWCR = (1<<TWEA) | (1<<TWEN) | (1<<TWIE) ;
// prescaler
if(I2C_PRESCALER & 1)
sbi(TWSR, TWPS0);
if(I2C_PRESCALER & 2)
sbi(TWSR, TWPS1);
TWAR = slave_address << 1 ; // change for TWAR format
IRQ_UNLOCK(flags);
}
void r3d2_monitor(void *dummy)
{
uint16_t object_angle, object_duty_cycle;
double float_duty_cycle;
uint8_t irq_ctx;
uint16_t object_beginnig_detection_local, object_end_detection_local, motor_period_local;
// if new position is avaliable
if(position_updated_value == position_updated)// <= 3);
{
// recopy value on local variable to avoid modification during computation
IRQ_LOCK(irq_ctx);
object_beginnig_detection_local = object_beginnig_detection;
object_end_detection_local = object_end_detection;
motor_period_local = motor_period;
IRQ_UNLOCK(irq_ctx);
// robot position computation
object_duty_cycle = object_end_detection_local - object_beginnig_detection_local;
object_angle = object_beginnig_detection_local + object_duty_cycle/2;
detected_robot_angle = (((float)object_angle)*360)/motor_period_local;
detected_robot_angle = (detected_robot_angle + robot_detected_angle_offset);
if (detected_robot_angle > 360)
{
detected_robot_angle -= 360;
}
else if(detected_robot_angle <0)
{
detected_robot_angle += 360;
}
float_duty_cycle = (((float)object_duty_cycle)*360)/motor_period_local;
detected_robot_distance = surface_reflection_ratio/(2 * tan((float_duty_cycle/2)*M_PI/180));
// update flags
robot_detected_value = robot_detected;
position_updated_value = position_not_updated;
// reset robot detection timout
robot_detected_timout_value =0;
}
else
{
// robot decetion timout reached
if (robot_detected_timout_value >= robot_detected_timout_treshold)
{
robot_detected_value = robot_not_detected;
}
else
{
robot_detected_timout_value ++;
}
}
// value not protected from irq
if(motor_rotating_timout_value == 0 && is_motor_powered())
{
WARNING(0,"motor stopped, restarting");
start_motor();
motor_rotating_timout_value = motor_rotating_timout_treshold;
motor_error_value = motor_error;
}
else
{
motor_error_value = motor_error; /// XXX modify this flag that will toogle in case of motor error detection
motor_rotating_timout_value --;
}
}
void beacon_calc(void *dummy)
{
static uint8_t a=0;
static int32_t local_rising, local_falling;
static int32_t middle;
static float size = 0;
int32_t local_angle;
int32_t local_dist;
int32_t local_count_diff_rising ;
int32_t local_count_diff_falling ;
int32_t local_beacon_coeff;
int32_t result_x=0;
int32_t result_y=0;
int32_t temp=0;
int32_t edge=0;
//int32_t total_size=0;
uint8_t flags;
//uint8_t i;
int8_t local_valid;
if(a)
LED4_ON();
else
LED4_OFF();
a = !a;
if (falling == -1){
/* 0.5 second timeout */
if (invalid_count < 25)
invalid_count++;
else {
IRQ_LOCK(flags);
beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
IRQ_UNLOCK(flags);
}
return;
}
invalid_count = 0;
IRQ_LOCK(flags);
local_valid = valid_beacon;
local_count_diff_rising = count_diff_rising;
local_count_diff_falling = count_diff_falling ;
local_rising = rising;
local_falling = falling;
local_beacon_coeff = beacon_coeff;
IRQ_UNLOCK(flags);
if (local_valid){
invalid_count = 0;
//BEACON_DEBUG("rising= %ld\t",local_rising);
//BEACON_DEBUG("falling= %ld\r\n",local_falling);
/* recalculate number of pulse by adding the value of the counter, then put value back into motor's round range */
local_rising = ((local_rising + (local_count_diff_rising * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
local_falling = ((local_falling + (local_count_diff_falling * local_beacon_coeff) / COEFF_MULT)) %(BEACON_STEP_TOUR);
//BEACON_DEBUG("rising1= %ld\t",local_rising);
//BEACON_DEBUG("falling1= %ld\r\n",local_falling);
//BEACON_DEBUG("count diff rising= %ld\t",local_count_diff_rising);
//BEACON_DEBUG("count diff falling= %ld\r\n",local_count_diff_falling);
/* if around 360 deg, rising > falling, so invert both and recalculate size and middle */
if(local_falling < local_rising){
temp = local_rising;
local_rising = local_falling;
local_falling = temp;
size = BEACON_STEP_TOUR - local_falling + local_rising;
middle = (local_falling + ((int32_t)(size)/2) + BEACON_STEP_TOUR) %(BEACON_STEP_TOUR);
edge = local_falling;
}
/* else rising > falling */
else{
size = local_falling - local_rising;
middle = local_rising + (size / 2);
edge = local_rising;
}
//for(i=BEACON_MAX_SAMPLE-1;i>0;i--){
// beacon_sample_size[i] = beacon_sample_size[i-1];
// total_size += beacon_sample_size[i];
//}
//beacon_sample_size[0] = size;
//total_size += size;
//total_size /= BEACON_MAX_SAMPLE;
//BEACON_DEBUG("rising2= %ld\t",local_rising);
//BEACON_DEBUG("falling2= %ld\r\n",local_falling);
/* BEACON_DEBUG("size= %ld %ld\t",size, total_size); */
BEACON_DEBUG("size= %f\r\n",size);
//BEACON_DEBUG("middle= %ld\r\n",middle);
local_angle = get_angle(middle,0);
BEACON_NOTICE("opponent angle= %ld\t",local_angle);
local_dist = get_dist(size);
BEACON_NOTICE("opponent dist= %ld\r\n",local_dist);
beacon_angle_dist_to_x_y(local_angle, local_dist, &result_x, &result_y);
IRQ_LOCK(flags);
beacon.opponent_x = result_x;
beacon.opponent_y = result_y;
beacon.opponent_angle = local_angle;
beacon.opponent_dist = local_dist;
/* for I2C test */
//beacon.opponent_x = OPPONENT_POS_X;
//beacon.opponent_y = OPPONENT_POS_Y;
IRQ_UNLOCK(flags);
BEACON_NOTICE("opponent x= %ld\t",beacon.opponent_x);
BEACON_NOTICE("opponent y= %ld\r\n\n",beacon.opponent_y);
}
else {
BEACON_NOTICE("non valid\r\n\n");
}
falling = -1;
}
