# -*- makefile -*-
-SUBDIRS = utils test
+SUBDIRS = test
default_CONFIG = CONFIG_OPEN_LOOP=n CONFIG_LOCK_CHECKING=n
# Library with general support functions for the robot
lib_LIBRARIES += robot
-robot_SOURCES = robot_orte.c servos.c robot.c fsmmove.cc fsmjoy.c \
+robot_SOURCES = robot_orte.c servos.c robot.c fsmmove.cc \
movehelper.cc fsmdisplay.c fsmloc.c map_handling.c
robot_GEN_SOURCES = roboevent.c
include_GEN_HEADERS += roboevent.h
case DEPOSITE_BALLS:
FSM_SIGNAL(MAIN, EV_DEPOSITE_BALLS, NULL);
break;
- case FDOOR_SERVO:
- if(robot.orte.servos.door_bottom==BOTTOM_DOOR_OPEN) {
- close_bottom_door();
- }
- else {
- open_bottom_door();
- }
- break;
- case BDOOR_SERVO:
- if(robot.orte.servos.door_back==BACK_DOOR_OPEN) {
- close_back_door();
- }
- else {
- open_back_door();
- }
- break;
case BRUSH_DRIVES:
if(robot.orte.drives.brush_left==LEFT_BRUSH_DRIVE_ON) {
brushes_drives_out();
ROBOT_UNLOCK(state);
}
break;
- case BAGR_DRIVE:
- if(robot.orte.drives.bagr==BAGR_DRIVE_ON) {
- off_bagr();
- }
- else {
- on_bagr();
- }
- break;
default:
break;
}
+++ /dev/null
-/*
- * fsmjoy.c 08/04/20
- *
- * Finite state machine of the joystick control.
- *
- * Copyright: (c) 2008 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#define FSM_JOY
-#include <robodata.h>
-#include <robot.h>
-#include <fsm.h>
-#include <movehelper.h>
-#include <servos.h>
-#include <math.h>
-
-#define CLOSE_LIMIT 850
-#define FAR_LIMIT 650
-
-#define VMAX 0.5;
-#define R 0.15
-
-//#define JOYSTICK_NAV
-
-#define JOY_BUTTONS 12
-#define JOY_AXIS 6
-
-static unsigned char buttons[JOY_BUTTONS];
-static unsigned char buttons_prev[JOY_BUTTONS];
-static unsigned char axis[JOY_AXIS];
-static unsigned char axis_prev[JOY_AXIS];
-static unsigned char carousel_position = 0;
-static unsigned int prev_axisR = 0;
-
-FSM_STATE_DECL(init);
-FSM_STATE_DECL(scan);
-
-FSM_STATE(init)
-{
- int i;
-
- printf("joystick control initialization\n");
- for(i=0; i<JOY_BUTTONS; i++) {
- buttons[i] = 0;
- buttons_prev[i] = 0;
- }
- for(i=0; i<JOY_AXIS; i++) {
- axis[i] = 0;
- axis_prev[i] = 0;
- }
- FSM_TRANSITION(scan);
-}
-
-FSM_STATE(scan)
-{
- FSM_TIMER(100);
- switch (FSM_EVENT) {
- case EV_ENTRY:
- break;
- case EV_TIMER:
- /* button1 - turn off all drives */
- if (robot.joy_data.button1 == 1) {
- if(buttons_prev[0]!=1) {
- buttons[0] = ~buttons[0];
- buttons_prev[0] = 1;
- }
- off_brush_left();
- off_brush_right();
- off_bagr();
- }
- else
- buttons_prev[0] = 0;
-
- /* button2 - open/close brush left/right */
- if (robot.joy_data.button2 == 1) {
- if(buttons_prev[1]!=1) {
- buttons[1] = ~buttons[1];
- buttons_prev[1] = 1;
- }
- if (buttons[1]) {
- open_brush_left();
- open_brush_right();
- } else {
- close_brush_left();
- close_brush_right();
- }
- }
- else
- buttons_prev[1] = 0;
-
- /* button3 - open close back door */
- if (robot.joy_data.button3 == 1) {
- if(buttons_prev[2]!=1) {
- buttons[2] = ~buttons[2];
- buttons_prev[2] = 1;
- }
- if (buttons[2])
- open_back_door();
- else
- close_back_door();
- }
- else
- buttons_prev[2] = 0;
-
- /* button4 - */
- if (robot.joy_data.button4 == 1) {
- if(buttons_prev[3]!=1) {
- buttons[3] = ~buttons[3];
- buttons_prev[3] = 1;
- }
- }
- else
- buttons_prev[3] = 0;
-
- /* button5 - */
- if (robot.joy_data.button5 == 1) {
- if(buttons_prev[4]!=1) {
- buttons[4] = ~buttons[4];
- buttons_prev[4] = 1;
- }
- if (buttons[4])
- open_bottom_door();
- else
- close_bottom_door();
- }
- else
- buttons_prev[4] = 0;
-
- /* button6 - turn brush drives off */
- if (robot.joy_data.button6 == 1) {
- if(buttons_prev[5]!=1) {
- buttons[5] = ~buttons[5];
- buttons_prev[5] = 1;
- }
- off_brush_left();
- off_brush_right();
- }
- else
- buttons_prev[5] = 0;
-
- /* button7 */
- if (robot.joy_data.button7 == 1) {
- if(buttons_prev[6]!=1) {
- buttons[6] = ~buttons[6];
- buttons_prev[6] = 1;
- }
- }
- else
- buttons_prev[6] = 0;
-
- /* button8 */
- if (robot.joy_data.button8 == 1) {
- if(buttons_prev[7]!=1) {
- buttons[7] = ~buttons[7];
- buttons_prev[7] = 1;
- }
- }
- else
- buttons_prev[7] = 0;
-
- /* button9 */
- if (robot.joy_data.button9 == 1) {
- if(buttons_prev[8]!=1) {
- buttons[8] = ~buttons[8];
- buttons_prev[8] = 1;
- }
- }
- else
- buttons_prev[8] = 0;
-
- /* button10 */
- if (robot.joy_data.button10 == 1) {
- if(buttons_prev[9]!=1) {
- buttons[9] = ~buttons[9];
- buttons_prev[9] = 1;
- }
- }
- else
- buttons_prev[9] = 0;
-
- /* button11 */
- if (robot.joy_data.button11 == 1) {
- if(buttons_prev[10]!=1) {
- buttons[10] = ~buttons[10];
- buttons_prev[10] = 1;
- }
- }
- else
- buttons_prev[10] = 0;
-
- /* button12 */
- if (robot.joy_data.button12 == 1) {
- if(buttons_prev[11]!=1) {
- buttons[11] = ~buttons[11];
- buttons_prev[11] = 1;
- }
- }
- else
- buttons_prev[11] = 0;
-
- /* axis - carousel left/right */
- if (robot.joy_data.axisS1 == -32767) {
- if(axis_prev[3] != 1) {
- axis[3] = ~axis[3];
- axis_prev[3] = 1;
- if (carousel_position > 0) {
- carousel_position--;
- set_carousel(carousel_position);
- }
- }
- } else
- axis_prev[3] = 0;
-
- if (robot.joy_data.axisS1 == 32767) {
- if(axis_prev[4] != 1) {
- axis[4] = ~axis[4];
- axis_prev[4] = 1;
- if (carousel_position < 4) {
- carousel_position++;
- set_carousel(carousel_position);
- }
- }
- } else
- axis_prev[4] = 0;
-
- /* axis - brush in/out */
- if (robot.joy_data.axisS2 == -32767) {
- brushes_drives_in();
- }
-
- if (robot.joy_data.axisS2 == 32767) {
- brushes_drives_out();
- }
-
- if (prev_axisR != robot.joy_data.axisR) {
- set_bagr(150 + (-robot.joy_data.axisR * 50 / 32767));
- prev_axisR = robot.joy_data.axisR;
- }
-
-
-#ifdef JOYSTICK_NAV // if joystick is used for navigating
- v = (double)robot.joy.axisY/32768;
- omega = (double)robot.joy.axisX/32768;
- lenght = sqrt( pow(v,2) + pow(omega,2));
- /* if lenght si more than 1 is used unit circle */
- if (lenght >= 1) {
- /* computing new speed and omega */
- angle = tan2(robot.joy.axisX/robot.joy.axisY);
- v = sin(angle);
- omega = cos(angle);
- }
-
- speedL = (v - R*omega)*VMAX;
- speedR = -(v + R*omega)*VMAX;
- robot_send_speed(speedL, speedR); // forcing speed command
- printf("v %f, omega %f\n",v, omega);//#endif
- printf("length %f, angle %f\n",lenght, angle);//#endif
- printf("axisX %d, axisY %d\n",robot.joy.axisX, robot.joy.axisY);//#endif
- printf("sent speed: %f, %f\n",speedL, speedR);//#endif
-#endif
- break;
- default:
- DBG_PRINT_EVENT("Unhandled event:");
- }
-}
CENTER
};
-int balls_to_collect = 0;
/************************************************************************
* MISC FUNCTIONS
return rv;
}
-int closed_to_container()
-{
- int rv = 0;
-
- ROBOT_LOCK(sharps);
- rv = ((robot.sharps.back_left < 0.05)
- && (robot.sharps.back_right < 0.05));
- ROBOT_UNLOCK(sharps);
-
- /* FIXME: */
- return 1;
-}
-
-int get_ball_number()
-{
- static int cycle = 0;
- int rv = 0;
-
- cycle++;
-
- switch (cycle) {
- case 1:
- case 4:
- rv = 3;
- break;
- case 2:
- case 3:
- rv = 2;
- break;
- default:
- rv = 1;
- break;
- }
-
- return rv;
-}
/**
* Competition timer. Stop robot when the timer exceeds.
/* initial and starting states */
FSM_STATE_DECL(init);
FSM_STATE_DECL(wait_for_start);
-FSM_STATE_DECL(decide_where_now);
/* movement states */
-FSM_STATE_DECL(go_to_our_white_dispenser);
-FSM_STATE_DECL(go_to_our_red_dispenser);
-FSM_STATE_DECL(go_to_opponent_white_dispenser);
-FSM_STATE_DECL(try_to_approach_dispenser);
-FSM_STATE_DECL(go_to_container);
-FSM_STATE_DECL(go_back_to_edge);
-/* ball and carousel states */
-FSM_STATE_DECL(get_balls);
-FSM_STATE_DECL(next_carousel_position);
-FSM_STATE_DECL(deposite_balls);
/************************************************************************
* INITIAL AND STARTING STATES
off_brush_left();
off_brush_right();
brushes_out();
- open_bottom_door();
- close_back_door();
FSM_TRANSITION(wait_for_start);
break;
default:
ROBOT_LOCK(state);
robot.state = COMPETITION_STARTED;
ROBOT_UNLOCK(state);
- FSM_TRANSITION(decide_where_now);
break;
case EV_LASER_POWER:
on_laser();
break;
case EV_DEPOSITE_BALLS:
- SUBFSM_TRANSITION(deposite_balls, NULL);
break;
case EV_RETURN:
break;
}
}
-FSM_STATE(decide_where_now)
-{
- static int cycle = 0;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- cycle++;
- off_bagr();
- off_brush_left();
- off_brush_right();
- if (robot.carousel_cnt >= BALL_TO_COLLECT ||
- robot.carousel_cnt >= CAROUSEL_SIZE) {
- FSM_TRANSITION(go_to_container);
- } else {
- switch (cycle) {
- case 1:
- case 4:
- FSM_TRANSITION(go_to_our_white_dispenser);
- break;
- case 2:
- case 5:
- FSM_TRANSITION(go_to_our_red_dispenser);
- break;
- case 3:
- case 6:
- FSM_TRANSITION(go_to_container);
- break;
- case 7:
- case 8:
- FSM_TRANSITION(go_to_opponent_white_dispenser);
- break;
- default:
- if (cycle % 2) {
- FSM_TRANSITION(go_to_our_white_dispenser);
- } else {
- FSM_TRANSITION(go_to_container);
- }
- break;
- }
- }
- default: break;
- }
-}
/************************************************************************
* MOVEMENT STATES
************************************************************************/
-FSM_STATE(go_to_our_white_dispenser)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(NULL);
- robot_trajectory_add_point_trans(0.7,
- PLAYGROUND_HEIGHT_M - 0.65);
- robot_trajectory_add_final_point_trans(0.35,
- PLAYGROUND_HEIGHT_M - 0.65, NO_TURN());
- break;
- case EV_MOTION_DONE:
- SUBFSM_TRANSITION(try_to_approach_dispenser, NULL);
- break;
- case EV_RETURN:
- balls_to_collect = get_ball_number();
- FSM_TRANSITION(get_balls);
- break;
- default: break;
- }
-}
-
-FSM_STATE(go_to_our_red_dispenser)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(NULL);
- robot_trajectory_add_point_trans(0.7,
- PLAYGROUND_HEIGHT_M - 0.45);
- robot_trajectory_add_final_point_trans(0.7,
- PLAYGROUND_HEIGHT_M - 0.35, NO_TURN());
- break;
- case EV_MOTION_DONE:
- SUBFSM_TRANSITION(try_to_approach_dispenser, NULL);
- break;
- case EV_RETURN:
- balls_to_collect = get_ball_number();
- FSM_TRANSITION(get_balls);
- break;
- default: break;
- }
-}
-
-FSM_STATE(go_to_opponent_white_dispenser)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(NULL);
- robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.7,
- PLAYGROUND_HEIGHT_M - 0.65);
- robot_trajectory_add_final_point_trans(
- PLAYGROUND_WIDTH_M - 0.25,
- PLAYGROUND_HEIGHT_M - 0.7, NO_TURN());
- break;
- case EV_MOTION_DONE:
- SUBFSM_TRANSITION(try_to_approach_dispenser, NULL);
- break;
- case EV_RETURN:
- balls_to_collect = get_ball_number();
- FSM_TRANSITION(get_balls);
- break;
- default: break;
- }
-}
-
-#define APPROACHING_ATTEMPS 2
-
-FSM_STATE(try_to_approach_dispenser)
-{
- static int approaching_attempts = 0;
- static unsigned char backward = 1;
- struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
-
- tc.maxv /= 3;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- if (closed_to_dispenser()) {
- SUBFSM_RET(NULL);
- approaching_attempts = 0;
- } else {
- robot_move_by(0.04, NO_TURN(), &tc);
- backward = 0;
- }
- break;
- case EV_MOTION_DONE:
- if (closed_to_dispenser() || (approaching_attempts++ > APPROACHING_ATTEMPS)) {
- SUBFSM_RET(NULL);
- } else {
- robot_move_by(0.04, NO_TURN(), &tc);
-// if (backward) {
-// FSM_TRANSITION(try_to_approach_dispenser);
-// } else {
-// robot_move_by(-0.02, NO_TURN(), &tc);
-// backward = 1;
-// }
- }
- break;
- default: break;
- }
-}
-
-#define GO_TO_CONTAINER_TIMER 10000
-
-FSM_STATE(go_to_container)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- off_brush_left();
- off_brush_right();
- off_bagr();
- robot_trajectory_new(NULL);
- robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.8, 1.0);
- robot_trajectory_add_final_point_trans(PLAYGROUND_WIDTH_M - 0.6,
- 0.4, TURN(DEG2RAD(90)));
- /*FSM_TIMER(GO_TO_CONTAINER_TIMER);*/
- break;
- case EV_TIMER:
- case EV_MOTION_DONE:
- FSM_TRANSITION(go_back_to_edge);
- break;
- default: break;
- }
-}
-
-FSM_STATE(go_back_to_edge)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- robot_move_by(-0.4, NO_TURN(), NULL);
- break;
- case EV_TIMER:
- case EV_MOTION_DONE:
- if (closed_to_container()) {
- robot.carousel_pos = 0;
- SUBFSM_TRANSITION(deposite_balls, NULL);
- } else {
- DBG("FIXME: go_closer_to_container\n");
- }
- break;
- case EV_RETURN:
- FSM_TRANSITION(decide_where_now);
- break;
- default: break;
- }
-}
-
-/************************************************************************
- * BALLS AND CAROUSEL MANIPULATION STATES
- ************************************************************************/
-
-#define MAX_GET_BALL_ATTEMPS 4
-#define GET_BALL_TIMER 1500
-#define WAIT_BALL_INSIDE 5000
-#define GET_BALL_BAGR_SPEED 120
-
-FSM_STATE(get_balls)
-{
- static int get_ball_attemps = 0;
- static int ball_inside = 0;
- static int last_get = 0;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- case EV_RETURN:
- if (last_get) {
- robot_move_by(-0.2, NO_TURN(), NULL);
- last_get = 0;
- } else {
- get_ball_attemps = 0;
- DBG("balls_to_collect = %d\n", balls_to_collect);
- open_bottom_door();
- set_bagr(GET_BALL_BAGR_SPEED);
- brushes_drives_in();
- FSM_TIMER(500);
- }
- break;
- case EV_TIMER:
- if (ball_inside) {
- ball_inside = 0;
- close_bottom_door();
- off_bagr();
- if (robot.carousel_cnt >= balls_to_collect) {
- last_get = 1;
- }
- SUBFSM_TRANSITION(
- next_carousel_position, NULL);
- robot.carousel_pos =
- (robot.carousel_pos+2) % CAROUSEL_SIZE;
- } else if (get_ball_attemps++ < MAX_GET_BALL_ATTEMPS) {
- FSM_TIMER(GET_BALL_TIMER);
- } else {
- robot_move_by(-0.2, NO_TURN(), NULL);
- }
- break;
- case EV_BALL_INSIDE:
- /* ball is already inside */
- if (ball_inside)
- break;
-
- ball_inside = 1;
- robot.carousel[robot.carousel_pos % CAROUSEL_SIZE] =
- (enum ball_color)robot.cmu.color;
- robot.carousel_cnt++;
- DBG("collected balls = %d\n", robot.carousel_cnt);
- FSM_TIMER(WAIT_BALL_INSIDE);
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(decide_where_now);
- break;
- default: break;
- }
-}
-
-#define MAX_CAROUSEL_ATTEMPTS 3
-#define CAROUSEL_ATTEMPT_TIMER 1000
-
-FSM_STATE(next_carousel_position)
-{
- static int carousel_attempts;
- int rv;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- carousel_attempts = 0;
- DBG("carousel_pos = %d\n", robot.carousel_pos);
- set_carousel(robot.carousel_pos);
- FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
- break;
- case EV_TIMER:
- carousel_attempts++;
- /* check out, if the carousel reached position */
- ROBOT_LOCK(drives);
- rv = (robot.drives.carousel_pos == robot.carousel_pos);
- ROBOT_UNLOCK(drives);
- if (!rv && carousel_attempts++ < MAX_CAROUSEL_ATTEMPTS) {
- FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
- } else if(rv) {
- DBG("carousel reached the position.\n");
- SUBFSM_RET(NULL);
- } else {
- DBG("FIXME: carousel lost.\n");
- SUBFSM_RET(NULL);
- }
- break;
- default: break;
- }
-}
-
-#define WAIT_FOR_DEPOSITION_TIMER 1500
-
-FSM_STATE(deposite_balls)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- if (robot.carousel_pos < CAROUSEL_SIZE) {
- close_back_door();
- SUBFSM_TRANSITION(next_carousel_position, NULL);
- } else {
- robot.carousel_cnt = 0;
- robot.carousel_pos = 0;
- DBG("carousel_cnt = %d\n", robot.carousel_cnt);
- SUBFSM_RET(NULL);
- }
- break;
- case EV_RETURN:
- open_back_door();
- FSM_TIMER(WAIT_FOR_DEPOSITION_TIMER);
- break;
- case EV_TIMER:
- robot.carousel_pos++;
- close_back_door();
- FSM_TRANSITION(deposite_balls);
- break;
- default: break;
- }
-}
-
robot.orte.pwr_ctrl.voltage80 = 1;
brushes_in();
- open_bottom_door();
- close_top_door();
- close_back_door();
off_brush_left();
off_brush_right();
- off_bagr();
/* MCL initialization */
robot.laser.width = PLAYGROUND_WIDTH_M; /* in meters */
ORTEPublicationSend(robot.orte.publication_laser_cmd);
ORTEPublicationSend(robot.orte.publication_laser_cmd);
- open_back_door();
- open_bottom_door();
- off_bagr();
off_brush_left();
off_brush_right();
robot.orte.motion_speed.right = 0;
#include <servos.h>
/* SERVOS */
-void open_top_door()
-{
- robot.orte.servos.door_top = TOP_DOOR_OPEN;
-}
-
-void close_top_door()
-{
- robot.orte.servos.door_top = TOP_DOOR_CLOSE;
-}
-
-void open_bottom_door()
-{
- robot.orte.servos.door_bottom = BOTTOM_DOOR_OPEN;
-}
-
-void close_bottom_door()
-{
- robot.orte.servos.door_bottom = BOTTOM_DOOR_CLOSE;
-}
-
-void open_back_door()
-{
- robot.orte.servos.door_back = BACK_DOOR_OPEN;
-}
-
-void close_back_door()
-{
- robot.orte.servos.door_back = BACK_DOOR_CLOSE;
-}
-
void open_brush_left()
{
robot.orte.servos.brush_left = BRUSH_LEFT_OPEN;
robot.orte.drives.brush_right = RIGHT_BRUSH_DRIVE_OUT;
}
-void on_bagr()
-{
- robot.orte.drives.bagr = BAGR_DRIVE_ON;
-}
-
-void off_bagr()
-{
- robot.orte.drives.bagr = BAGR_DRIVE_OFF;
-}
-
-void set_bagr(unsigned char speed)
-{
- robot.orte.drives.bagr = speed;
-}
-
-void set_carousel(unsigned char position)
-{
- robot.orte.drives.carousel_pos = position;
-}
-
void on_laser()
{
robot.orte.laser_cmd.speed = LASER_DRIVE_ON;
#ifdef __cplusplus
extern "C" {
#endif
-void open_top_door();
-void close_top_door();
-void open_bottom_door();
-void close_bottom_door();
-void open_back_door();
-void close_back_door();
+
void open_brush_left();
void close_brush_left();
void open_brush_right();
{
off_brush_left();
off_brush_right();
- off_bagr();
sleep(COMPETITION_TIME);
printf("%d seconds timer exceeded! exiting!\n", COMPETITION_TIME);
robot_exit();
off_brush_left();
off_brush_right();
brushes_in();
- open_bottom_door();
- close_back_door();
FSM_TRANSITION(wait_for_start);
// FSM_TRANSITION(go_to_container);
// FSM_TRANSITION(get_balls);
robot.obstacle_avoidance_enabled = true;
off_brush_left();
off_brush_right();
- off_bagr();
robot_goto_trans(PLAYGROUND_WIDTH_M - 0.5, 0.5, NO_TURN(), NULL);
// robot_trajectory_new(NULL);
// robot_trajectory_add_point_trans(PLAYGROUND_WIDTH_M - 0.8, 1.0);
case EV_ENTRY:
case EV_RETURN:
brushes_in();
- open_bottom_door();
- set_bagr(GET_BALL_BAGR_SPEED);
brushes_drives_in();
FSM_TIMER(500);
break;
ball_inside = 1;
if (ball_inside) {
ball_inside = 0;
- close_bottom_door();
- off_bagr();
if (robot.carousel_cnt >= BALL_TO_COLLECT) {
/* FIXME: just to test deposition */
// robot.carousel_pos = 0;
case EV_ENTRY:
carousel_attempts = 0;
DBG("carousel_pos = %d\n", robot.carousel_pos);
- set_carousel(robot.carousel_pos);
FSM_TIMER(CAROUSEL_ATTEMPT_TIMER);
break;
case EV_TIMER:
switch (FSM_EVENT) {
case EV_ENTRY:
if (robot.carousel_pos < 5) {
- close_back_door();
SUBFSM_TRANSITION(next_carousel_position, NULL);
} else {
robot.carousel_cnt = 0;
break;
case EV_RETURN:
DBG("open_back_door\n");
- open_back_door();
FSM_TIMER(WAIT_FOR_DEPOSITION_TIMER);
break;
case EV_TIMER:
robot.carousel_cnt--;
robot.carousel_pos++;
- close_back_door();
DBG("carousel_cnt = %d\n", robot.carousel_cnt);
FSM_TRANSITION(deposite_balls);
break;
+++ /dev/null
-# Generic directory or leaf node makefile for OCERA make framework
-
-ifndef MAKERULES_DIR
-MAKERULES_DIR := $(shell ( old_pwd="" ; while [ ! -e Makefile.rules ] ; do if [ "$$old_pwd" = `pwd` ] ; then exit 1 ; else old_pwd=`pwd` ; cd -L .. 2>/dev/null ; fi ; done ; pwd ) )
-endif
-
-ifeq ($(MAKERULES_DIR),)
-all : default
-.DEFAULT::
- @echo -e "\nThe Makefile.rules has not been found in this or partent directory\n"
-else
-include $(MAKERULES_DIR)/Makefile.rules
-endif
-
+++ /dev/null
-# -*- makefile -*-
-
-bin_PROGRAMS += joystick
-joystick_SOURCES = joystick.cc
-
-# Libraries linked to all programs in this Makefile
-lib_LOADLIBES = robot mcl robodim laser-nav robomath roboorte roboorte \
- robottype robottype pthread rt m orte pathplan sharp map fsm rbtree motion uoled oledlib sercom
+++ /dev/null
-/*
- * joystick.cc 07/10/11
- *
- * Program to control robot using a joystick.
- *
- * Copyright: (c) 2007 CTU Dragons
- * CTU FEE - Department of Control Engineering
- * License: GNU GPL v.2
- */
-
-#define FSM_MAIN
-#include <unistd.h>
-#include <math.h>
-#include <robomath.h>
-#include <sharp.h>
-#include <robot.h>
-#include <fsm.h>
-#include <movehelper.h>
-#include <servos.h>
-#include <trgen.h>
-#include <map.h>
-
-/**
- * FSM states start here.
- */
-FSM_STATE_DECL(init);
-
-/**
- * Set starting position.
- *
- * @ingroup fsmmain
- */
-FSM_STATE(init)
-{
- /* start event ocurred */
- switch (FSM_EVENT) {
- case EV_ENTRY:
- case EV_START:
-// robot_set_act_pos_trans(ROBOT_WIDTH_M/2,
-// PLAYGROUND_HEIGHT_M - ROBOT_AXIS_TO_BACK_M,
-// DEG2RAD(-90));
- robot_set_est_pos_trans(0.0,
- 2.0,
- DEG2RAD(0));
- FSM_TIMER(100);
- break;
- case EV_TIMER:
- FSM_TIMER(100);
- break;
- default: break;
- }
-}
-
-/* main loop */
-int main()
-{
- int rv;
-
- /* robot initialization */
- rv = robot_init();
- if (rv) {
- printf("robot_init(): terminated with errors.\n");
- exit(1);
- }
-
- robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
- robot.fsm[FSM_ID_JOY].state = &fsm_state_joy_init;
-
- /* Start threads and wait */
- rv = robot_start();
- if (rv) {
- printf("robot_start(): terminated with errors.\n");
- exit(1);
- }
-
- robot_wait();
-
- /* clean up */
- robot_destroy();
-
- return 0;
-}
/* FIXME: servos action comes here */
}
-void RobomonAtlantis::moveFrontDoor(int state)
-{
- if (state)
- orte.servos.door_bottom = BOTTOM_DOOR_OPEN;
- else
- orte.servos.door_bottom = BOTTOM_DOOR_CLOSE;
-}
-
-void RobomonAtlantis::moveTopDoor(int state)
-{
- if (state)
- orte.servos.door_top = TOP_DOOR_OPEN;
- else
- orte.servos.door_top = TOP_DOOR_CLOSE;
-}
-
-void RobomonAtlantis::moveBackDoor(int state)
-{
- if (state)
- orte.servos.door_back = BACK_DOOR_OPEN;
- else
- orte.servos.door_back = BACK_DOOR_CLOSE;
-}
-
void RobomonAtlantis::moveLeftBrush(int state)
{
if (state)
void setBagrDriveCB(int value);
void stopMotors();
void moveServos(int value);
- void moveFrontDoor(int state);
- void moveTopDoor(int state);
- void moveBackDoor(int state);
void moveLeftBrush(int state);
void moveRightBrush(int state);
void setDO(int state);
void RobomonTuning::setServos(int value)
{
- orte.servos.door_top = (u_int8_t)servoDoorTop->value();
- servoDoorTopLEdit->setText(QString("%1").arg(orte.servos.door_top));
- orte.servos.door_back = (u_int8_t)servoDoorBack->value();
- servoDoorBackLEdit->setText(QString("%1").arg(orte.servos.door_back));
- orte.servos.door_bottom = (u_int8_t)servoDoorBottom->value();
- servoDoorBottomLEdit->setText(QString("%1").arg(orte.servos.door_bottom));
orte.servos.brush_left = (u_int8_t)servoBrushLeft->value();
servoBrushLeftLEdit->setText(QString("%1").arg(orte.servos.brush_left));
orte.servos.brush_right = (u_int8_t)servoBrushRight->value();
servoBrushRightLEdit->setText(QString("%1").arg(orte.servos.brush_right));
- printf("setting servos: %d, %d, %d, %d, %d\n",orte.servos.door_top,orte.servos.door_back,
- orte.servos.door_bottom,orte.servos.brush_left,orte.servos.brush_right);
-
ORTEPublicationSend(orte.publication_servos);
}
octet brush_right;
octet reserve1;
octet reserve2;
- octet door_bottom; // FIXME to be removed
octet door_top; // FIXME to be removed
octet door_back; // FIXME to be removed
octet reserve; //FIXME to be removed
projects / eurobot / public.git / commitdiff