X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fsensorboard%2Fbeacon.c;fp=projects%2Fmicrob2010%2Fsensorboard%2Fbeacon.c;h=0000000000000000000000000000000000000000;hp=34f168603e1d9aa5d1edd3ea7224ed44fd94f9ab;hb=8d6a47e9e21a9a31f4bc12d32fb3d11091a4b305;hpb=821f753c0f88aff895d9feae59c442a6c446f96b

diff --git a/projects/microb2010/sensorboard/beacon.c b/projects/microb2010/sensorboard/beacon.c
deleted file mode 100755
index 34f1686..0000000
--- a/projects/microb2010/sensorboard/beacon.c
+++ /dev/null
@@ -1,397 +0,0 @@
-
-#include <stdio.h>
-#include <string.h>
-#include <math.h>
-
-#include <aversive.h>
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <uart.h>
-#include <i2c.h>
-#include <ax12.h>
-#include <parse.h>
-#include <rdline.h>
-#include <pwm_ng.h>
-#include <encoders_spi.h>
-#include <timer.h>
-#include <scheduler.h>
-#include <pid.h>
-#include <time.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <adc.h>
-#include <spi.h>
-
-#include <blocking_detection_manager.h>
-
-#include "sensor.h"
-
-#include "../common/i2c_commands.h"
-#include "main.h"
-#include "beacon.h"
-
-struct beacon beacon;
-
-#define BEACON_PWM_VALUE 1000
-#define IR_SENSOR() (!!(PINK&(1<<2)))
-#define MODULO_TIMER (1023L)
-#define COEFF_TIMER (2)
-#define COEFF_MULT (100L)
-#define COEFF_MULT2 (1000L)
-#define BEACON_SIZE (9)
-#define BEACON_MAX_SAMPLE (3)
-
-#define OPPONENT_POS_X (11)
-#define OPPONENT_POS_Y (22)
-
-#define BEACON_DEBUG(args...) DEBUG(E_USER_BEACON, args)
-#define BEACON_NOTICE(args...) NOTICE(E_USER_BEACON, args)
-#define BEACON_ERROR(args...) ERROR(E_USER_BEACON, args)
-
-static volatile int32_t rising = -1;
-static volatile int32_t falling = -1;
-
-static int32_t get_dist(float size);
-static int32_t get_angle(int32_t middle, int32_t ref);
-
-static int32_t pos_ref = 0;
-static int32_t invalid_count = 0;
-
-static volatile int32_t beacon_speed;
-static volatile int32_t beacon_save_count = 0;
-static volatile int32_t beacon_prev_save_count = 0;
-static volatile int32_t count = 0;
-static volatile int32_t count_diff_rising  = 0;
-static volatile int32_t count_diff_falling = 0;
-static int32_t beacon_coeff = 0;
-
-static volatile int8_t valid_beacon = 0;
-
-static volatile int32_t beacon_pos;
-
-//static int32_t beacon_sample_size[BEACON_MAX_SAMPLE];
-
-int32_t encoders_spi_get_value_beacon(void *number)
-{
-	int32_t ret;
-
-	ret = encoders_spi_get_value(number);
-	return ret*4;
-}
-
-
-void encoders_spi_set_value_beacon(void * number, int32_t v)
-{
-	encoders_spi_set_value(number, v/4);
-} 
-
-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 beacon_init(void)
-{
-	//int8_t i;
-
-	beacon_reset_pos();
-	pos_ref = encoders_spi_get_value_beacon(BEACON_ENCODER);
-
-	memset(&beacon, 0, sizeof(struct beacon));
-	beacon.opponent_x = I2C_OPPONENT_NOT_THERE;
-			
-	beacon_speed = 0;
-
-	/*for(i=0;i<BEACON_MAX_SAMPLE;i++)
-		beacon_sample_size[i] = 0;*/
-
-	/* set external interrupt (any edge) */
-	PCMSK2 = (1<<PCINT18);
-	PCICR  = (1<<PCIE2);
-
-
-}
-
-void beacon_calibre_pos(void)
-{
-	sensorboard.flags &= ~DO_CS;
-
-	/* init beacon pos */
-	pwm_ng_set(BEACON_PWM, 100);
-
-	/* find rising edge of the mirror*/
-	wait_ms(100);
-	while (sensor_get(BEACON_POS_SENSOR));
-	wait_ms(100);
-	while (!sensor_get(BEACON_POS_SENSOR));
-
-	pwm_ng_set(BEACON_PWM, 0);
-
-
-	beacon_reset_pos();
-	pid_reset(&sensorboard.beacon.pid);
-	encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE);
-
-	cs_set_consign(&sensorboard.beacon.cs, 0);
-
-	sensorboard.flags |= DO_CS;
-}
-
-void beacon_start(void)
-{
-	beacon_reset_pos();
-	sensorboard.beacon.on = 1;
-	cs_set_consign(&sensorboard.beacon.cs, 600);
-}
-
-void beacon_stop(void)
-{
-	sensorboard.beacon.on = 0;
-	pwm_ng_set(BEACON_PWM, 0);
-}
-
-void beacon_reset_pos(void)
-{
-	pwm_ng_set(BEACON_PWM, 0);
-	encoders_spi_set_value(BEACON_ENCODER, 0);
-}
-
-
-
-int32_t encoders_spi_get_beacon_speed(void * dummy)
-{
-	return beacon_speed;
-}
-
-
-//port K bit 2
-/* motor speed (top tour) */
-SIGNAL(SIG_PIN_CHANGE2) 
-{
-	uint8_t flags;
-
-	/* rising edge */
-	if ( IR_SENSOR()) {
-		IRQ_LOCK(flags);
-		count = TCNT3;
-		rising = beacon_pos;
-		count_diff_rising = (count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER;
-		valid_beacon = 0;
-		IRQ_UNLOCK(flags);
-
-	}
-	/* falling edge */
-	else {
-		IRQ_LOCK(flags);
-		count = TCNT3;
-		falling = beacon_pos;
-		count_diff_falling = (count - beacon_save_count + MODULO_TIMER + 1)&MODULO_TIMER;
-		valid_beacon = 1;
-		IRQ_UNLOCK(flags);
-	}
-}
-
-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;
-}
-
-static int32_t get_dist(float size)
-{
-	int32_t dist=0;
-	//int32_t alpha=0;
-
-	//alpha = (size*2*M_PI*COEFF_MULT2);
-	//dist = ((2*BEACON_SIZE*BEACON_STEP_TOUR*COEFF_MULT2)/alpha)/2;
-	
-	/* function found by measuring points up to 80cm */	
-	//dist = ((size - 600)*(size - 600)) / 2400 +28;
-
-	/* new function */
-	/* dist = a0 + a1*x + a2*x² + a3x³ */
-	dist = 1157.3 + (1.4146*size) + (-0.013508*size*size) + (0.00001488*size*size*size);
-	
-	return dist;
-
-}
-
-static int32_t get_angle(int32_t middle, int32_t ref)
-{
-	int32_t ret_angle;
-	
-	ret_angle = (middle - ref) * 360 / BEACON_STEP_TOUR;
-	
-	if(ret_angle > 360)
-		ret_angle -= 360;
-
-	return ret_angle;
-}
-
-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;
-
-}