ORTEPublicationSend(orte->publication_robot_bumpers);
break;
- /* positioning by odometry */
+ /* positioning by independent odometry */
case CAN_ODO_DATA:
orte->odo_data.right =
((frame.data[0]<<24)|
ORTEPublicationSend(orte->publication_odo_data);
break;
- /* positioning by odometry */
+ /* positioning by odometry from motors*/
case CAN_MOTION_ODOMETRY_SIMPLE:
orte->motion_irc.left =
((frame.data[0]<<24)|
#define CAN_JAW_OUT_OF_BOUNDS 0x04
//flags sent in CAN_SWITCHES
-#define CAN_SWITCH_COLOR 2
+#define CAN_SWITCH_COLOR_0 2
#define CAN_SWITCH_STRATEGY 4
+#define CAN_SWITCH_COLOR_1 8
+#define CAN_SWITCH_HOME 16
//flags sent in CAN_ROBOT_BUMPERS
#define CAN_BUMPER_REAR_LEFT 1
#define CAN_SPEED 1000000 //< CAN bus speed
#define CAN_ISR 0
+#define START_SEND_PRIOD_FAST 50 /* [miliseconds] */
+#define START_SEND_PRIOD_SLOW 300 /* [miliseconds] */
+#define START_SEND_FAST_COUNT 10 /* How many times to send start with small period (after a change) */
+
#define ADC_ISR 1
-#define CL_SENSOR_IN 16
-#define CL_SENSOR_OUT EXPPORT_3
+#define CL_SENSOR_IN 16 //port for sending command to color sensor(via power switch)
#define JAW_LEFT 0
#define JAW_RIGHT 1
#define TIMER_IRQ_PRIORITY 5
-#define BUMPER_LEFT EXPPORT_5
-#define BUMPER_RIGHT EXPPORT_4
+#define BUMPER_LEFT GPIOPORT_8
+#define BUMPER_RIGHT GPIOPORT_6
-#define BUMPER_LEFT_ACROSS EXPPORT_6
-#define BUMPER_RIGHT_ACROSS EXPPORT_8
+#define BUMPER_LEFT_ACROSS GPIOPORT_3
+#define BUMPER_RIGHT_ACROSS GPIOPORT_1
-#define BUMPER_REAR_LEFT EXPPORT_7
-#define BUMPER_REAR_RIGHT EXPPORT_1
+#define CL_SENSOR_OUT/*BUMPER_REAR_LEFT*/ GPIOPORT_7 //port for receiving data from color sensor used to be BUMPER_REAR_LEFT
+#define BUMPER_REAR GPIOPORT_4
-#define COLOR_PIN GPIOPORT_1
-#define SWITCH_STRATEGY_PIN GPIOPORT_2
+#define COLOR_PIN0 EXPPORT_4
+#define COLOR_PIN1 EXPPORT_5
+#define SWITCH_STRATEGY_PIN EXPPORT_8
+#define START_PIN EXPPORT_7
+#define SWITCH_HOME_PIN EXPPORT_6
struct fsm fsm_jaw_right;
VICIntEnable = 1<<source;
}
+void start_button(void)
+{
+ can_msg_t msg;
+ bool start_condition;
+ static bool last_start_condition = 0;
+
+ static int count = 0;
+ static uint32_t next_send = 0;
+
+
+ start_condition = (IO0PIN & (1<<START_PIN)) == 0;
+
+ if (start_condition != last_start_condition) {
+
+
+ last_start_condition = start_condition;
+ count = 0;
+ next_send = timer_msec; /* Send now */
+
+ }
+
+ if (timer_msec >= next_send) {
+ msg.id = CAN_ROBOT_CMD;
+ msg.flags = 0;
+ msg.dlc = 1;
+ msg.data[0] = start_condition;
+
+// send_rs_str("start\n");
+
+ /*while*/ (can_tx_msg(&msg));
+
+ if (count < START_SEND_FAST_COUNT) {
+ count++;
+ next_send = timer_msec + START_SEND_PRIOD_FAST;
+ } else
+ next_send = timer_msec + START_SEND_PRIOD_SLOW;
+ }
+
+
+}
+
+
void robot_switches_handler()
{
static uint32_t color_time = 0;
color_time = timer_msec;
- if (IO1PIN & (1<<COLOR_PIN))
- sw |= CAN_SWITCH_COLOR;
+ if (IO0PIN & (1<<COLOR_PIN0))
+ sw |= CAN_SWITCH_COLOR_0;
else
- sw &= ~CAN_SWITCH_COLOR;
+ sw &= ~CAN_SWITCH_COLOR_0;
+
+ if (IO0PIN & (1<<COLOR_PIN1))
+ sw |= CAN_SWITCH_COLOR_1;
+ else
+ sw &= ~CAN_SWITCH_COLOR_1;
- if (IO1PIN & (1<<SWITCH_STRATEGY_PIN))
+ if (IO0PIN & (1<<SWITCH_STRATEGY_PIN))
sw &= ~CAN_SWITCH_STRATEGY;
else
sw |= CAN_SWITCH_STRATEGY;
- if (sw & CAN_SWITCH_COLOR){
+ if (IO0PIN & (1<<SWITCH_HOME_PIN))
+ sw &= ~CAN_SWITCH_HOME;
+ else
+ sw |= CAN_SWITCH_HOME;
+
+ if ((sw & CAN_SWITCH_COLOR_0) && (sw & CAN_SWITCH_COLOR_1)){
deb_led_off(LEDY);
- send_rs_str("color\n");}
+ send_rs_str("white\n");}
+ else
+ deb_led_on(LEDY);
+
+ if ((sw & CAN_SWITCH_COLOR_0) && !(sw & CAN_SWITCH_COLOR_1)){
+ deb_led_off(LEDY);
+
+ send_rs_str("green\n");}
+ else
+ deb_led_on(LEDY);
+
+ if (!(sw & CAN_SWITCH_COLOR_0) && (sw & CAN_SWITCH_COLOR_1)){
+ deb_led_off(LEDY);
+
+ send_rs_str("yellow\n");}
else
deb_led_on(LEDY);
}
else
deb_led_on(LEDY);
+ if (sw & CAN_SWITCH_HOME){
+ deb_led_off(LEDY);
+
+ send_rs_str("home\n");
+ }
+ else
+ deb_led_on(LEDY);
// send_rs_int(IO1PIN);
// send_rs_int(sw);
tx_msg.dlc = 1;
tx_msg.flags = 0;
tx_msg.data[0] = sw;
-
can_tx_msg(&tx_msg);
}
}
set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
init_uart();
- init_adc(ADC_ISR);
+ //init_adc(ADC_ISR);
init_servo(7); //7 is interrupt priority
set_servo(JAW_LEFT, 0xB0);
}
+void cl_sensor_send_status(){
+ can_msg_t tx_msg;
+ tx_msg.id = CAN_CL_SENSOR_STATUS;
+ tx_msg.dlc = 5;
+ tx_msg.flags = 0;
+ tx_msg.data[0] = ((IO1PIN & 1<<CL_SENSOR_OUT) != 0) & 0xFF;// 1 - pattern match 0 - no match
+ if((IO1PIN & 1<<CL_SENSOR_OUT) != 0) send_rs_str("trefa\n");
+ (can_tx_msg(&tx_msg));
+}
+
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
- if (IO0PIN & (1<<BUMPER_REAR_LEFT)){
+ /*if (IO1PIN & (1<<BUMPER_REAR_LEFT)){
sw &= ~CAN_BUMPER_REAR_LEFT;
send_rs_str("rear_left\n");
sw |= CAN_BUMPER_REAR_LEFT;
// deb_led_off(LEDY);
- }
+ }*/
- if (IO0PIN & (1<<BUMPER_REAR_RIGHT)){
+ if (IO1PIN & (1<<BUMPER_REAR)){
sw &= ~CAN_BUMPER_REAR_RIGHT;
-
- send_rs_str("rear_right\n");
+ sw &= ~CAN_BUMPER_REAR_LEFT;
+ send_rs_str("reart\n");
// deb_led_on(LEDY);
}
else{
sw |= CAN_BUMPER_REAR_RIGHT;
+ sw |= CAN_BUMPER_REAR_LEFT;
// deb_led_off(LEDY);
}
- if (IO0PIN & (1<<BUMPER_LEFT)){
+ if (IO1PIN & (1<<BUMPER_LEFT)){
sw &= ~CAN_BUMPER_LEFT;
// deb_led_off(LEDB);
}
- if (IO0PIN & (1<<BUMPER_RIGHT)){
+ if (IO1PIN & (1<<BUMPER_RIGHT)){
sw &= ~CAN_BUMPER_RIGHT;
send_rs_str("right\n");
// deb_led_on(LEDG);
// deb_led_off(LEDG);
}
- if (IO0PIN & (1<<BUMPER_LEFT_ACROSS)){
+ if (IO1PIN & (1<<BUMPER_LEFT_ACROSS)){
sw &= ~CAN_BUMPER_LEFT_ACROSS;
send_rs_str("left_across\n");
// deb_led_on(LEDR);
// deb_led_off(LEDR);
}
- if (IO0PIN & (1<<BUMPER_RIGHT_ACROSS)){
+ if (IO1PIN & (1<<BUMPER_RIGHT_ACROSS)){
sw &= ~CAN_BUMPER_RIGHT_ACROSS;
send_rs_str("right_across\n");
// deb_le0d_on(LEDY);
// send_rs_int(IO1PIN);
// send_rs_int(sw);
// send_rs_str("\n");
-//
-
tx_msg.id = CAN_ROBOT_BUMPERS;
tx_msg.dlc = 1;
tx_msg.flags = 0;
- tx_msg.data[0] = sw;
-
+ tx_msg.data[0] = sw;
can_tx_msg(&tx_msg);
}
}
void cl_sensor_init(){
IO0DIR |= 1<<CL_SENSOR_IN;
- IO0DIR &= ~(1<<CL_SENSOR_OUT);
+ IO1DIR &= ~(1<<CL_SENSOR_OUT);
IO0SET = (1<<CL_SENSOR_IN);
option = 3;
option_new = 0;
uint32_t status_time_left = timer_usec;
uint32_t status_time_right = timer_usec;
uint32_t status_time_sensor = timer_msec;
- bool feedback;
+ //bool feedback;
init_periphery();
- SET_PIN(PINSEL1, 1, PINSEL_0);
- SET_PIN(PINSEL1, 2, PINSEL_0);
- SET_PIN(PINSEL1, 3, PINSEL_0);
- SET_PIN(PINSEL0, BUMPER_REAR_LEFT, PINSEL_0);
- SET_PIN(PINSEL0, BUMPER_REAR_RIGHT, PINSEL_0);
- SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
+ SET_PIN(PINSEL0, COLOR_PIN0, PINSEL_0);
+ SET_PIN(PINSEL0, START_PIN, PINSEL_0);
+ SET_PIN(PINSEL1, (SWITCH_STRATEGY_PIN-16), PINSEL_0);
+ SET_PIN(PINSEL1, (COLOR_PIN1-16), PINSEL_0);
+ SET_PIN(PINSEL1, (SWITCH_HOME_PIN - 16), PINSEL_0);
+
+ PINSEL2 &= ~(1 << 3) ; //setting P1.25:16 as GPIO pins -> GPIO PORT on eb_board
+ IO1DIR &= ~((1<<BUMPER_REAR) | /*(1<<BUMPER_REAR_LEFT) |*/ (1<<BUMPER_RIGHT) | (1<<BUMPER_LEFT) | (1<<BUMPER_RIGHT_ACROSS) | (1<<BUMPER_LEFT_ACROSS));
+ IO0DIR &= ~((1<<COLOR_PIN0) | (1<<COLOR_PIN1)| (1<<SWITCH_HOME_PIN) | (1<<START_PIN) | (1<<SWITCH_STRATEGY_PIN));
+ cl_sensor_init();
+
-// IO0DIR &= ~((1<<BUMPER_REAR) | (1<<BUMPER_RIGHT) | (1<<BUMPER_LEFT) | (1<<BUMPER_RIGHT_ACROSS) | (1<<BUMPER_LEFT_ACROSS));
- PINSEL2 &= ~(1 << 3) ;
- IO1DIR &= ~((1<<COLOR_PIN) | (1<<SWITCH_STRATEGY_PIN));
- cl_sensor_init();
send_rs_str("Jaws started\n");
//uint32_t i = 5000000;
// uint32_t j;
// while(i--) for(j = 0; j < 10; j++);
//close jaw, max_pos, open jaw min_pos
//letf jaw potenciometer on ADC 0, header J32 on board
- fsm_jaw_left.max_pos=0x320; //max 0x324
- fsm_jaw_left.min_pos=0xB0; //min 0xC3
- fsm_jaw_left.act_pos = adc_val[0];
+ //fsm_jaw_left.max_pos=0x320; //max 0x324
+ //fsm_jaw_left.min_pos=0xB0; //min 0xC3
+ //fsm_jaw_left.act_pos = adc_val[0];
//close jaw, max_pos, open jaw min_pos
//letf jaw potenciometer on ADC 1, header J33 on board
- fsm_jaw_right.max_pos=0x380; //max 0x382
- fsm_jaw_right.min_pos=0x010; //min 0xF5
- fsm_jaw_right.act_pos = adc_val[1];
+ //fsm_jaw_right.max_pos=0x380; //max 0x382
+ //fsm_jaw_right.min_pos=0x010; //min 0xF5
+ //fsm_jaw_right.act_pos = adc_val[1];
//left jaw engine is engine A, conector MOTA on board
- fsm_jaw_left.engine_en = &engine_A_en;
- fsm_jaw_left.engine_dir = &engine_A_dir;
- fsm_jaw_left.engine_pwm = &engine_A_pwm;
+ //fsm_jaw_left.engine_en = &engine_A_en;
+ //fsm_jaw_left.engine_dir = &engine_A_dir;
+ //fsm_jaw_left.engine_pwm = &engine_A_pwm;
fsm_jaw_left.can_id=CAN_JAW_LEFT_STATUS;
//right jaw engine is engine B, conector MOTB on board
- fsm_jaw_right.engine_en = &engine_B_en;
- fsm_jaw_right.engine_dir = &engine_B_dir;
- fsm_jaw_right.engine_pwm = &engine_B_pwm;
+ //fsm_jaw_right.engine_en = &engine_B_en;
+ //fsm_jaw_right.engine_dir = &engine_B_dir;
+ //fsm_jaw_right.engine_pwm = &engine_B_pwm;
fsm_jaw_right.can_id=CAN_JAW_RIGHT_STATUS;
+
- init_fsm(&fsm_jaw_right, &fsm_jaw_init);
- init_fsm(&fsm_jaw_left, &fsm_jaw_init);
+// init_fsm(&fsm_jaw_right, &fsm_jaw_init);
+// init_fsm(&fsm_jaw_left, &fsm_jaw_init);
/* test_vhn(); */
/* return; */
while(1){
+ //run fsms
if(timer_usec >= main_time + 1000){
main_time = timer_usec;
//dbg_print_time();
//fsm_jaw_left.act_pos = adc_val[0];
- //run_fsm(&fsm_jaw_left);
+// run_fsm(&fsm_jaw_left);
//fsm_jaw_right.act_pos = adc_val[1];
- //run_fsm(&fsm_jaw_right);
+// run_fsm(&fsm_jaw_right);
}
+ //R/W color sensor
if(timer_msec >= status_time_sensor + 10){
status_time_sensor = timer_msec;
if(cl_sensor_timer < timer_msec){
}
}
}
+ cl_sensor_send_status();
// feedback = (IO0PIN & 1<<CL_SENSOR_OUT) != 0;
// if(feedback) send_rs_str("shoda barev\n");
// else send_rs_str("jina barva\n");
}
+ //CAN send left jaw status
if (timer_msec >= status_time_left + 100 /*|| //repeat sending message every 100 ms
fsm_jaw_left.trigger_can_send*/) { //or when something important happen
//fsm_jaw_left.trigger_can_send = false;
status_time_left = timer_msec; //save new time, when message was sent
- // can_send_status(&fsm_jaw_left);
+ can_send_status(&fsm_jaw_left);
}
-
+// CAN send left jaw status
if (timer_msec >= status_time_right + 100 /*|| //repeat sending message every 100 ms
fsm_jaw_right.trigger_can_send*/) { //or when something important happen
//fsm_jaw_right.trigger_can_send = false;
status_time_right = timer_msec; //save new time, when message was sent
- // can_send_status(&fsm_jaw_right);
+ can_send_status(&fsm_jaw_right);
- }
-
+ }
+ start_button();
handle_bumper();
robot_switches_handler();
blink_led();
int ShmapIsFreeCell(int x, int y)
{
struct map_cell *cell;
- if(map && ShmapIsCellInMap(x,y)) {
- bool free;
+
+ if (map && ShmapIsCellInMap(x,y)) {
+ bool free;
cell = &(map->cells[y][x]);
- free = (cell->detected_obstacle == 0) &&
- (((cell->flags & (MAP_FLAG_WALL|MAP_FLAG_DET_OBST|MAP_FLAG_PLAN_MARGIN)) == 0) ||
- ((cell->flags & MAP_FLAG_WALL) && (cell->flags & MAP_FLAG_INVALIDATE_WALL)));
- return free;
+ free = ((cell->detected_obstacle == 0) &&
+ ((cell->flags & (MAP_FLAG_WALL | MAP_FLAG_DET_OBST | MAP_FLAG_PLAN_MARGIN)) == 0));
+ return free;
}
else return -1;
}
/** "Safety margin" around obstacle - used only during A* planning and
* not during runtime obstacle avoidance. */
#define MAP_FLAG_PLAN_MARGIN 128
-#define MAP_FLAG_INVALIDATE_WALL 256 /**< Area, where the wall should be forgotten */
/** @}*/
/** @name Shared Memory macros */
#include "robodim.h"
#include <robomath.h>
-
-/*
- * Beacon positions. For some version of MCL, this must be sorted in
- * the same order as laser rotation.
- *
- * (pecam1, 2009: not sure, if the sense is right for laser MCL;
- * anyway, I hope, we will be happy without the laser)
- */
-
-const struct bonus_pos bonus[BONUS_CNT] = { //bonus[1].x
- {975, 1575},
- {975, 875},
- {1325, 175},
- {1675, 175},
- {2025, 1575},
- {2025, 875},
-};
-
-const struct pawn_pos pawn[PAWN_CNT] = {
- {800, 1400}, // center
- {800, 350},
- {1150, 1750},
- {1150, 350},
- {1500, 1050},
- {1850, 1750},
- {1850, 350},
- {2200, 1400},
- {2200, 350},
- {200, 290}, // left dispensing
- {200, 290 + 1*280},
- {200, 290 + 2*280},
- {2800, 290}, // right dispensing
- {2800, 290 + 1*280},
- {2800, 290 + 2*280},
-};
-
-const struct queen_pos queen[QUEEN_CNT] = {
- {200, 290 + 3*280},
- {2800, 290 + 3*280},
-};
-
-const struct king_pos king[KING_CNT] = {
- {200, 290 + 4*280},
- {2800, 290 + 4*280},
-};
-
-const struct square_center_red red_sq[SQ_CNTR] = {
- {0.45 + 0.175 + 5*0.35, 5*0.35 + 0.175}, // 1
- {0.45 + 0.175 + 5*0.35, 3*0.35 + 0.175}, // 2
- {0.45 + 0.175 + 5*0.35, 1*0.35 + 0.175}, // 3
- {0.45 + 0.175 + 4*0.35, 4*0.35 + 0.175}, // 4
- {0.45 + 0.175 + 4*0.35, 2*0.35 + 0.175}, // 5
- {0.45 + 0.175 + 3*0.35, 5*0.35 + 0.175}, // 6
- {0.45 + 0.175 + 3*0.35, 3*0.35 + 0.175}, // 7
- {0.45 + 0.175 + 3*0.35, 1*0.35 + 0.175}, // 8
- {0.45 + 0.175 + 2*0.35, 4*0.35 + 0.175}, // 9
- {0.45 + 0.175 + 2*0.35, 2*0.35 + 0.175}, // 10
- {0.45 + 0.175 + 1*0.35, 5*0.35 + 0.175}, // 11
- {0.45 + 0.175 + 1*0.35, 3*0.35 + 0.175}, // 12
- {0.45 + 0.175 + 1*0.35, 1*0.35 + 0.175}, // 13
- {0.45 + 0.175 + 0*0.35, 4*0.35 + 0.175}, // 14
- {0.45 + 0.175 + 0*0.35, 2*0.35 + 0.175}, // 15
-};
-
-const struct square_center_blue blue_sq[SQ_CNTR] = {
- {0.45 + 0.175 + 0*0.35, 5*0.35 + 0.175}, // 1
- {0.45 + 0.175 + 0*0.35, 3*0.35 + 0.175}, // 2
- {0.45 + 0.175 + 0*0.35, 1*0.35 + 0.175}, // 3
- {0.45 + 0.175 + 1*0.35, 4*0.35 + 0.175}, // 4
- {0.45 + 0.175 + 1*0.35, 2*0.35 + 0.175}, // 5
- {0.45 + 0.175 + 2*0.35, 5*0.35 + 0.175}, // 6
- {0.45 + 0.175 + 2*0.35, 3*0.35 + 0.175}, // 7
- {0.45 + 0.175 + 2*0.35, 1*0.35 + 0.175}, // 8
- {0.45 + 0.175 + 3*0.35, 4*0.35 + 0.175}, // 9
- {0.45 + 0.175 + 3*0.35, 2*0.35 + 0.175}, // 10
- {0.45 + 0.175 + 4*0.35, 5*0.35 + 0.175}, // 11
- {0.45 + 0.175 + 4*0.35, 3*0.35 + 0.175}, // 12
- {0.45 + 0.175 + 4*0.35, 1*0.35 + 0.175}, // 13
- {0.45 + 0.175 + 5*0.35, 4*0.35 + 0.175}, // 14
- {0.45 + 0.175 + 5*0.35, 2*0.35 + 0.175}, // 15
-};
-// const struct beacon_pos beacon_green[BEACON_CNT] = {
-// { 3.062, -0.05}, /* EB2009: one side is 10mm only plexiglass */
-// {-0.062, 1.05}, /* the rest is 22mm wood */
-// { 3.062, 2.162},
-// };
-//
-// const struct beacon_pos beacon_red[BEACON_CNT] = {
-// /* beacons are rotated, not mirrored! */
-// {-0.062, 2.162},
-// { 3.062, 1.05},
-// {-0.062, -0.05},
-// };
-
-
-/*
- * Ukrizovavse pak jej, rozdelili roucha jeho, mecice o ne los, aby se
- * naplnilo povedeni proroka, rkouciho: Rozdelili sobe roucho me, a o muj
- * odev metali los. (Mt 27,35)
- *
- * And they crucified him, and parted his garments, casting lots: that it
- * might be fulfilled which was spoken by the prophet, They parted my garments
- * among them, and upon my vesture did they cast lots.
- *
- *
- * Mame dost mongoloidni souradnej system, vsimli jste si?
- * Inteligent by dal cervenej ctverec do 0,0. No, proletosek uz na to chmatat
- * nebudeme.
- */
/*
- * robodim_eb2010.h
+ * robodim.h
*
* Dimensions for the robot, playground and other stuff.
- * Eurobot 2010
+ * Demo application
*
* Copyright: (c) 2008 - 2010 CTU Dragons
* CTU FEE - Department of Control Engineering
* License: GNU GPL v.2
*/
-#ifndef ROBODIM_EB2010_H
-#define ROBODIM_EB2010_H
+#ifndef ROBODIMH
+#define ROBODIMH
/**
* FIXME: update robot's dimensions !!!
* and update the pitcture
*
- * ROBOT DIMENSIONS FOR COMPETITION 2011, Play Chess!
+ * ROBOT DIMENSIONS
+ *
+ * ^ +--------------------------+
+ * | | | | |
+ * ^ | | ------- |
+ * RR| | | : |
+ * v | | : __ |
+ * W| | + Center |__| |
+ * | | AB : AF HOK |
+ * | |<---->:<----------------->|
+ * | | ------- |
+ * | | | | |
+ * v +--------------------------+
+ * <-------> <-->
+ * V WR
*
- * ^ +--------------------------+@------\
- * | | | | | \
- * ^ | | ------- | .....
- * RR| | | HOK : | . .
- * v | | __ : | . .
- * W| | |__| |. .
- * | | AF : AB | . .
- * | |<---->:<----------------->| . .
- * | | ------- | .......
- * | | | | | /
- * v +--------------------------+@-------/
- * <<= <--> <--------->
- * direction WR JA
- * of motion <------------>
- * PW
*/
-/* FIXME update robot's dimensions!!! */
-#define ROBOT_WIDTH_MM 310.0 /* W*/
+#define ROBOT_WIDTH_MM 280 /* W*/
#define ROBOT_WIDTH_M (ROBOT_WIDTH_MM/1000.0)
#define ROBOT_ROTATION_RADIUS_MM ((230.0)/2.0) /* RR */
#define ROBOT_ROTATION_RADIUS_M (ROBOT_ROTATION_RADIUS_MM/1000.0)
#define SICK_CENTER_OFFSET_M (SICK_CENTER_OFFSET_MM/1000.0)
#define SICK_RANGE_ANGLE_LEFT 135.0 /* view angle in degrees from center axis */
#define SICK_RANGE_ANGLE_RIGHT 135.0
-#define SICK_ORIENTATION (-1) /* 1 = screws up, -1 = screws down */
+#define SICK_ORIENTATION (1) /* 1 = screws up, -1 = screws down */
#define ODOMETRY_WHEEL_RADIUS_MM 30.0
#define ODOMETRY_WHEEL_RADIUS_M (ODOMETRY_WHEEL_RADIUS_MM/1000.0)
#define ODOMETRY_ROTATION_RADIUS_MM (284.0/2.0)
#define ODOMETRY_ROTATION_RADIUS_M (ODOMETRY_ROTATION_RADIUS_MM/1000.0)
+#define ROBOT_DIAGONAL_RADIUS_MM sqrt((ROBOT_WIDTH_MM*ROBOT_WIDTH_MM/4) + (ROBOT_AXIS_TO_FRONT_MM*ROBOT_AXIS_TO_FRONT_MM))
+#define ROBOT_DIAGONAL_RADIUS_M (ROBOT_DIAGONAL_RADIUS_MM/1000.0)
#define ROBOT_JAWS_LENGHT_MM 130.0 /* JA */
#define ROBOT_JAWS_LENGHT_M (ROBOT_JAWS_LENGHT_MM/1000.0)
/**
* PLAYGROUND DIMENSIONS
- *
- * S2 R3
- * +---------------------------------+---------------------------------+
- * ^ | | [W,H]|
- * | | SL_T_R | |
- * | |<------------->| |
- * | | | |
- * | | |
- * | | |
- * | | |
- * | | |
- * H | R1| |S1
- * | | |
- * | | |
- * | | |
- * | | |
- * | | |
- * | | |
- * | | |
- * v |[0,0] |
- * +---------------------------------+---------------------------------+
- * S3 R2
- * <----------------------------------------------------------------->
- * W
*/
#define PLAYGROUND_WIDTH_MM 20000
#define PLAYGROUND_WIDTH_M (PLAYGROUND_WIDTH_MM/1000.0)
#define PLAYGROUND_HEIGHT_MM 20000
#define PLAYGROUND_HEIGHT_M (PLAYGROUND_HEIGHT_MM/1000.0)
-#define SLOPE_TO_RIM_MM 740
-#define SLOPE_TO_RIM_M (SLOPE_TO_RIM_MM/1000.0)
-#define SLOPE_LENGTH_MM 519.23
-#define SLOPE_LENGTH_M (SLOPE_LENGTH_MM/1000.0)
-
-#define SQUARE_WIDTH_MM 350
-#define SQUARE_HEIGHT_MM 350
-
-#define BLINE_WIDTH_MM 50
-#define BLINE_HEIGHT_MM 2100
-
-#define BLOCK_WIDTH_MM 400
-#define BLOCK_HEIGHT_MM 22
-
-#define STARTAREA_WIDTH_MM 500
-#define STARTAREA_HEIGHT_MM 500
-
-#define DISPENSING_WIDTH_MM 400
-#define DISPENSING_HEIGHT_MM 1678
-
-#define PROTECTEDBORDER_WIDTH_MM 22
-#define PROTECTEDBORDER_HEIGHT_MM 130
-
-#define PROTECTEDBLOCK_WIDTH_MM 700
-#define PROTECTEDBLOCK_HEIGHT_MM 120
-
-#define FIGURE_WIDTH_MM 200
-
-#define CORN_DIAMETER_MM 50
-#define CORN_DIAMETER_M (CORN_DIAMETER_MM/1000.0)
-#define CORN_RADIUS_MM (50/2)
-#define CORN_RADIUS_M (CORN_RADIUS_MM/1000.0)
-
-#define CORN_NEIGHBOURHOOD_RADIUS_MM 220
-#define CORN_NEIGHBOURHOOD_RADIUS_M (CORN_NEIGHBOURHOOD_RADIUS_MM/1000.0)
-/*
- * 3 ultrasonic beacons
+/**
+ * TARGET DIMENSIONS
*/
-#define BEACON_WIDTH 0.08
-#define BEACON_HEIGHT BEACON_WIDTH
-
-#define BEACON_GREEN 0
-#define BEACON_RED 1
-#define BEACON_CNT 3
-
-/* bonus squares */
-#define BONUS_CNT 6
-
-/* pawns */
-#define PAWN_CNT 15
-
-/* king */
-#define KING_CNT 2
-
-/* queen */
-#define QUEEN_CNT 2
-
-#define SQ_CNTR 15
+#define TARGET_RADIUS_MM 100
+#define TARGET_RADIUS_M (TARGET_RADIUS_MM/1000.0)
-
-struct beacon_pos {
- float x, y;
-};
-
-struct bonus_pos {
- int x, y;
-};
-
-struct pawn_pos {
- int x, y;
-};
-
-struct king_pos {
- int x, y;
-};
-
-struct queen_pos {
- int x, y;
-};
-
-struct square_center_red {
- float x, y;
-};
-
-struct square_center_blue {
- float x, y;
-};
-
-extern const struct beacon_pos beacon_green[BEACON_CNT];
-extern const struct beacon_pos beacon_red[BEACON_CNT];
-extern const struct bonus_pos bonus[BONUS_CNT];
-extern const struct pawn_pos pawn[PAWN_CNT];
-extern const struct king_pos king[KING_CNT];
-extern const struct queen_pos queen[QUEEN_CNT];
-
-extern const struct square_center_red red_sq[SQ_CNTR];
-extern const struct square_center_blue blue_sq[SQ_CNTR];
-
-/*
- * Position of Shapr sensors on the robot with respect to the robot center
- * (not used in EB2009)
- */
struct sharp_pos {
float x, y, ang;
};
-#define SHARP_COUNT 0 /* no sharp sensors in EB2009, I hope */
-
-#endif /* ROBODIM_EB2008_H */
+#endif /* ROBODIMH */
robot_config_DEFINES = CONFIG_LOCK_CHECKING HAVE_PRIO_INHERIT
bin_PROGRAMS += sick-day
-sick-day_SOURCES = sick-day.cc
+sick-day_SOURCES = sick-day.cc common-states.cc sub-states.cc
# Library with general support functions for the robot
lib_LIBRARIES += robot
act_lift(0, 0, 1);
}
+void act_camera_recognize(char* target_color)
+{
+ char i = 0;
+
+ orte->camera_control.on = 1;
+
+ /* send RGB color of target to recognize */
+ for (i; i < 2; i++) {
+ orte->camera_control.color[i] = &target_color[i];
+ }
+
+ ORTEPublicationSend(orte->publication_camera_control);
+}
+
void act_camera_on(void)
{
orte->camera_control.on = 1;
void act_camera_on(void);
void act_camera_off(void);
+void act_camera_recognize(char* target_color);
void act_lift(uint16_t req_pos, char speed, char homing);
void act_jaws(jaws_cmds cmd);
#define FSM_MAIN
-#include "robodata.h"
#include <robot.h>
#include <fsm.h>
#include <unistd.h>
#include <string.h>
#include <robodim.h>
#include <error.h>
-#include "actuators.h"
-#include <sharp.h>
#include <trgen.h>
-#include "match-timing.h"
#include <stdbool.h>
#include <ul_log.h>
+#include <shape_detect.h>
-UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
-
+#include "robodata.h"
+#include "actuators.h"
+#include "match-timing.h"
#include "common-states.h"
+#include "sub-states.h"
+
+UL_LOG_CUST(ulogd_common_states); /* Log domain name = ulogd + name of the file */
/************************************************************************
* Functions used in and called from all the (almost identical)
fsm->debug_name, robot_current_time(), \
name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
+/************************************************************************
+ * Trajectory constraints used; They are initialized in the main() function in competition.cc
+ ************************************************************************/
+
+struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
+
+/**
+ * Vector where all absolute positions of all detected targets are stored.
+ */
+std::vector<robot_pos_type> detected_target;
+
+/**
+ * Safe distance for target recognition
+ */
+const double approach_radius = TARGET_RADIUS_M + 2.0*MAP_CELL_SIZE_M + ROBOT_DIAGONAL_RADIUS_M;
void set_initial_position()
{
- robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
- 0);
+ robot_set_est_pos_trans(HOME_POS_X_M, HOME_POS_Y_M, DEG2RAD(HOME_POS_ANG_DEG));
}
void actuators_home()
{
- act_jaws(CLOSE);
-
-// bool pokus = robot.fsm.motion.state_name == "wait_and_try_again";
- // drive lift to home position
- act_lift(0, 0, 1);
- // unset the homing request
- //act_lift(0, 0, 0);
+ //act_crane(CRANE_UP);
+ //act_magnet(0);
}
-void start_entry()
+/* Check if the new point is within the playground scene */
+bool goal_is_in_playground(double goalx, double goaly)
{
- pthread_t thid;
- robot.check_turn_safety = false;
- pthread_create(&thid, NULL, timing_thread, NULL);
- start_timer();
+ if ((goalx < 0) || (goalx > PLAYGROUND_WIDTH_M) || (goaly < 0) || (goaly > PLAYGROUND_HEIGHT_M))
+ return false;
+ else
+ return true;
}
-// We set initial position periodically in order for it to be updated
-// on the display if the team color is changed during waiting for
-// start.
-void start_timer()
+/* Check if the new point is close to the robot */
+bool close_goal(double goalx, double goaly)
{
- set_initial_position();
- if (robot.start_state == START_PLUGGED_IN)
- actuators_home();
+ const double close = 0.5;
+ double x, y, phi;
+ robot_get_est_pos(&x, &y, &phi);
+
+ if ((abs(goalx - x) < close) && (abs(goaly - y) < close) )
+ return true;
+ else
+ return false;
}
-void start_go()
+/**
+ * Take data from hokuyo and run shape detection on it.
+ *
+ * Absolute positions of all detected targets centers are stored in alobal variable (vector).
+ *
+ * @return True if at least one target detected, else false.
+ */
+static bool detect_target()
{
- sem_post(&robot.start);
- actuators_home();
- set_initial_position();
-}
+ struct hokuyo_scan_type hokuyo = robot.hokuyo;
-void start_exit()
-{
+ Shape_detect sd;
+ std::vector<Shape_detect::Arc> arcs;
+ sd.prepare(hokuyo.data);
+ sd.arc_detect(arcs);
-}
+ // clear old targets
+ detected_target.clear();
-bool read_sharp()
-{
- int sharp_data = robot.orte.jaws_status.act_pos.left;
- int sharp_dist = 0;
- sharp_dist = s_gp2y0a21_ir2mmLong(sharp_data);
- printf("sharp data: %dmm\n", sharp_dist);
- return (sharp_dist <= 150 ? true : false);
-}
+ if (arcs.size() > 0) {
+ robot_pos_type e, target, hok;
-void inline enable_bumpers(bool enabled)
-{
- robot.use_left_bumper = enabled;
- robot.use_right_bumper = enabled;
- robot.use_back_bumpers = enabled;
-}
+ robot_get_est_pos(&e.x, &e.y, &e.phi);
+ double sinus = sin(e.phi);
+ double cosinus = cos(e.phi);
+ // save absolute positions of all detected targets
+ for (int i = 0; i < arcs.size(); i++) {
+ Shape_detect::Arc *a = &arcs[i];
-/************************************************************************
- * Trajectory constraints used; They are initialized in the main() function in competition.cc
- ************************************************************************/
+ hok.x = HOKUYO_CENTER_OFFSET_M + (double)a->center.x / 1000.0;
+ hok.y = (double)a->center.y / 1000.0;
-struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
-double totem_x;
-double totem_y;
-bool up;
+ /* transform target position which is relative to Hokuyo
+ center to absolute position in space */
+ target.x = (hok.x * cosinus) - (hok.y * sinus) + e.x;
+ target.y = (hok.x * sinus) + (hok.y * cosinus) + e.y;
-FSM_STATE(approach_central_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcVeryFast); // new trajectory
- robot_trajectory_add_point_trans(
- 0.6,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
- robot_trajectory_add_point_trans(
- 0.64,
- 1.2);
- robot_trajectory_add_point_trans(
- 0.64,
- 0.7);
- robot_trajectory_add_final_point_trans(
- 1.0,
- 0.45,
- ARRIVE_FROM(DEG2RAD(24),0.1));
-// robot_trajectory_add_final_point_trans(
-// 1.3,
-// 0.58,
-// NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(catch_central_buillon);
- break;
- default:
- break;
- }
+ // filter those targets not in playground range
+ if (goal_is_in_playground(target.x, target.y))
+ detected_target.push_back(target);
+ }
+ }
+ return detected_target.size();
}
-FSM_STATE(catch_central_buillon)
+/**
+ * Calculates point to approach the target.
+ *
+ * @param target Position of the center of the target.
+ * @param approach Pointer to the the intersection point of circle around
+ * the target and line between robot center and target.
+ */
+void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach)
{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
- robot_trajectory_add_final_point_trans(
- 1.3,
- 0.58,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_jaws(CATCH);
- FSM_TIMER(500);
- break;
- case EV_TIMER:
- FSM_TRANSITION(leave_central_buillon);
- break;
- default:
- break;
- }
-}
+ double xrobot, yrobot, phi;
+ double delta;
-FSM_STATE(leave_central_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new_backward(&tcFast); // new trajectory
- robot_trajectory_add_final_point_trans(
- 0.85,
- 0.38,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
- break;
- default:
- break;
- }
-}
+ robot_get_est_pos(&xrobot, &yrobot, &phi);
-FSM_STATE(push_bottle_bw)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot.use_back_bumpers = false;
- robot_trajectory_new_backward(&tcSlow); // new trajectory
- robot_trajectory_add_point_trans(
- 0.7,
- 0.3);
- robot_trajectory_add_final_point_trans(
- 0.64+0.08,
- ROBOT_AXIS_TO_BACK_M + 0.01,
- ARRIVE_FROM(DEG2RAD(270), 0.10));
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(return_home);
- break;
- default:
- break;
- }
-}
+ delta = distance(xrobot, yrobot, xtarget, ytarget);
-FSM_STATE(return_home)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
- robot_trajectory_add_point_trans(
- 0.64 + 0.08,
- 0.8);
- robot_trajectory_add_final_point_trans(
- 0.55,
- 1.1,
- ARRIVE_FROM(DEG2RAD(180), 0.01));
- break;
- case EV_MOTION_DONE:
- robot.use_back_bumpers = true;
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- SUBFSM_RET(NULL);
- break;
- default:
- break;
- }
-}
+ *xapproach = xtarget - (approach_radius * (xtarget - xrobot) / delta);
+ *yapproach = ytarget - (approach_radius * (ytarget - yrobot) / delta);
-FSM_STATE(leave_home)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- //robot_trajectory_new_backward(&tcSlow); // new trajectory
- robot_move_by(-0.18, NO_TURN(), &tcSlow);
- break;
- case EV_MOTION_DONE:
- act_jaws(CLOSE);
- FSM_TRANSITION(leave_home_for_totem);
- break;
- default:
- break;
- }
+ *phi_approach = get_approach_angle(xtarget, ytarget);
}
-FSM_STATE(leave_home_for_totem)
+/**
+ * Calculates point to approach the target.
+ *
+ * @param target Position of the center of the target.
+ * @return Angle to approach the target form.
+ */
+double get_approach_angle(double xtarget, double ytarget)
{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
- if(up) {
- robot_trajectory_add_final_point_trans(
- 0.7,
- 1.3,
- NO_TURN());
- }
- else {
- robot_trajectory_add_final_point_trans(
- 0.7,
- 0.9,
- NO_TURN());
- }
- break;
- case EV_MOTION_DONE:
- if(up) FSM_TRANSITION(approach_totem_up);
- else FSM_TRANSITION(approach_totem_down);
- break;
- default:
- break;
- }
-}
+ double xrobot, yrobot,phi;
-FSM_STATE(approach_totem_down)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- 0.4,
- TURN_CCW(DEG2RAD(90)));
- break;
- case EV_MOTION_DONE:
- act_lift(UP, 0, 0);
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(catch_totem_buillon_down);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(catch_totem_buillon_down)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcSlow); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- totem_y - ROBOT_AXIS_TO_FRONT_M - 0.05,
- ARRIVE_FROM(DEG2RAD(90), 0.10));
- break;
- case EV_MOTION_DONE:
- act_jaws(CATCH);
- FSM_TIMER(500);
- break;
- case EV_TIMER:
- FSM_TRANSITION(leave_totem_down);
- default:
- break;
- }
+ robot_get_est_pos(&xrobot, &yrobot, &phi);
+
+ return atan2((ytarget - yrobot), (xtarget - xrobot));
}
-FSM_STATE(leave_totem_down)
+
+/**
+ * FSM state for neighborhood observation.
+ *
+ * Detect targets using shape_detect.
+ * If no target detected, turn 120deg and try again.
+ * Scan all 360deg and then go back to move_around state.
+ *
+ * If target detected, go to approach_target state.
+ */
+FSM_STATE(survey)
{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new_backward(&tcSlow); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- 0.42,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- act_lift(DOWN, 0, 0);
- FSM_TIMER(2000);
+ static char turn_cntr = 0;
+ double x, y, phi;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ DBG_PRINT_EVENT("survey");
+ if (turn_cntr > 1) {
+ robot_pos_type target;
+ detected_target.clear();
+ for (double i = 1; i < 5; i++) {
+ target.x = i;
+ target.y = i/2.0;
+ detected_target.push_back(target);
+ }
+ // target detected, go to the target
+ FSM_TRANSITION(approach_target);
+ DBG_PRINT_EVENT("Target detected!");
+ } else {
+ // no target detected in this heading, turn 120°
+ robot_get_est_pos(&x, &y, &phi);
+ robot_goto_notrans(x, y, TURN(DEG2RAD(120)+phi), &tcSlow);
+ turn_cntr++;
+ DBG_PRINT_EVENT("no target");
+ }
break;
case EV_TIMER:
- FSM_TRANSITION(place_buillon_home);
+ if (turn_cntr > 2) {
+ FSM_TRANSITION(move_around);
+ } else {
+ FSM_TRANSITION(survey);
+ }
break;
- default:
- break;
- }
+ case EV_MOTION_DONE:
+ FSM_TIMER(1000);
+ break;
+ case EV_START:
+ case EV_RETURN:
+ case EV_JAWS_DONE:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event!");
+ break;
+ case EV_MOTION_ERROR:
+ FSM_TRANSITION(move_around);
+ break;
+ case EV_EXIT:
+ turn_cntr = 0;
+ break;
+ }
}
-FSM_STATE(place_buillon_home)
+/**
+ * FSM state for approaching all detected targets.
+ *
+ * Try to approach target.
+ * If approach OK - go to subautomaton and do target recognition, touch and load.
+ * On subautomaton return check if target loaded/valid.
+ *
+ * If target loaded, go home.
+ * If target not valid, try next target if any.
+ * If approach not succesfull - go to move_around state.
+ */
+FSM_STATE(approach_target)
{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- act_jaws(CATCH);
- robot_trajectory_new(&tcSlow); // new trajectory
- if(up) {
- robot_trajectory_add_point_trans(
- 0.9,
- 1.52);
- }
- else {
- robot_trajectory_add_point_trans(
- 0.9,
- 0.48);
- robot_trajectory_add_point_trans(
- 0.7,
- 0.8);
- }
- robot_trajectory_add_final_point_trans(
- 0.55,
- 1.1,
- ARRIVE_FROM(DEG2RAD(180),0.01));
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- FSM_TIMER(1000);
- break;
- case EV_TIMER:
- SUBFSM_RET(NULL);
- default:
- break;
- }
-}
+ static int target_cntr = 0;
+ int max_target = detected_target.size();
+ double x_target, y_tatget;
+ double x_approach, y_approach, phi_approach;
-FSM_STATE(approach_totem_up)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- 1.6,
- TURN_CW(DEG2RAD(270)));
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- act_lift(UP, 0, 0);
- FSM_TIMER(2000);
- break;
- case EV_TIMER:
- FSM_TRANSITION(catch_totem_buillon_up);
- break;
- default:
- break;
- }
-}
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ DBG_PRINT_EVENT("approaching target");
+ x_target = detected_target[target_cntr].x;
+ y_tatget = detected_target[target_cntr].y;
+ target_cntr++;
-FSM_STATE(catch_totem_buillon_up)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcSlow); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- totem_y + ROBOT_AXIS_TO_FRONT_M + 0.05,
- ARRIVE_FROM(DEG2RAD(270), 0.10));
- break;
- case EV_MOTION_DONE:
- act_jaws(CATCH);
- FSM_TIMER(500);
- break;
- case EV_TIMER:
- FSM_TRANSITION(leave_totem_up);
- break;
- default:
- break;
- }
+ get_approach_point(x_target, y_tatget, &x_approach, &y_approach, &phi_approach);
+ robot_goto_notrans(x_approach, y_approach, ARRIVE_FROM(phi_approach, 0.2), &tcSlow);
+ break;
+ case EV_MOTION_DONE:
+ DBG_PRINT_EVENT("target approached");
+ SUBFSM_TRANSITION(recognize, NULL);
+ break;
+ case EV_RETURN:
+ if (robot.target_loaded) {
+ FSM_TRANSITION(go_home);
+ } else if (robot.target_valid) {
+ //FIXME target is valid but not loaded - try another approach direction
+
+ } else if (!robot.target_valid && (target_cntr < max_target)) {
+ // go for next target if any
+ FSM_TRANSITION(approach_target);
+ } else {
+ // go to new point and survey
+ FSM_TRANSITION(move_around);
+ }
+ break;
+ case EV_MOTION_ERROR:
+ DBG_PRINT_EVENT("ERROR approaching");
+ if (target_cntr < max_target) {
+ FSM_TRANSITION(approach_target);
+ } else {
+ FSM_TRANSITION(move_around);
+ }
+ break;
+ case EV_TIMER:
+ case EV_START:
+ case EV_JAWS_DONE:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
+ case EV_EXIT:
+ target_cntr = 0;
+ break;
+ }
}
-FSM_STATE(leave_totem_up)
+FSM_STATE(move_around)
{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new_backward(&tcSlow); // new trajectory
- robot_trajectory_add_final_point_trans(
- totem_x,
- 1.6,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- act_jaws(OPEN);
- act_lift(DOWN, 0, 0);
- SUBFSM_RET(NULL);
- default:
- break;
- }
-}
+ double goalx, goaly, phi;
+ static int survey_cnt = 0;
+ switch (FSM_EVENT) {
+ case EV_ENTRY:
+
+ do {
+ goalx = ((rand()%PLAYGROUND_HEIGHT_MM)/1000.0);
+ goaly = ((rand()%PLAYGROUND_WIDTH_MM)/1000.0);
+ } while (!ShmapIsFreePoint(goalx, goaly) && close_goal(goalx, goaly));
-FSM_STATE(push_second_bottle_fw)
+ robot_goto_notrans(goalx, goaly, NO_TURN(), &tcFast);
+ DBG_PRINT_EVENT("new survey point");
+ survey_cnt++;
+ break;
+ case EV_MOTION_ERROR:
+ DBG_PRINT_EVENT("ERROR: can not access survey point");
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(survey);
+ break;
+ case EV_TIMER:
+ break;
+ case EV_RETURN:
+ break;
+ case EV_START:
+ /* do nothing */
+ break;
+ case EV_EXIT:
+ //ShmapFree();
+ break;
+ default:
+ break;
+ }
+}
+
+FSM_STATE(go_home)
{
- switch(FSM_EVENT) {
+ switch (FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcFast); // new trajectory
-// robot_trajectory_add_point_trans(0.7, 0.5);
- robot_goto_notrans(
- 1.85,
- ROBOT_AXIS_TO_FRONT_M,
- ARRIVE_FROM(DEG2RAD(270), 0.15), &tcFast);
+ DBG_PRINT_EVENT("homing");
+ robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, ARRIVE_FROM(DEG2RAD(HOME_POS_ANG_DEG), 0.2), &tcSlow);
+ break;
+ case EV_MOTION_ERROR:
+ DBG_PRINT_EVENT("ERROR: home position is not reachable!");
+ FSM_TIMER(1000);
break;
case EV_MOTION_DONE:
- SUBFSM_RET(NULL);
+ break;
+ case EV_TIMER:
+ FSM_TRANSITION(go_home);
+ break;
+ case EV_START:
+ /* do nothing */
+ break;
+ case EV_EXIT:
+ break;
default:
+ DBG_PRINT_EVENT("Mission completed!");
+ robot_exit();
break;
}
}
-FSM_STATE(go_back_from_home)
+/*
+FSM_STATE()
{
switch(FSM_EVENT) {
case EV_ENTRY:
- act_jaws(OPEN);
- robot_move_by(-0.2, NO_TURN(), &tcSlow);
break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_CRANE_DONE:
case EV_MOTION_DONE:
- act_jaws(CLOSE);
- FSM_TRANSITION(push_second_bottle_fw);
- break;
- default:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
break;
}
-}
\ No newline at end of file
+}
+*/
#include "roboevent.h"
#include <fsm.h>
-extern struct TrajectoryConstraints tcFast, tcVeryFast, tcSlow, tcVerySlow;
-extern double totem_x, totem_y;
-extern bool up;
-/* strategy FSM */
-FSM_STATE_DECL(central_buillon_wait_for_start);
-FSM_STATE_DECL(homologation_wait_for_start);
-FSM_STATE_DECL(calibrate);
-
-/* Strategy catch buillon in center */
-FSM_STATE_DECL(approach_central_buillon);
-FSM_STATE_DECL(catch_central_buillon);
-FSM_STATE_DECL(leave_central_buillon);
-FSM_STATE_DECL(push_bottle_bw);
-FSM_STATE_DECL(return_home);
-
-/* Ignore central buillon */
-//FSM_STATE_DECL(push_nearest_buillon);
-//FSM_STATE_DECL(push_bottle_fw);
-
-/* Autocalibration */
-FSM_STATE_DECL(go_back_for_cal);
-
-/* Common states */
-FSM_STATE_DECL(leave_home);
-FSM_STATE_DECL(leave_home_for_totem);
-FSM_STATE_DECL(approach_totem_down);
-FSM_STATE_DECL(catch_totem_buillon_down);
-FSM_STATE_DECL(leave_totem_down);
-FSM_STATE_DECL(place_buillon_home);
-FSM_STATE_DECL(approach_totem_up);
-FSM_STATE_DECL(catch_totem_buillon_up);
-FSM_STATE_DECL(leave_totem_up);
-
-/*FSM_STATE_DECL(place_down_buillon);
-FSM_STATE_DECL(approach_totem_up);
-FSM_STATE_DECL(catch_up_totem_buillon);
-FSM_STATE_DECL(leave_totem_up);
-FSM_STATE_DECL(place_up_buillon);
-
-FSM_STATE_DECL(push_second_bottle);
-*/
-/* HOMOLOGATION states */
-/* movement states - buillon */
-FSM_STATE_DECL(homologation_approach_buillon);
-/* Pushing the bottle */
-FSM_STATE_DECL(homologation_push_bottle);
-FSM_STATE_DECL(push_second_bottle_fw);
-FSM_STATE_DECL(go_back_from_home);
-
-
-void start_entry();
-void start_timer();
-void start_go();
-void start_exit();
-bool read_sharp();
-
-void robot_calibrate_odometry();
-
-
-
+#define HOME_POS_X_M 10
+#define HOME_POS_Y_M 1
+#define HOME_POS_ANG_DEG 90
+
+extern struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
+
+/************************************************************************
+ * FSM STATES DECLARATION
+ ************************************************************************/
+FSM_STATE_DECL(survey);
+FSM_STATE_DECL(approach_target);
+FSM_STATE_DECL(move_around);
+FSM_STATE_DECL(go_home);
+
+void set_initial_position(void);
+void actuators_home(void);
+bool goal_is_in_playground(double goalx, double goaly);
+bool close_goal(double goalx, double goaly);
+void get_approach_point(double xtarget, double ytarget, double *xapproach, double *yapproach, double *phi_approach);
+double get_approach_angle(double xtarget, double ytarget);
#endif
TARGET_ERROR // Fatal errror during planning
};
-// we may need something similar in future
-void invalidate(Point point)
-{
- //ShmapSetRectangleFlag(x-s, y-s, x+s, y+s, MAP_FLAG_INVALIDATE_WALL, 0)
-}
-
static double free_space_in_front_of_robot()
{
int i, i1, i2;
- int min = 10000, m;
+ int min = 4000, m;
i1 = HOKUYO_DEG_TO_INDEX(-45);
i2 = HOKUYO_DEG_TO_INDEX(+45);
for (i=MIN(i1, i2); i < MAX(i1, i2); i++) {
if (m > 19 && m < min)
min = m;
}
- return min/1000.0 - HOKUYO_CENTER_OFFSET_M;;
+ return min/1000.0 - (ROBOT_AXIS_TO_FRONT_M - HOKUYO_CENTER_OFFSET_M);
}
static bool obstackle_in_front_if_turn(Trajectory *t)
Trajectory *t = new Trajectory(move_target->tc, backward);
- /*
- // Clear all invalidate flags;
- ShmapSetRectangleFlag(0, 0, PLAYGROUND_WIDTH_M, PLAYGROUND_HEIGHT_M, 0, MAP_FLAG_INVALIDATE_WALL);
- // Set invalidate flags if we are going to walled area
- if (false) // we may need this in future
- invalidate(start);
- */
-
if (move_target->heading.operation == TOP_ARRIVE_FROM) {
get_arrive_from_point(move_target->x, move_target->y, move_target->heading,
&target.x, &target.y);
FSM_TRANSITION(wait_for_command);
break;
case EV_OBSTACLE:
+ stop();
switch (recalculate_trajectory()) {
// We can go to the target:
case TARGET_OK: break;
} else { /* go away if the point is not accessible, try max. 5x */
target_inacc_cnt = 0;
FSM_TRANSITION(wait_for_command);
- FSM_SIGNAL(MAIN, EV_MOTION_DONE, NULL);
+ FSM_SIGNAL(MAIN, EV_MOTION_ERROR, NULL);
DBG_PRINT_EVENT("Target inaccessible, go to new target.");
}
break;
#define IGNORE_FURTHER_THAN_M 0.5 /**< Ignore data from sharp if further than this */
#define OBS_FORGET_PERIOD 100 /**< The period of thread_obstacle_forgeting [ms] */
#define OBS_FORGET_SEC 1 /**< Time to completely forget detected obstacle. */
-#define OBS_OFFSET 0.6
void obstacle_detected_at(double x, double y, bool real_obstacle)
{
UL_LOG_CUST(ulogd_match_timing); // Log domain name = "ulogd_" + the name of the source file
#ifdef COMPETITION
-#define COMPETITION_TIME_DEFAULT 87
-#define TIME_TO_DEPOSITE_DEFAULT 65
+#define COMPETITION_TIME_DEFAULT 600
+#define TIME_TO_DEPOSITE_DEFAULT 500
#else
-#define COMPETITION_TIME_DEFAULT 87
-#define TIME_TO_DEPOSITE_DEFAULT 65
+#define COMPETITION_TIME_DEFAULT 600
+#define TIME_TO_DEPOSITE_DEFAULT 500
#endif
/* competition time in seconds */
if (fabs(future_pos.v) < 0.01)
continue;
- x = future_pos.x + cos(future_pos.phi)*ROBOT_AXIS_TO_FRONT_M;
- y = future_pos.y + sin(future_pos.phi)*ROBOT_AXIS_TO_FRONT_M;
+ x = future_pos.x /*+ cos(future_pos.phi)*ROBOT_AXIS_TO_FRONT_M */;
+ y = future_pos.y /*+ sin(future_pos.phi)*ROBOT_AXIS_TO_FRONT_M */;
ShmapPoint2Cell(x, y, &xcell, &ycell, &valid);
if (!valid)
next_period(&next, MOTION_PERIOD_NS);
if (measurement_ok) {
if (measurement_ok == 2) {
- fprintf(stderr, "problem: measurement timeout!!!!!!!!!!!");
+ fprintf(stderr, "problem: measurement timeout!!!!!!!!!!!\n");
}
measurement_ok--;
}
static inline double __trans_ang(double phi)
{
- if (robot.team_color == VIOLET) {
+ double a;
+
+ switch (robot.team_color) {
+ case WHITE:
+ case GREEN:
+ case YELLOW:
return phi;
- } else {
- double a;
+ default:
a = M_PI/2.0 - phi + M_PI/2.0;
a = fmod(a, 2.0*M_PI);
if (phi >= 0 && a >= 0) a -= 2.0*M_PI;
static inline struct move_target_heading __trans_heading(struct move_target_heading h)
{
- if (robot.team_color == VIOLET) {
+ switch (robot.team_color) {
+ case WHITE:
+ case GREEN:
+ case YELLOW:
return h;
- } else {
+ default:
if (h.operation != TOP_DONT_TURN) {
h.angle = __trans_ang(h.angle);
switch (h.operation) {
static inline double __trans_x(double x)
{
- if (robot.team_color == VIOLET)
+ switch (robot.team_color) {
+ case WHITE:
+ case GREEN:
+ case YELLOW:
return x;
- else
+ default:
return PLAYGROUND_WIDTH_M - x;
+ }
}
static inline double __trans_y(double y)
"EV_JAWS_DONE" : "",
"EV_LIFT_DONE" : "",
+ "EV_CAMERA_DONE" : "",
"EV_SWITCH_STRATEGY" : "",
"EV_MOTION_DONE" : "Previously submitted motion is finished",
robot.fsm.act.transition_callback = trans_callback;
robot.fsm.motion.transition_callback = trans_callback;
- robot.team_color = VIOLET;
-
- if (robot.team_color == VIOLET) {
- ul_loginf("We are VIOLET!\n");
- } else {
- ul_loginf("We are RED!\n");
- }
+ robot.team_color = WHITE;
+
+ switch (robot.team_color) {
+ case WHITE:
+ ul_loginf("We are WHITE!\n");
+ break;
+ case GREEN:
+ ul_loginf("We are GREEN!\n");
+ break;
+ case YELLOW:
+ ul_loginf("We are YELLOW!\n");
+ break;
+ }
robot_set_est_pos_trans(ROBOT_START_X_M, ROBOT_START_Y_M, DEG2RAD(ROBOT_START_ANGLE_DEG));
robot.orte.pwr_ctrl.voltage50 = 1;
robot.orte.pwr_ctrl.voltage80 = 1;
- robot.orte.camera_control.on = true;
+ robot.orte.camera_control.on = false;
robot.fsm.motion.state = &fsm_state_motion_init;
* Competition parameters
*/
enum team_color {
- VIOLET = 0, /* Coordinate transformation does not apply */
- RED /* Coordinate transformation applies (in *_trans() functions) */
+ WHITE = 0, /* Coordinate transformation does not apply */
+ GREEN,
+ YELLOW
};
enum robot_start_state {
struct sick_scan_type sick;
bool ignore_sick;
+ /* variables for target detection */
+ bool target_loaded;
+ bool target_valid;
+ char target_color[3];
+
struct map *map; /* Map for pathplanning (no locking) */
enum robot_status status[ROBOT_COMPONENT_NUMBER];
void *recvCallBackParam)
{
struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+ static bool last_response = false;
switch (info->status) {
case NEW_DATA: {
- ROBOT_LOCK(camera_result);
- // TODO prijmu data z kamery a podle toho nekam jedu\
-
- //robot.corns_conf_side = instance->side;
- //robot.corns_conf_center = instance->center;
- ROBOT_UNLOCK(camera_result);
- robot.status[COMPONENT_CAMERA] = STATUS_OK;
+ if (instance->data_valid && instance->data_valid != last_response) {
+ ROBOT_LOCK(camera_result);
+ robot.target_valid = instance->target_valid;
+ ROBOT_UNLOCK(camera_result);
+ FSM_SIGNAL(MAIN, EV_CAMERA_DONE, NULL);
+ }
+ last_response = instance->data_valid;
break;
}
case DEADLINE:
if (robot.orte.camera_control.on) {
robot.status[COMPONENT_CAMERA] = STATUS_FAILED;
- //system("killall -9 rozpuk");
}
DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
#endif
#define FSM_MAIN
+
#include <robot.h>
#include <fsm.h>
#include <unistd.h>
#include <string.h>
#include <robodim.h>
#include <error.h>
-#include "actuators.h"
#include <trgen.h>
-#include "match-timing.h"
#include <ul_log.h>
#include <path_planner.h>
+#include <can_msg_def.h>
-UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file */
-
-#define HOME_POS_X_M 5
-#define HOME_POS_Y_M 5
-#define HOME_POS_ANG_RAD 0
-
-/************************************************************************
- * Trajectory constraints used, are initialized in the init state
- ************************************************************************/
-
-struct TrajectoryConstraints tcSlow, tcFast;
+#include "actuators.h"
+#include "match-timing.h"
+#include "common-states.h"
-/************************************************************************
- * FSM STATES DECLARATION
- ************************************************************************/
+UL_LOG_CUST(ulogd_demo); /* Log domain name = ulogd + name of the file */
/* initial and starting states */
FSM_STATE_DECL(init);
FSM_STATE_DECL(wait_for_start);
-/* movement states */
-FSM_STATE_DECL(move_around);
-FSM_STATE_DECL(go_home);
-
-
-void set_initial_position()
-{
- robot_set_est_pos_trans(HOME_POS_X_M, HOME_POS_Y_M, HOME_POS_ANG_RAD);
-}
-void actuators_home()
-{
- //act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
-}
+/* mission competed */
+FSM_STATE_DECL(go_home);
-/* Check if the new point is within the playground scene */
-bool goal_is_in_playground(double goalx, double goaly) {
- if ((goalx < 0) || (goalx > PLAYGROUND_WIDTH_M) || (goaly < 0) || (goaly > PLAYGROUND_HEIGHT_M))
- return false;
- else
- return true;
-}
-
-/* Check if the new point is close to the robot */
-bool close_goal(double goalx, double goaly) {
- if ((abs(goalx - robot.ref_pos.x) < 0.5) && (abs(goaly - robot.ref_pos.y) < 0.5) )
- return true;
- else
- return false;
-}
FSM_STATE(init)
{
switch (FSM_EVENT) {
case EV_ENTRY:
- tcSlow = trajectoryConstraintsDefault;
- tcSlow.maxv = 0.5;
- tcSlow.maxacc = 0.5;
-
tcFast = trajectoryConstraintsDefault;
tcFast.maxv = 1;
- tcFast.maxacc = 2;
- tcFast.maxomega = 3;
+ tcFast.maxacc = 0.5;
+ tcFast.maxomega = 0.5;
+
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.3;
+ tcSlow.maxacc = 0.2;
+ tcSlow.maxomega = 0.2;
+
+ tcVerySlow = trajectoryConstraintsDefault;
+ tcVerySlow.maxv = 0.05;
+ tcVerySlow.maxacc = 0.05;
+ tcVerySlow.maxomega = 0.5;
FSM_TRANSITION(wait_for_start);
break;
switch (FSM_EVENT) {
case EV_ENTRY:
pthread_create(&thid, NULL, timing_thread, NULL);
- FSM_TIMER(1000);
+ set_initial_position();
+ FSM_TIMER(2000);
break;
case EV_START:
/* start competition timer */
sem_post(&robot.start);
- actuators_home();
- set_initial_position();
- FSM_TRANSITION(move_around);
+ FSM_TRANSITION(survey);
break;
case EV_TIMER:
- FSM_TIMER(1000);
- set_initial_position();
- if (robot.start_state == START_PLUGGED_IN)
+ FSM_TIMER(1000);
+ if (robot.start_state == START_PLUGGED_IN)
actuators_home();
break;
case EV_RETURN:
case EV_MOTION_ERROR:
case EV_MOTION_DONE:
+ case EV_JAWS_DONE:
+ DBG_PRINT_EVENT("unhandled event");
+ break;
case EV_EXIT:
break;
}
}
-FSM_STATE(move_around)
-{
- double goalx, goaly;
- static int survey_cnt = 0;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- do {
- goalx = robot.ref_pos.x + ((rand()%4000)/1000.0) - 2;
- goaly = robot.ref_pos.y + ((rand()%4000)/1000.0) - 2;
- //goalx = ((rand()%10000)/1000.0);
- //goaly = ((rand()%10000)/1000.0);
- } while (!ShmapIsFreePoint(goalx, goaly)
- && !goal_is_in_playground(goalx, goaly)
- && close_goal(goalx, goaly));
-
- robot_goto_notrans(goalx, goaly, NO_TURN(), &tcFast);
- break;
- case EV_MOTION_ERROR:
- ul_logdeb("Goal is not reachable\n");
- FSM_TIMER(1000);
- break;
- case EV_MOTION_DONE:
- if(++survey_cnt > 10) {
- FSM_TRANSITION(go_home);
- survey_cnt = 0;
- } else {
- FSM_TRANSITION(move_around);
- }
- break;
- case EV_TIMER:
- FSM_TRANSITION(move_around);
- break;
- case EV_START:
- /* do nothing */
- break;
- case EV_EXIT:
- //ShmapFree();
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(go_home)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_goto_notrans(HOME_POS_X_M, HOME_POS_Y_M, NO_TURN(), &tcFast);
- break;
- case EV_MOTION_ERROR:
- ul_logdeb("Goal is not reachable\n");
- FSM_TIMER(1000);
- break;
- case EV_MOTION_DONE:
- case EV_TIMER:
- FSM_TRANSITION(wait_for_start);
- break;
- case EV_START:
- /* do nothing */
- break;
- case EV_EXIT:
- //ShmapFree();
- break;
- default:
- break;
- }
-}
-
/************************************************************************
* MAIN FUNCTION
--- /dev/null
+#define FSM_MAIN
+#include <robot.h>
+#include <fsm.h>
+#include <unistd.h>
+#include <math.h>
+#include <movehelper.h>
+#include <map.h>
+#include <robomath.h>
+#include <string.h>
+#include <robodim.h>
+#include <error.h>
+#include <trgen.h>
+#include <stdbool.h>
+#include <ul_log.h>
+#include <hokuyo.h>
+
+#include "robodata.h"
+#include "actuators.h"
+#include "match-timing.h"
+#include "common-states.h"
+#include "sub-states.h"
+
+UL_LOG_CUST(ulogd_sub_states); /* Log domain name = ulogd + name of the file */
+
+extern struct TrajectoryConstraints tcSlow, tcVerySlow;
+
+/************************************************************************
+ * Functions used in and called from all the (almost identical)
+ * "wait for start" states in particular strategies.
+ ************************************************************************/
+
+#undef DBG_FSM_STATE
+#define DBG_FSM_STATE(name) do { if (fsm->debug_states) ul_loginf("fsm %s %.1f: %s(%s)\n", \
+ fsm->debug_name, robot_current_time(), \
+ name, fsm_event_str(fsm->events[fsm->ev_head])); } while(0)
+
+
+#define HOK_MIN_MM (ROBOT_AXIS_TO_FRONT_MM - HOKUYO_CENTER_OFFSET_MM)
+#define HOK_MIN_M (HOK_MIN_MM/1000.0)
+#define HOK_MAX_MM (1000)
+
+/**
+ * Count the angle to turn by to see the minimum distance in the robot axis.
+ * @param alpha poiner to the angle to turn by
+ * @param minimum pointer to the minimum distance from hokuyo
+ */
+void get_hokuyo_min(double *alpha, double *minimum)
+{
+ double beta;
+ double min = 1000;
+ int min_i;
+
+ struct hokuyo_scan_type scan = robot.hokuyo;
+ uint16_t *data = scan.data;
+
+ for (int i = 0; i < HOKUYO_ARRAY_SIZE; i++) {
+ beta = HOKUYO_INDEX_TO_RAD(i);
+ if((beta < (-50.0 / 180.0 * M_PI)) || ((beta > (50.0 / 180.0 * M_PI))))
+ continue;
+
+ if(data[i] > HOK_MIN_MM && data[i] < HOK_MAX_MM) {
+ if (data[i] < min ) {
+ min = data[i];
+ min_i = i;
+ }
+ // printf("min: %f, beta: %f\n", min, beta);
+ }
+ }
+ min /= 1000.0;
+ beta = HOKUYO_INDEX_TO_RAD(min_i);
+ *alpha = atan2((min * sin(beta)), (HOKUYO_CENTER_OFFSET_M + min * cos(beta)));
+ *minimum = min - HOK_MIN_M;
+
+ printf("alpha: %f, beta: %f, minimum: %f\n", *alpha, beta, *minimum);
+}
+
+void enable_obstacle_avoidance(bool enable)
+{
+ robot.obstacle_avoidance_enabled = enable;
+ robot.ignore_hokuyo = !enable;
+ robot.check_turn_safety = enable;
+}
+
+FSM_STATE(recognize)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ act_camera_recognize(robot.target_color);
+ DBG_PRINT_EVENT("recognition");
+ FSM_TIMER(1000);
+ break;
+ case EV_TIMER:
+ FSM_TIMER(1000);
+ break;
+ case EV_CAMERA_DONE:
+ act_camera_off();
+ if (robot.target_valid) {
+ DBG_PRINT_EVENT("camera: Target valid!");
+ FSM_TRANSITION(get_target_turn);
+ } else {
+ DBG_PRINT_EVENT("camera: Target not valid!");
+ robot.target_loaded = false;
+ SUBFSM_RET(NULL);
+ }
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(get_target_turn)
+{
+ double alpha, minimum;
+ double x, y, phi;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ DBG_PRINT_EVENT("turn");
+ enable_obstacle_avoidance(false);
+ get_hokuyo_min(&alpha, &minimum);
+ robot_get_est_pos(&x, &y, &phi);
+ robot_move_by(0, TURN(alpha+phi), &tcVerySlow);
+ break;
+ case EV_TIMER:
+ break;
+ case EV_MOTION_DONE:
+ FSM_TRANSITION(get_target_touch);
+ break;
+ case EV_MOTION_ERROR:
+ enable_obstacle_avoidance(true);
+ SUBFSM_RET(NULL);
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(get_target_touch)
+{
+ const double close = 0.05;
+ const double step = 0.02;
+ double alpha, minimum;
+ double x, y, phi;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ DBG_PRINT_EVENT("touch");
+ get_hokuyo_min(&alpha, &minimum);
+ robot_move_by(step, NO_TURN(), &tcVerySlow);
+ break;
+ case EV_TIMER:
+ break;
+ case EV_MOTION_DONE:
+ get_hokuyo_min(&alpha, &minimum);
+ if (minimum < close) {
+ FSM_TRANSITION(get_target_load);
+ } else {
+ FSM_TRANSITION(get_target_turn);
+ }
+ break;
+ case EV_MOTION_ERROR:
+ enable_obstacle_avoidance(true);
+ SUBFSM_RET(NULL);
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(get_target_load)
+{
+ static int direction = 0;
+
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ direction = 0;
+ act_jaws(OPEN);
+ break;
+ case EV_TIMER:
+ act_jaws(CATCH);
+ robot.target_loaded = true;
+ FSM_TRANSITION(get_target_back);
+ break;
+ case EV_JAWS_DONE:
+ if (direction == 0) {
+ direction = 1;
+ FSM_TIMER(2000);
+ }
+ break;
+ case EV_EXIT:
+ break;
+ }
+}
+
+FSM_STATE(get_target_back)
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ robot_move_by(-0.5, NO_TURN(), &tcVerySlow);
+ break;
+ case EV_TIMER:
+ break;
+ case EV_JAWS_DONE:
+ break;
+ case EV_MOTION_DONE:
+ case EV_MOTION_ERROR:
+ SUBFSM_RET(NULL);
+ break;
+ case EV_EXIT:
+ enable_obstacle_avoidance(true);
+ break;
+ }
+}
+
+/*
+FSM_STATE()
+{
+ switch(FSM_EVENT) {
+ case EV_ENTRY:
+ break;
+ case EV_START:
+ case EV_TIMER:
+ case EV_RETURN:
+ case EV_CRANE_DONE:
+ case EV_MOTION_DONE:
+ case EV_MOTION_ERROR:
+ case EV_SWITCH_STRATEGY:
+ DBG_PRINT_EVENT("unhandled event");
+ case EV_EXIT:
+ break;
+ }
+}
+*/
\ No newline at end of file
--- /dev/null
+#ifndef SUB_STATES_H
+#define SUB_STATES_H
+
+#define FSM_MAIN
+
+#include "roboevent.h"
+#include <fsm.h>
+
+/************************************************************************
+ * FSM STATES DECLARATION
+ ************************************************************************/
+
+FSM_STATE_DECL(recognize);
+FSM_STATE_DECL(get_target_turn);
+FSM_STATE_DECL(get_target_touch);
+FSM_STATE_DECL(get_target_load);
+FSM_STATE_DECL(get_target_back);
+
+void get_hokuyo_min(double *alpha, double *minimum);
+
+#endif
\ No newline at end of file
struct map_cell *cell = &map->cells[j][i];
color = lightGray;
- if ((cell->flags & MAP_FLAG_WALL) &&
- (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
+ if (cell->flags & MAP_FLAG_WALL)
color = darkYellow;
if (cell->flags & MAP_FLAG_IGNORE_OBST)
color = darkGreen;
read_scan(device_handle, dists, NULL, scan_max);
// Print measured data
- printf("Ranges:\n");
+ //printf("Ranges:\n");
int j;
for (j = 0; j < scan_max; j++) {
//printf("index: %d, distance: %li, reflex intensity: %li\n", j, dists[j], refls[j]);
- printf("index: %d, distance: %d\n", j, dists[j]);
+ //printf("index: %d, distance: %d\n", j, dists[j]);
orte.sick_scan.data[j] = dists[j];
/*if (refls) {
orte.sick_scan.data[j + scan_max] = refls[j];
-
struct motion_irc {
long left; // PXMC's actual position (pxms.ap)
long right; // PXMC's actual position (pxms.ap)
double time;
};
-// FIXME: What's this??? M.S.
-struct binary_data {
- octet data;
-};
-
struct fsm_state {
char state_name[21];
};
struct result {
octet x;
octet y;
- octet target_valid;
- octet data_valid;
+
+ boolean data_valid;
+ boolean target_valid;
error error;
};
};
type=lift_status topic=lift_status deadline=0.5
type=lift_cmd topic=lift_cmd
type=match_time topic=match_time deadline=0.5
-type=binary_data topic=binary_data
type=can_msg topic=can_msg
type=robot_pos topic=est_pos_odo
type=robot_pos topic=est_pos_indep_odo
projects / eurobot / public.git / commitdiff