SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c
SRC += commands_cs.c commands_ballboard.c commands.c
SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
-SRC += state.c
+SRC += state.c beacon.c
# List Assembler source files here.
# Make them always end in a capital .S. Files ending in a lowercase .s
--- /dev/null
+
+#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 <clock_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<<5)))
+#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<<PCINT21);
+ PCICR = (1<<PCIE2);
+
+
+}
+
+void beacon_calibre_pos(void)
+{
+ ballboard.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(&ballboard.beacon.pid);
+ encoders_spi_set_value_beacon(BEACON_ENCODER, BEACON_OFFSET_CALIBRE);
+
+ cs_set_consign(&ballboard.beacon.cs, 0);
+
+ ballboard.flags |= DO_CS;
+}
+
+void beacon_start(void)
+{
+ beacon_reset_pos();
+ ballboard.beacon.on = 1;
+ cs_set_consign(&ballboard.beacon.cs, 600);
+}
+
+void beacon_stop(void)
+{
+ ballboard.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 5
+/* 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;
+
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation (2008)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: beacon.h,v 1.2 2009-05-27 20:04:07 zer0 Exp $
+ *
+ */
+
+struct beacon {
+ int32_t beacon_speed;
+
+ int32_t opponent_angle;
+ int32_t opponent_dist;
+ int32_t prev_opponent_angle;
+ int32_t prev_opponent_dist;
+ int32_t robot_x;
+ int32_t robot_y;
+ int32_t robot_angle;
+ int32_t opponent_x;
+ int32_t opponent_y;
+};
+
+/* real encoder value: 3531.75 so, multiple by 4 to have round
+ * value */
+#define BEACON_STEP_TOUR (14127L)
+#define BEACON_OFFSET_CALIBRE 40
+
+extern struct beacon beacon;
+
+void beacon_dump(void);
+void beacon_init(void);
+void beacon_calibre_pos(void);
+void beacon_start(void);
+void beacon_stop(void);
+void beacon_calc(void *dummy);
+void beacon_angle_dist_to_x_y(int32_t angle, int32_t dist, int32_t *x, int32_t *y);
+
+void beacon_reset_pos(void);
+void beacon_set_consign(int32_t val);
+
+int32_t encoders_spi_get_beacon_speed(void *dummy);
+int32_t encoders_spi_update_beacon_speed(void *number);
+int32_t encoders_spi_get_value_beacon(void *number);
+
pwm_ng_set(ROLLER_PWM, 0);
pwm_ng_set(FORKTRANS_PWM, 0);
pwm_ng_set(FORKROT_PWM, 0);
+ pwm_ng_set(BEACON_PWM, 0);
}
ballboard.flags &= (~bit);
}
prog_char csb_roller_str[] = "roller";
prog_char csb_forktrans_str[] = "forktrans";
prog_char csb_forkrot_str[] = "forkrot";
+prog_char csb_beacon_str[] = "beacon";
struct csb_list csb_list[] = {
{ .name = csb_roller_str, .csb = &ballboard.roller },
{ .name = csb_forktrans_str, .csb = &ballboard.forktrans },
{ .name = csb_forkrot_str, .csb = &ballboard.forkrot },
+ { .name = csb_beacon_str, .csb = &ballboard.beacon },
};
struct cmd_cs_result {
};
/* token to be used for all cs-related commands */
-prog_char str_csb_name[] = "roller#forktrans#forkrot";
+prog_char str_csb_name[] = "roller#forktrans#forkrot#beacon";
parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
struct cs_block *cs_from_name(const char *name)
static const prog_char sensor_log[] = "sensor";
static const prog_char block_log[] = "bd";
static const prog_char state_log[] = "state";
+static const prog_char beacon_log[] = "beacon";
struct log_name_and_num {
const prog_char * name;
{ sensor_log, E_USER_SENSOR },
{ block_log, E_BLOCKING_DETECTION_MANAGER },
{ state_log, E_USER_ST_MACH },
+ { beacon_log, E_USER_BEACON },
};
static uint8_t
prog_char str_log_arg1_type[] = "type";
parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
/* keep it sync with log_name_and_num above */
-prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#state";
+prog_char str_log_arg2_type[] = "uart#i2c#i2cproto#sensor#bd#state#beacon";
parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
prog_char str_log_arg3[] = "on#off";
parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
#include "main.h"
#include "actuator.h"
+#include "beacon.h"
int32_t encoders_spi_update_roller_speed(void *number)
{
cs_manage(&ballboard.forktrans.cs);
if (ballboard.forkrot.on)
cs_manage(&ballboard.forkrot.cs);
+ if (ballboard.beacon.on)
+ cs_manage(&ballboard.beacon.cs);
}
if ((ballboard.flags & DO_BD) && (ballboard.flags & DO_POWER)) {
bd_manage_from_cs(&ballboard.forktrans.bd, &ballboard.forktrans.cs);
bd_set_speed_threshold(&ballboard.forkrot.bd, 150);
bd_set_current_thresholds(&ballboard.forkrot.bd, 500, 8000, 1000000, 200);
+ /* BEACON */
+
+ /* PID */
+ pid_init(&ballboard.beacon.pid);
+ pid_set_gains(&ballboard.beacon.pid, 80, 80, 250);
+ pid_set_maximums(&ballboard.beacon.pid, 0, 10000, 1500);
+ pid_set_out_shift(&ballboard.beacon.pid, 6);
+ pid_set_derivate_filter(&ballboard.beacon.pid, 6);
+
+ /* CS */
+ cs_init(&ballboard.beacon.cs);
+ cs_set_correct_filter(&ballboard.beacon.cs, pid_do_filter, &ballboard.beacon.pid);
+ cs_set_process_in(&ballboard.beacon.cs, pwm_ng_set, BEACON_PWM);
+ cs_set_process_out(&ballboard.beacon.cs, encoders_spi_update_beacon_speed, BEACON_ENCODER);
+ cs_set_consign(&ballboard.beacon.cs, 0);
+
/* set them on !! */
ballboard.roller.on = 1;
ballboard.forktrans.on = 1;
ballboard.forkrot.on = 0;
+ ballboard.beacon.on = 0;
scheduler_add_periodical_event_priority(do_cs, NULL,
#include "main.h"
#include "state.h"
#include "sensor.h"
+#include "beacon.h"
#include "actuator.h"
void i2c_protocol_init(void)
ans.ball_count = state_get_ball_count();
ans.lcob = cob_detect_left();
ans.rcob = cob_detect_right();
+ ans.opponent_x = beacon.opponent_x;
+ ans.opponent_y = beacon.opponent_y;
+ ans.opponent_a = beacon.opponent_angle;
+ ans.opponent_d = beacon.opponent_dist;
i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
sizeof(ans), I2C_CTRL_GENERIC);
break;
}
+ case I2C_CMD_BALLBOARD_SET_BEACON:
+ {
+ struct i2c_cmd_ballboard_start_beacon *cmd = void_cmd;
+ if (size != sizeof (*cmd))
+ goto error;
+
+ if (cmd->enable)
+ beacon_start();
+ else
+ beacon_stop();
+ break;
+ }
+
case I2C_CMD_BALLBOARD_SET_MODE:
{
struct i2c_cmd_ballboard_set_mode *cmd = void_cmd;
#include "cs.h"
#include "i2c_protocol.h"
#include "state.h"
+#include "beacon.h"
/* 0 means "programmed"
* ---- with 16 Mhz quartz
&PORTD, 5);
PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED |
PWM_NG_MODE_SIGN_INVERTED, &PORTD, 6);
- PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
+ PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED |
+ PWM_NG_MODE_SIGN_INVERTED,
&PORTD, 7);
/* sensors, will also init hardware adc */
sensor_init();
+ /* beacon */
+ beacon_init();
+ scheduler_add_periodical_event_priority(beacon_calc, NULL,
+ 20000L / SCHEDULER_UNIT,
+ BEACON_PRIO);
/* TIME */
time_init(TIME_PRIO);
sei();
actuator_init();
+ beacon_calibre_pos();
printf_P(PSTR("\r\n"));
printf_P(PSTR("Dass das Gluck deinen Haus setzt.\r\n"));
#define ROLLER_ENCODER ((void *)0)
#define FORKTRANS_ENCODER ((void *)1)
#define FORKROT_ENCODER ((void *)2)
+#define BEACON_ENCODER ((void *)3)
#define ROLLER_PWM ((void *)&gen.pwm1_4A)
#define FORKTRANS_PWM ((void *)&gen.pwm2_4B)
#define FORKROT_PWM ((void *)&gen.pwm3_1A)
-#define XXX_PWM ((void *)&gen.pwm4_1B)
+#define BEACON_PWM ((void *)&gen.pwm4_1B)
-#define BALL_PRESENT_SENSOR 2 /* XXX dummy example */
+#define BEACON_POS_SENSOR 2
/** ERROR NUMS */
#define E_USER_I2C_PROTO 195
#define E_USER_SENSOR 196
#define E_USER_ST_MACH 197
+#define E_USER_BEACON 198
#define LED_PRIO 170
#define TIME_PRIO 160
#define ADC_PRIO 120
#define CS_PRIO 100
+#define BEACON_PRIO 80
#define I2C_POLL_PRIO 20
#define CS_PERIOD 5000L
struct cs_block roller;
struct cs_block forktrans;
struct cs_block forkrot;
+ struct cs_block beacon;
/* robot status */
uint8_t our_color;
/* synchronize with sensor.c */
#define S_HIGH_BARRIER 0
#define S_LOW_BARRIER 1
-#define S_CAP3 2
-#define S_CAP4 3
+#define S_CAP3 2 /* TT balise */
+#define S_CAP4 3 /* INTR balise */
#define S_R_IR 4
#define S_R_US 5
#define S_L_US 6
uint8_t mode;
};
+#define I2C_CMD_BALLBOARD_SET_BEACON 0x04
+
+struct i2c_cmd_ballboard_start_beacon {
+ struct i2c_cmd_hdr hdr;
+ uint8_t enable;
+};
+
/****/
/* requests and their answers */
/****/
#define I2C_BALLBOARD_STATUS_F_EXCPT 0x02
uint8_t status;
+ int16_t opponent_x;
+ int16_t opponent_y;
+ int16_t opponent_a;
+ int16_t opponent_d;
+
uint8_t ball_count;
/* detection of cobs */
SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
SRC += strat_utils.c strat_base.c strat.c strat_corn.c
-SRC += strat_db.c strat_avoid.c beacon.c
+SRC += strat_db.c strat_avoid.c
ifeq ($(H),1)
SRC += robotsim.c
endif
+++ /dev/null
-/*
- * Copyright Droids Corporation (2010)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: strat.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $
- *
- */
-
-#include <stdio.h>
-#include <string.h>
-
-#include <aversive/pgmspace.h>
-#include <aversive/wait.h>
-#include <aversive/error.h>
-
-#include <ax12.h>
-#include <uart.h>
-#include <pwm_ng.h>
-#include <i2c.h>
-#include <clock_time.h>
-
-#include <scheduler.h>
-#include <pid.h>
-#include <quadramp.h>
-#include <control_system_manager.h>
-#include <trajectory_manager.h>
-#include <trajectory_manager_utils.h>
-#include <vect_base.h>
-#include <lines.h>
-#include <polygon.h>
-#include <obstacle_avoidance.h>
-#include <blocking_detection_manager.h>
-#include <robot_system.h>
-#include <position_manager.h>
-
-#include <rdline.h>
-#include <parse.h>
-#include <parse_string.h>
-#include <parse_num.h>
-
-#include "../common/i2c_commands.h"
-#include "main.h"
-#include "strat_utils.h"
-
-#define BEACON_UART_NUM 2
-
-#define INIT 0
-#define OPP0 1
-#define OPP1 2
-#define OPP2 3
-#define OPP3 4
-#define STA0 5
-#define STA1 6
-#define STA2 7
-#define STA3 8
-#define STA4 9
-#define STA5 10
-
-#define BEACON_ANGLE_OFFSET 449
-
-static volatile uint8_t opp_age = 0;
-static volatile int16_t opp_a = I2C_OPPONENT_NOT_THERE;
-static volatile int16_t opp_d = I2C_OPPONENT_NOT_THERE;
-
-static volatile uint8_t pos_age = 0;
-static volatile int16_t pos_x = I2C_BEACON_NOT_FOUND;
-static volatile int16_t pos_y = I2C_BEACON_NOT_FOUND;
-static volatile int16_t pos_a = I2C_BEACON_NOT_FOUND;
-
-#define BEACON_POS_OFFSET (-50.)
-int8_t beacon_get_pos_double(double *x, double *y, double *a_rad)
-{
- uint8_t flags;
- int16_t tmpx, tmpy, tmpa;
- double dtmpx, dtmpy, dtmpa;
-
- IRQ_LOCK(flags);
- tmpx = beaconboard.posx;
- tmpy = beaconboard.posy;
- tmpa = beaconboard.posa;
- IRQ_UNLOCK(flags);
-
- if (tmpx == I2C_BEACON_NOT_FOUND)
- return -1;
-
- dtmpx = tmpx;
- dtmpy = tmpy;
- dtmpa = RAD((double)tmpa / 10.);
-
- dtmpx += cos(dtmpa) * BEACON_POS_OFFSET;
- dtmpx += sin(dtmpa) * BEACON_POS_OFFSET;
-
- *x = dtmpx;
- *y = dtmpy;
- *a_rad = dtmpa;
- return 0;
-}
-
-#ifndef HOST_VERSION
-static void beacon_uart_cb(char c)
-{
- static uint8_t state;
- static uint16_t tmp_opp_d, tmp_opp_a;
- static uint16_t x, y, a;
-
- /* init command */
- if ((c & 0x80) == 0)
- state = INIT;
-
- switch (state) {
- case INIT:
- /* recv opp */
- if (c == 0) {
- state = OPP0;
- tmp_opp_d = 0;
- tmp_opp_a = 0;
- }
- /* recv opp */
- else if (c == 1) {
- state = STA0;
- x = 0;
- y = 0;
- a = 0;
- }
- break;
- case OPP0:
- tmp_opp_d = ((uint16_t)c) & 0x7F;
- state = OPP1;
- break;
- case OPP1:
- tmp_opp_d |= (((uint16_t)c << 7) & 0x3F80);
- state = OPP2;
- break;
- case OPP2:
- tmp_opp_a = ((uint16_t)c) & 0x7F;
- state = OPP3;
- break;
- case OPP3:
- tmp_opp_a |= (((uint16_t)c << 7) & 0x3F80);
- opp_a = tmp_opp_a;
- opp_d = tmp_opp_d;
- opp_age = 0;
- state = INIT;
- break;
- case STA0:
- x = ((uint16_t)c) & 0x7F;
- state = STA1;
- break;
- case STA1:
- x |= (((uint16_t)c << 7) & 0x3F80);
- state = STA2;
- break;
- case STA2:
- y = ((uint16_t)c) & 0x7F;
- state = STA3;
- break;
- case STA3:
- y |= (((uint16_t)c << 7) & 0x3F80);
- state = STA4;
- break;
- case STA4:
- a = ((uint16_t)c) & 0x7F;
- state = STA5;
- break;
- case STA5:
- a |= (((uint16_t)c << 7) & 0x3F80);
- pos_x = x;
- pos_y = y;
- pos_a = a;
- pos_age = 0;
- state = INIT;
- break;
- default:
- state = INIT;
- break;
- }
-}
-#endif
-
-static void beacon_opponent_event(void)
-{
-#ifdef HOST_VERSION
- uint8_t flags;
- int16_t oppx, oppy;
- double oppa, oppd;
-
- IRQ_LOCK(flags);
- if (beaconboard.oppx == I2C_OPPONENT_NOT_THERE) {
- IRQ_UNLOCK(flags);
- return;
- }
- oppx = beaconboard.oppx;
- oppy = beaconboard.oppy;
- abs_xy_to_rel_da(oppx, oppy, &oppd, &oppa);
- beaconboard.oppa = DEG(oppa);
- if (beaconboard.oppa < 0)
- beaconboard.oppa += 360;
- beaconboard.oppd = oppd;
- IRQ_UNLOCK(flags);
-#else
- uint8_t flags;
- double fd, fa, fx, fy;
- int16_t id, ia, ix, iy;
-
- /* if beacon is too old, remove it */
- IRQ_LOCK(flags);
- if (opp_age < 50)
- opp_age ++;
- else {
- beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
- IRQ_UNLOCK(flags);
- return;
- }
-
- ia = opp_a;
- id = opp_d;
- IRQ_UNLOCK(flags);
-
- ia = (ia + BEACON_ANGLE_OFFSET);
- if (ia > 3600)
- ia -= 3600;
- fa = ia;
- fa = RAD(fa);
- fd = id;
- rel_da_to_abs_xy(fd, fa, &fx, &fy);
-
- ix = fx;
- iy = fy;
-
- IRQ_LOCK(flags);
- beaconboard.oppx = ix;
- beaconboard.oppy = iy;
- beaconboard.oppa = ia / 10;
- beaconboard.oppd = id;
- IRQ_UNLOCK(flags);
-#endif
-}
-
-static void beacon_static_event(void)
-{
- uint8_t flags;
-
- /* if beacon is too old, remove it */
- IRQ_LOCK(flags);
- if (pos_age < 3)
- pos_age ++;
- else {
- beaconboard.posx = I2C_BEACON_NOT_FOUND;
- IRQ_UNLOCK(flags);
- return;
- }
-
- beaconboard.posx = pos_x;
- beaconboard.posy = pos_y;
- beaconboard.posa = pos_a;
- IRQ_UNLOCK(flags);
-}
-
-
-static void beacon_event(void *dummy)
-{
- beacon_opponent_event();
- beacon_static_event();
-}
-
-void beacon_set_color(uint8_t color)
-{
- uart_send(BEACON_UART_NUM, color);
-}
-
-void beacon_init(void)
-{
-#ifndef HOST_VERSION
- uart_register_rx_event(BEACON_UART_NUM, beacon_uart_cb);
-#endif
- scheduler_add_periodical_event_priority(beacon_event, NULL,
- 100000L / SCHEDULER_UNIT,
- BEACON_PRIO);
-}
+++ /dev/null
-/*
- * Copyright Droids Corporation (2010)
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Revision : $Id: strat.h,v 1.7 2009-11-08 17:24:33 zer0 Exp $
- *
- */
-
-void beacon_init(void);
-void beacon_set_color(uint8_t color);
-int8_t beacon_get_pos_double(double *x, double *y, double *a_rad);
extern parse_pgm_inst_t cmd_spi_test;
extern parse_pgm_inst_t cmd_opponent;
extern parse_pgm_inst_t cmd_opponent_set;
-extern parse_pgm_inst_t cmd_beacon;
extern parse_pgm_inst_t cmd_start;
extern parse_pgm_inst_t cmd_interact;
extern parse_pgm_inst_t cmd_color;
extern parse_pgm_inst_t cmd_time_monitor;
extern parse_pgm_inst_t cmd_strat_event;
extern parse_pgm_inst_t cmd_sleep;
+extern parse_pgm_inst_t cmd_beacon_start;
extern parse_pgm_inst_t cmd_test;
/* commands_traj.c */
(parse_pgm_inst_t *)&cmd_spi_test,
(parse_pgm_inst_t *)&cmd_opponent,
(parse_pgm_inst_t *)&cmd_opponent_set,
- (parse_pgm_inst_t *)&cmd_beacon,
(parse_pgm_inst_t *)&cmd_start,
(parse_pgm_inst_t *)&cmd_interact,
(parse_pgm_inst_t *)&cmd_color,
(parse_pgm_inst_t *)&cmd_time_monitor,
(parse_pgm_inst_t *)&cmd_strat_event,
(parse_pgm_inst_t *)&cmd_sleep,
+ (parse_pgm_inst_t *)&cmd_beacon_start,
(parse_pgm_inst_t *)&cmd_test,
/* commands_traj.c */
#include "strat_corn.h"
#include "i2c_protocol.h"
#include "actuator.h"
-#include "beacon.h"
struct cmd_event_result {
fixed_string_t arg0;
};
/**********************************************************/
-/* Beacon tests */
+/* Beacon_Start */
-/* this structure is filled when cmd_beacon is parsed successfully */
-struct cmd_beacon_result {
+/* this structure is filled when cmd_beacon_start is parsed successfully */
+struct cmd_beacon_start_result {
fixed_string_t arg0;
fixed_string_t arg1;
};
-/* function called when cmd_beacon is parsed successfully */
-static void cmd_beacon_parsed(void *parsed_result, void *data)
+/* function called when cmd_beacon_start is parsed successfully */
+static void cmd_beacon_start_parsed(void *parsed_result, void *data)
{
- double x, y, a;
+ struct cmd_beacon_start_result *res = parsed_result;
- if (beacon_get_pos_double(&x, &y, &a) < 0)
- printf_P(PSTR("No position from beacon\r\n"));
+ if (!strcmp_P(res->arg1, PSTR("start")))
+ i2c_ballboard_set_beacon(1);
else
- printf_P(PSTR("x=%2.2f y=%2.2f a=%2.2f\r\n"), x, y, DEG(a));
+ i2c_ballboard_set_beacon(0);
}
-prog_char str_beacon_arg0[] = "beacon";
-parse_pgm_token_string_t cmd_beacon_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_result, arg0, str_beacon_arg0);
-prog_char str_beacon_arg1[] = "show";
-parse_pgm_token_string_t cmd_beacon_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_result, arg1, str_beacon_arg1);
+prog_char str_beacon_start_arg0[] = "beacon";
+parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0);
+prog_char str_beacon_start_arg1[] = "start#stop";
+parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1);
-prog_char help_beacon[] = "Show (x,y) beacon";
-parse_pgm_inst_t cmd_beacon = {
- .f = cmd_beacon_parsed, /* function to call */
+prog_char help_beacon_start[] = "Beacon enabled/disable";
+parse_pgm_inst_t cmd_beacon_start = {
+ .f = cmd_beacon_start_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
- .help_str = help_beacon,
+ .help_str = help_beacon_start,
.tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_beacon_arg0,
- (prog_void *)&cmd_beacon_arg1,
+ (prog_void *)&cmd_beacon_start_arg0,
+ (prog_void *)&cmd_beacon_start_arg1,
NULL,
},
};
mainboard.our_color = I2C_COLOR_YELLOW;
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
- beacon_set_color(I2C_COLOR_YELLOW);
}
else if (!strcmp_P(res->color, PSTR("blue"))) {
mainboard.our_color = I2C_COLOR_BLUE;
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
- beacon_set_color(I2C_COLOR_BLUE);
}
+ printf_P(PSTR("Press a key when beacon ready\r\n"));
+ i2c_ballboard_set_beacon(0);
+ while(!cmdline_keypressed());
+ i2c_ballboard_set_beacon(1);
+
strat_start();
gen.logs[NB_LOGS] = 0;
mainboard.our_color = I2C_COLOR_YELLOW;
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
- beacon_set_color(I2C_COLOR_YELLOW);
}
else if (!strcmp_P(res->color, PSTR("blue"))) {
mainboard.our_color = I2C_COLOR_BLUE;
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
- beacon_set_color(I2C_COLOR_BLUE);
}
printf_P(PSTR("Done\r\n"));
#endif
#include "strat_db.h"
#include "../common/i2c_commands.h"
#include "i2c_protocol.h"
-#include "beacon.h"
/**********************************************************/
/* Traj_Speeds for trajectory_manager */
#ifndef HOST_VERSION
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
- beacon_set_color(I2C_COLOR_YELLOW);
#endif
auto_position();
}
#ifndef HOST_VERSION
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
- beacon_set_color(I2C_COLOR_BLUE);
#endif
auto_position();
}
tmp = ans->rcob;
if (tmp != I2C_COB_NONE)
ballboard.rcob = tmp;
+ ballboard.opponent_x = ans->opponent_x;
+ ballboard.opponent_y = ans->opponent_y;
+ ballboard.opponent_a = ans->opponent_a;
+ ballboard.opponent_d = ans->opponent_d;
break;
}
return i2c_send_command(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
}
+int8_t i2c_ballboard_set_beacon(uint8_t enable)
+{
+ struct i2c_cmd_ballboard_start_beacon buf;
+ buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_BEACON;
+ buf.enable = enable;
+ return i2c_send_command(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+
int8_t i2c_cobboard_deploy_nomove(uint8_t side);
int8_t i2c_ballboard_set_mode(uint8_t mode);
+int8_t i2c_ballboard_set_beacon(uint8_t enable);
#endif
#include "strat_db.h"
#include "strat_avoid.h"
#include "i2c_protocol.h"
-#include "beacon.h"
/* 0 means "programmed"
struct mainboard mainboard;
volatile struct cobboard cobboard;
volatile struct ballboard ballboard;
-volatile struct beaconboard beaconboard;
#ifndef HOST_VERSION
/***********************/
ballboard.lcob = I2C_COB_NONE;
ballboard.rcob = I2C_COB_NONE;
- beaconboard.oppx = I2C_OPPONENT_NOT_THERE;
- beaconboard.posx = I2C_BEACON_NOT_FOUND;
-
/* UART */
uart_init();
uart_register_rx_event(CMDLINE_UART, emergency);
/* sensors, will also init hardware adc */
sensor_init();
- /* beacon */
- beacon_init();
-
#ifndef HOST_VERSION
/* start i2c slave polling */
scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL,
uint8_t ball_count;
uint8_t lcob;
uint8_t rcob;
-};
-
-/* state of beaconboard, sync'd through uart */
-struct beaconboard {
- int16_t oppx;
- int16_t oppy;
- int16_t oppa;
- int16_t oppd;
- uint16_t posx;
- uint16_t posy;
- uint16_t posa; /* between 0 and 3600 */
+ int16_t opponent_x;
+ int16_t opponent_y;
+ int16_t opponent_a;
+ int16_t opponent_d;
};
extern struct genboard gen;
extern struct mainboard mainboard;
extern volatile struct cobboard cobboard;
extern volatile struct ballboard ballboard;
-extern volatile struct beaconboard beaconboard;
/* start the bootloader */
void bootloader(void);
return I2C_COLOR_YELLOW;
}
-/* get the da pos of the opponent robot */
-int8_t get_opponent_da(int16_t *d, int16_t *a)
+/* get the xy pos of the opponent robot */
+int8_t get_opponent_xy(int16_t *x, int16_t *y)
{
uint8_t flags;
- int16_t x;
-
IRQ_LOCK(flags);
- *d = beaconboard.oppd;
- *a = beaconboard.oppa;
- x = beaconboard.oppx;
+ *x = ballboard.opponent_x;
+ *y = ballboard.opponent_y;
IRQ_UNLOCK(flags);
- if (x == I2C_OPPONENT_NOT_THERE)
+ if (*x == I2C_OPPONENT_NOT_THERE)
return -1;
return 0;
}
-/* get the xy pos of the opponent robot */
-int8_t get_opponent_xy(int16_t *x, int16_t *y)
+/* 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 = beaconboard.oppx;
- *y = beaconboard.oppy;
+ x_tmp = ballboard.opponent_x;
+ *d = ballboard.opponent_d;
+ *a = ballboard.opponent_a;
IRQ_UNLOCK(flags);
- if (*x == I2C_OPPONENT_NOT_THERE)
+ if (x_tmp == I2C_OPPONENT_NOT_THERE)
return -1;
-
return 0;
}
int8_t get_opponent_xyda(int16_t *x, int16_t *y, int16_t *d, int16_t *a)
{
uint8_t flags;
-
IRQ_LOCK(flags);
- *x = beaconboard.oppx;
- *y = beaconboard.oppy;
- *d = beaconboard.oppd;
- *a = beaconboard.oppa;
+ *x = ballboard.opponent_x;
+ *y = ballboard.opponent_y;
+ *d = ballboard.opponent_d;
+ *a = ballboard.opponent_a;
IRQ_UNLOCK(flags);
if (*x == I2C_OPPONENT_NOT_THERE)
return -1;
-
return 0;
}
git.droids-corp.org / aversive.git / commitdiff