//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-#define END_SWITCH_UP 9 //pin 4, exp. port on board
-#define END_SWITCH_DOWN 8 //pin 3, exp. port on board
-#define SWITCH_HOME 19 //pin 6, exp. port on board
+#define END_SWITCH_UP_PIN 9 //pin 4, exp. port on board
+#define END_SWITCH_DOWN_PIN 8 //pin 3, exp. port on board
+#define SWITCH_HOME_PIN 19 //pin 6, exp. port on board
+#define START_PIN 15 //pin 7, exp. port on board
+#define COLOR_PIN 18 //pin 5, exp. port on board
+
+#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 LIFT_IRC_VAL_MAX 0x19C
+#define LIFT_IRC_VAL_MIN 0x0
+
+#define IRC_A_PIN 2 //pin 1, exp. port on board
+#define IRC_B_PIN 3 //pin 2, exp. port on board
+
+#define IRC_A_MASK 0x04 //(1<<IRC_A)
+#define IRC_B_MASK 0x08 //(1<<IRC_B)
+#define IRC_AB_MASK 0x0C //((1<<IRC_A)&(1<<IRC_B))
void lift_switches_handler(void);
void lift_switches_handler()
{
- if (IO0PIN & (1<<END_SWITCH_UP)){
- fsm_lift.flags |= CAN_LIFT_SWITCH_UP;
- deb_led_on(LEDR);
- } else {
+ if (IO0PIN & (1<<END_SWITCH_UP_PIN)){
fsm_lift.flags &= ~CAN_LIFT_SWITCH_UP;
deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_UP;
+ deb_led_on(LEDR);
}
- if (IO0PIN & (1<<END_SWITCH_DOWN)){
- fsm_lift.flags |= CAN_LIFT_SWITCH_DOWN;
- deb_led_on(LEDR);
- } else {
+ if (IO0PIN & (1<<END_SWITCH_DOWN_PIN)){
fsm_lift.flags &= ~CAN_LIFT_SWITCH_DOWN;
deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_DOWN;
+ deb_led_on(LEDR);
}
- if (IO0PIN & (1<<SWITCH_HOME)){
- fsm_lift.flags |= CAN_LIFT_SWITCH_HOME;
- deb_led_on(LEDR);
- } else {
+ if (IO0PIN & (1<<SWITCH_HOME_PIN)){
fsm_lift.flags &= ~CAN_LIFT_SWITCH_HOME;
deb_led_off(LEDR);
+ } else {
+ fsm_lift.flags |= CAN_LIFT_SWITCH_HOME;
+ deb_led_on(LEDR);
}
-}
-
-#define LIFT_IRC_VAL_MAX 0x19C
-#define LIFT_IRC_VAL_MIN 0x0
-
-#define IRC_A 2 //pin 1, exp. port on board
-#define IRC_B 3 //pin 2, exp. port on board
-
-#define IRC_A_MASK 0x04 //(1<<IRC_A)
-#define IRC_B_MASK 0x08 //(1<<IRC_B)
-#define IRC_AB_MASK 0x0C //((1<<IRC_A)&(1<<IRC_B))
-
-int irc_read_tick(uint16_t *last_irc);
-void irc_init(void);
-
-void irc_init(void){
- SET_PIN(PINSEL0, IRC_A, PINSEL_0);
- SET_PIN(PINSEL0, IRC_B, PINSEL_0);
-
- SET_PIN(PINSEL0, END_SWITCH_UP, PINSEL_0);
- SET_PIN(PINSEL0, END_SWITCH_DOWN, PINSEL_0);
+// if (IO0PIN & (1<<IRC_A_PIN)){
+// deb_led_off(LEDR);
+// } else {
+// deb_led_on(LEDR);
+// }
+// if (IO0PIN & (1<<IRC_B_PIN)){
+// deb_led_off(LEDR);
+// } else {
+// deb_led_on(LEDR);
+// }
}
//source code from http://www.vosrk.cz/robotika/Stavba/Enkoder.pdf
-int irc_read_tick(uint16_t *last_irc){
+int32_t irc_read_tick(){
- static char cnt = 0;
+ static uint16_t cnt_up = 0;
+ static uint16_t cnt_down = 0;
+ static uint16_t last_irc = 0;
+ static int32_t position = 0;
uint16_t irc, temp;
irc = IO0PIN & IRC_AB_MASK;
irc ^= IRC_A_MASK;
}
- temp = (irc - *last_irc) & IRC_AB_MASK;
+ temp = (irc - last_irc) & IRC_AB_MASK;
- *last_irc = irc;
+ last_irc = irc;
if (temp == IRC_A_MASK){
/* count 100times slower - we do not need 250 ticks per milimeter*/
- if (--cnt < 100) {
- cnt = 0;
- return -1;
+ if (++cnt_down >= 100) {
+ cnt_down = 0;
+ cnt_up = 0;
+ deb_led_change(LEDB);
+ return --position;
}
} else if (temp == IRC_AB_MASK){
/* count 100times slower - we do not need 250 ticks per milimeter*/
- if (++cnt > 100) {
- cnt = 0;
- return 1;
+ if (++cnt_up >= 100) {
+ cnt_up = 0;
+ cnt_down = 0;
+ deb_led_change(LEDB);
+ return ++position;
}
}
- return 0;
+ return position;
}
void init_motors(void){
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-
-#define START_PIN 15 //pin 7, exp. port on board
-#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) */
-
-
void start_button(void)
{
can_msg_t msg;
init_uart();
// init_adc(ADC_ISR);
- irc_init();
-
-
}
/*********************************************************/
void can_send_status(void)
}
}
-
-
-#define COLOR_PIN 18 //pin 5, exp. port on board
-
void color_handler()
{
static uint32_t color_time = 0;
{
uint32_t main_time = timer_usec;
uint32_t status_time = timer_usec;
-
- static uint16_t last_irc=0;
-
+
//lift motor is motor A, MOTA connctor on board
- init_periphery();
-
- SET_PIN(PINSEL0, START_PIN, PINSEL_0); //init of start pin
- SET_PIN(PINSEL0, 3, PINSEL_0); //init of color pin
- SET_PIN(PINSEL0, 4, PINSEL_0); //init of home pin
-
+ init_periphery();
+
+ SET_PIN(PINSEL0, IRC_A_PIN, PINSEL_0);
+ SET_PIN(PINSEL0, IRC_B_PIN, PINSEL_0);
+
+ SET_PIN(PINSEL0, END_SWITCH_UP_PIN, PINSEL_0);
+ SET_PIN(PINSEL0, END_SWITCH_DOWN_PIN, PINSEL_0);
+
+ SET_PIN(PINSEL0, START_PIN, PINSEL_0); //init of start pin
+ SET_PIN(PINSEL1, (COLOR_PIN - 16), PINSEL_0); //init of color pin
+ SET_PIN(PINSEL1, (SWITCH_HOME_PIN - 16), PINSEL_0); //init of home pin
+
+ IO0DIR &= ~((1<<START_PIN) | (1<<SWITCH_HOME_PIN) | (1 << COLOR_PIN));
+ IO0DIR &= ~((1<<END_SWITCH_UP_PIN) | (1<<END_SWITCH_DOWN_PIN));
+ IO0DIR &= ~((1<<IRC_A_PIN) | (1<<IRC_B_PIN));
send_rs_str("Lift started\n");
fsm_lift.act_pos = 0;
can_send_status();
}
- fsm_lift.act_pos = fsm_lift.act_pos + irc_read_tick(&last_irc);
+ fsm_lift.act_pos = irc_read_tick();
start_button();
color_handler();
/*
* homologation.cc 08/04/29
- *
+ *
* Robot's control program intended for homologation (approval phase) on Eurobot 2009.
*
* Copyright: (c) 2009 CTU Dragons
#include <string.h>
#include <robodim.h>
#include <error.h>
-#include "corns_configs.h"
#include "actuators.h"
#include <trgen.h>
#include "match-timing.h"
FSM_STATE_DECL(init);
FSM_STATE_DECL(wait_for_start);
/* movement states */
-FSM_STATE_DECL(climb_the_slope);
-FSM_STATE_DECL(sledge_down);
-FSM_STATE_DECL(to_container_diag);
+FSM_STATE_DECL(aproach_first_fugure);
+FSM_STATE_DECL(load_first_figure);
+FSM_STATE_DECL(to_red_square);
FSM_STATE_DECL(to_container_ortho);
-FSM_STATE_DECL(experiment_decider);
-FSM_STATE_DECL(approach_next_corn);
-FSM_STATE_DECL(rush_the_corn);
-FSM_STATE_DECL(turn_around);
-FSM_STATE_DECL(zvedej_vidle);
+// FSM_STATE_DECL(experiment_decider);
+// FSM_STATE_DECL(approach_next_corn);
+// FSM_STATE_DECL(rush_the_corn);
+// FSM_STATE_DECL(turn_around);
+// FSM_STATE_DECL(zvedej_vidle);
/************************************************************************
* INITIAL AND STARTING STATES
************************************************************************/
-FSM_STATE(init)
+FSM_STATE(init)
{
switch (FSM_EVENT) {
case EV_ENTRY:
tcVerySlow = trajectoryConstraintsDefault;
tcVerySlow.maxv = 0.05;
tcVerySlow.maxacc = 0.05;
-
+
FSM_TRANSITION(wait_for_start);
break;
default:
void set_initial_position()
{
//FIXME:
- robot_set_est_pos_trans(ROBOT_AXIS_TO_FRONT_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
- DEG2RAD(180));
+ robot_set_est_pos_trans(0.4 - ROBOT_AXIS_TO_FRONT_M,
+ PLAYGROUND_HEIGHT_M - 0.21,
+ 0);
}
void actuators_home()
{
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
+ act_jaws(CLOSE);
+ // drive lift to home position
+ act_lift(0, 0, 1);
+ // unset the homing request
+ act_lift(0, 0, 0);
}
#ifdef COMPETITION
sem_post(&robot.start);
actuators_home();
set_initial_position();
- FSM_TRANSITION(climb_the_slope);
+ FSM_TRANSITION(aproach_first_fugure);
break;
case EV_TIMER:
FSM_TIMER(1000);
case EV_RETURN:
case EV_MOTION_ERROR:
case EV_MOTION_DONE:
- case EV_VIDLE_DONE:
+ //case EV_VIDLE_DONE:
case EV_SWITCH_STRATEGY:
DBG_PRINT_EVENT("unhandled event");
break;
case EV_EXIT:
- robot.corns = get_all_corns(robot.corns_conf_side, robot.corns_conf_center);
-
- /*
- //opras na testovani zluteho:
- robot_set_est_pos_trans(PLAYGROUND_WIDTH_M - ROBOT_AXIS_TO_FRONT_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2),
- DEG2RAD(0));
- robot.team_color = YELLOW;
- */
break;
}
}
-FSM_STATE(zvedej_vidle)
-{
- static int cnt = 0;
- switch(FSM_EVENT) {
- case EV_ENTRY:
- case EV_TIMER:
- FSM_TIMER(500);
- act_vidle((VIDLE_UP - VIDLE_DOWN)*cnt/3 + VIDLE_DOWN, VIDLE_FAST_SPEED);
- ul_logdeb("--------------------cnt: %d\n", cnt);
- cnt++;
- if(cnt >= 3) {
- robot_exit();
- //FSM_TRANSITION(sledge_down);
- }
- break;
- case EV_START:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
+// FSM_STATE(zvedej_vidle)
+// {
+// static int cnt = 0;
+// switch(FSM_EVENT) {
+// case EV_ENTRY:
+// case EV_TIMER:
+// FSM_TIMER(500);
+// act_vidle((VIDLE_UP - VIDLE_DOWN)*cnt/3 + VIDLE_DOWN, VIDLE_FAST_SPEED);
+// ul_logdeb("--------------------cnt: %d\n", cnt);
+// cnt++;
+// if(cnt >= 3) {
+// robot_exit();
+// //FSM_TRANSITION(sledge_down);
+// }
+// break;
+// case EV_START:
+// case EV_RETURN:
+// case EV_VIDLE_DONE:
+// case EV_MOTION_DONE:
+// case EV_MOTION_ERROR:
+// case EV_SWITCH_STRATEGY:
+// DBG_PRINT_EVENT("unhandled event");
+// case EV_EXIT:
+// break;
+// }
+// }
/************************************************************************
* MOVEMENT STATES
************************************************************************/
-FSM_STATE(climb_the_slope)
+FSM_STATE(aproach_first_fugure)
{
switch(FSM_EVENT) {
case EV_ENTRY:
// disables using side switches on bumpers when going up
- robot.use_left_switch = false;
- robot.use_right_switch = false;
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
- robot.ignore_hokuyo = true;
- robot_trajectory_new_backward(&tcSlow);
+ robot.use_left_bumper = false;
+ robot.use_right_bumper = false;
+ robot.use_back_bumpers = false;
+
+ robot_trajectory_new(&tcSlow);
robot_trajectory_add_point_trans(
- 0.5 - ROBOT_AXIS_TO_BACK_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2));
- /* position for collecting oranges*/
+ 0.4+0.05+0.25,
+ PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.05);
robot_trajectory_add_final_point_trans(
- SLOPE_TO_RIM_M + SLOPE_LENGTH_M - ROBOT_AXIS_TO_BACK_M + 0.05,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) +0.01,
- NO_TURN());
+ 0.4+0.05+0.25,
+ 0.29+0.28+0.28,
+ TURN_CCW(0));
break;
case EV_MOTION_DONE:
- FSM_TIMER(2000);
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
+ FSM_TIMER(3000);
+ act_jaws(OPEN);
+ //FSM_TRANSITION(load_first_figure);
break;
case EV_START:
case EV_TIMER:
- FSM_TRANSITION(sledge_down);
+ FSM_TRANSITION(load_first_figure);
break;
case EV_RETURN:
- case EV_VIDLE_DONE:
+ //case EV_VI_DONE:
case EV_MOTION_ERROR:
case EV_SWITCH_STRATEGY:
DBG_PRINT_EVENT("unhandled event");
}
}
-FSM_STATE(sledge_down)
+FSM_STATE(load_first_figure)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcFast);
- robot_trajectory_add_point_trans(
- 1 -ROBOT_AXIS_TO_BACK_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2)+0.01);
- robot_trajectory_add_point_trans(
- SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.26,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2) - 0.10);
- robot_trajectory_add_final_point_trans(0.50, PLAYGROUND_HEIGHT_M - 0.6, NO_TURN());
+ robot_trajectory_new_backward(&tcSlow);
+ robot_trajectory_add_final_point_trans(
+ 0.2+0.1+ROBOT_AXIS_TO_BACK_M,
+ 0.29+0.28+0.28, NO_TURN());
break;
case EV_MOTION_DONE:
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- FSM_TRANSITION(to_container_diag);
- //FSM_TRANSITION(to_container_ortho);
+ FSM_TIMER(2000);
+ act_jaws(CATCH);
break;
case EV_START:
case EV_TIMER:
+ FSM_TRANSITION(to_red_square);
case EV_RETURN:
- case EV_VIDLE_DONE:
+ //case EV_VIDLE_DONE:
case EV_MOTION_ERROR:
case EV_SWITCH_STRATEGY:
DBG_PRINT_EVENT("unhandled event");
case EV_EXIT:
// enables using side switches on bumpers
- robot.use_left_switch = true;
- robot.use_right_switch = true;
- robot.ignore_hokuyo = false;
+ //robot.use_left_switch = true;
+ //robot.use_right_switch = true;
+ //robot.ignore_hokuyo = false;
break;
}
}
-FSM_STATE(to_container_diag)
+FSM_STATE(to_red_square)
{
switch(FSM_EVENT) {
case EV_ENTRY:
- robot_trajectory_new(&tcFast);
+ robot_trajectory_new(&tcSlow);
// face the rim with front of the robot
//robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
// face the rim with back of the robot
- robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M-0.6, 0.35);
- robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, ARRIVE_FROM(DEG2RAD(90),0.05));
+ //robot_trajectory_add_point_trans(0.4+0.05+0.2, 0.35+0.35+0.17);
+ robot_trajectory_add_final_point_trans(0.4+0.05+0.2 + ROBOT_AXIS_TO_FIGURE_CENTER_M + 0.05, 0.35+0.35+0.17, NO_TURN());
break;
case EV_MOTION_DONE:
- FSM_TIMER(6000);
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
+ FSM_TIMER(5000);
+ act_jaws(OPEN);
break;
case EV_TIMER:
- act_vidle(VIDLE_UP, VIDLE_FAST_SPEED);
- FSM_TRANSITION(approach_next_corn);
- break;
- case EV_START:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(to_container_ortho)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcFast);
- robot_trajectory_add_point_trans(
- SLOPE_TO_RIM_M - ROBOT_AXIS_TO_BACK_M - 0.15,
- PLAYGROUND_HEIGHT_M - 0.355);
- robot_trajectory_add_point_trans(0.55, PLAYGROUND_HEIGHT_M - 0.65);
- robot_trajectory_add_point_trans(0.90, PLAYGROUND_HEIGHT_M - 0.75);
- robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.60, PLAYGROUND_HEIGHT_M - 0.7);
- robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.35, PLAYGROUND_HEIGHT_M - 0.9);
-
- // face the rim with front of the robot
- //robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.12, ARRIVE_FROM(DEG2RAD(-90), 0.10));
- // face the rim with back of the robot
- robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.40, TURN_CCW(DEG2RAD(90)));
- break;
- case EV_MOTION_DONE:
- act_vidle(VIDLE_DOWN, VIDLE_FAST_SPEED);
+ //act_jaws(OPEN);
+ //FSM_TRANSITION(wait_for_start);
break;
case EV_START:
- case EV_TIMER:
case EV_RETURN:
- case EV_VIDLE_DONE:
+ //case EV_VIDLE_DONE:
case EV_MOTION_ERROR:
case EV_SWITCH_STRATEGY:
DBG_PRINT_EVENT("unhandled event");
}
}
-static enum where_to_go {
- CORN,
- TURN_AROUND,
- CONTAINER,
- NO_MORE_CORN
-} where_to_go = CORN;
-
-static struct corn *corn_to_get;
-
-FSM_STATE(experiment_decider)
-{
-
- switch(FSM_EVENT) {
- case EV_ENTRY:
- if (where_to_go == CORN) {
- FSM_TRANSITION(approach_next_corn);
- } else if (where_to_go == CONTAINER) {
- FSM_TRANSITION(rush_the_corn);
- } else if (where_to_go == TURN_AROUND) {
- FSM_TRANSITION(turn_around);
- } else /* NO_MORE_CORN */ {
- }
- break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-static int cnt = 0;
-FSM_STATE(approach_next_corn)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY: {
- double x, y, phi;
- robot_get_est_pos(&x, &y, &phi);
- ul_logdeb("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
- cnt, x, y, phi);
-
- corn_to_get = choose_next_corn();
- if (corn_to_get) {
- Pos *p = get_corn_approach_position(corn_to_get);
- corn_to_get->was_collected = true;
- //robot_trajectory_new(&tcFast);
- //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
- robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
- delete(p);
- where_to_go = CONTAINER;
- } else {
- where_to_go = NO_MORE_CORN;
- }
- break;
- }
- case EV_MOTION_DONE:
- cnt++;
- FSM_TRANSITION(experiment_decider);
- break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-FSM_STATE(rush_the_corn)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- double x;
- if (robot.team_color == BLUE) {
- x = corn_to_get->position.x;
- } else {
- x = PLAYGROUND_WIDTH_M - corn_to_get->position.x;
- }
- ul_logdeb("x = %.3f, %.3f \n", x, corn_to_get->position.y);
- remove_wall_around_corn(x, corn_to_get->position.y);
- robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
- where_to_go = TURN_AROUND;
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(experiment_decider);
- break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
-
-// used to perform the maneuvre
-FSM_STATE(turn_around)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new_backward(&tcFast);
- robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
- break;
- case EV_MOTION_DONE:
- where_to_go = CORN;
- FSM_TRANSITION(experiment_decider);
- break;
- case EV_START:
- case EV_TIMER:
- case EV_RETURN:
- case EV_VIDLE_DONE:
- case EV_MOTION_ERROR:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- case EV_EXIT:
- break;
- }
-}
+// static enum where_to_go {
+// CORN,
+// TURN_AROUND,
+// CONTAINER,
+// NO_MORE_CORN
+// } where_to_go = CORN;
+//
+// static struct corn *corn_to_get;
+//
+// FSM_STATE(experiment_decider)
+// {
+//
+// switch(FSM_EVENT) {
+// case EV_ENTRY:
+// if (where_to_go == CORN) {
+// FSM_TRANSITION(approach_next_corn);
+// } else if (where_to_go == CONTAINER) {
+// FSM_TRANSITION(rush_the_corn);
+// } else if (where_to_go == TURN_AROUND) {
+// FSM_TRANSITION(turn_around);
+// } else /* NO_MORE_CORN */ {
+// }
+// break;
+// case EV_START:
+// case EV_TIMER:
+// case EV_RETURN:
+// case EV_VIDLE_DONE:
+// case EV_MOTION_DONE:
+// case EV_MOTION_ERROR:
+// case EV_SWITCH_STRATEGY:
+// DBG_PRINT_EVENT("unhandled event");
+// case EV_EXIT:
+// break;
+// }
+// }
+
+// static int cnt = 0;
+// FSM_STATE(approach_next_corn)
+// {
+// switch(FSM_EVENT) {
+// case EV_ENTRY: {
+// double x, y, phi;
+// robot_get_est_pos(&x, &y, &phi);
+// ul_logdeb("approach_next_corn: puck cnt: %d, est pos %.3f, %.3f, %.3f\n",
+// cnt, x, y, phi);
+//
+// corn_to_get = choose_next_corn();
+// if (corn_to_get) {
+// Pos *p = get_corn_approach_position(corn_to_get);
+// corn_to_get->was_collected = true;
+// //robot_trajectory_new(&tcFast);
+// //robot_trajectory_add_final_point_trans(robot_trajectory_add_final_point_trans(p->x, p->y, TURN(p->phi));
+// robot_goto_trans(p->x, p->y, TURN(p->phi), &tcFast);
+// delete(p);
+// where_to_go = CONTAINER;
+// } else {
+// where_to_go = NO_MORE_CORN;
+// }
+// break;
+// }
+// case EV_MOTION_DONE:
+// cnt++;
+// FSM_TRANSITION(experiment_decider);
+// break;
+// case EV_START:
+// case EV_TIMER:
+// case EV_RETURN:
+// case EV_VIDLE_DONE:
+// case EV_MOTION_ERROR:
+// case EV_SWITCH_STRATEGY:
+// DBG_PRINT_EVENT("unhandled event");
+// case EV_EXIT:
+// break;
+// }
+// }
+
+// FSM_STATE(rush_the_corn)
+// {
+// switch(FSM_EVENT) {
+// case EV_ENTRY:
+// double x;
+// if (robot.team_color == BLUE) {
+// x = corn_to_get->position.x;
+// } else {
+// x = PLAYGROUND_WIDTH_M - corn_to_get->position.x;
+// }
+// ul_logdeb("x = %.3f, %.3f \n", x, corn_to_get->position.y);
+// remove_wall_around_corn(x, corn_to_get->position.y);
+// robot_goto_trans(PLAYGROUND_WIDTH_M - 0.4, 0.15, ARRIVE_FROM(DEG2RAD(-90), 0.02), &tcFast);
+// where_to_go = TURN_AROUND;
+// break;
+// case EV_MOTION_DONE:
+// FSM_TRANSITION(experiment_decider);
+// break;
+// case EV_START:
+// case EV_TIMER:
+// case EV_RETURN:
+// case EV_VIDLE_DONE:
+// case EV_MOTION_ERROR:
+// case EV_SWITCH_STRATEGY:
+// DBG_PRINT_EVENT("unhandled event");
+// case EV_EXIT:
+// break;
+// }
+// }
+
+// // used to perform the maneuvre
+// FSM_STATE(turn_around)
+// {
+// switch(FSM_EVENT) {
+// case EV_ENTRY:
+// robot_trajectory_new_backward(&tcFast);
+// robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M-0.35, 0.45, TURN_CCW(90));
+// break;
+// case EV_MOTION_DONE:
+// where_to_go = CORN;
+// FSM_TRANSITION(experiment_decider);
+// break;
+// case EV_START:
+// case EV_TIMER:
+// case EV_RETURN:
+// case EV_VIDLE_DONE:
+// case EV_MOTION_ERROR:
+// case EV_SWITCH_STRATEGY:
+// DBG_PRINT_EVENT("unhandled event");
+// case EV_EXIT:
+// break;
+// }
+// }
/************************************************************************
projects / eurobot / public.git / commitdiff