+++ /dev/null
-/**
- * @file fsmdisplay.cc
- * @brief FSM for uOLED display
- * @author Martin Zidek
- *
- */
-#define FSM_DISP
-
-#include <robodata.h>
-#include <robot.h>
-#include <signal.h>
-#include <pthread.h>
-#include <unistd.h>
-#include <uoled.h>
-#include <string.h>
-#include <oledlib.h>
-#include <actuators.h>
-
-#define TRANS_LEN 300
-
-#ifdef DISPLAY_DEBUG
- #define DBG(format, ...) printf(format, ##__VA_ARGS__)
-#else
- #define DBG(format, ...)
-#endif
-
-/**
- * A thread for serial line communication.
- *
- * This thread is waititng for a character to be intercepted
- * on the serial line, then the character is evaluated
- * and appropriate EVENT is issued to inform the automaton.
- * @param arg
- *
- * @return
- */
-
-struct sercom_data *sercom;
-uint8_t msg[10];
-bool msg_waiting;
-bool wait_flag;
-uint8_t mode_to_go;
-
-enum {
- LEFT=0,
- RIGHT
-};
-
-void serial_comm(int status)
-{
- uint8_t buff[10];
- int nr;
-
- memset(buff, 0, 10);
- nr = read(sercom->fd, buff, 10);
-
- if(buff[0]==MSG_START_DISP) {
- ROBOT_LOCK(disp);
- memcpy(msg,buff,10);
- msg_waiting = true;
- ROBOT_UNLOCK(disp);
-// printf("received serial msg: \n");
-// for(i=0;i<nr;i++)
-// printf("%d ",msg[i]);
-// printf("\n");
- }
-
-
-}
-
-int set_actuators(uint8_t actuator)
-{
- switch(actuator) {
-// case BRUSH_SERVOS:
-#if 0
- if(robot.orte.servos.brush_left==BRUSH_LEFT_OPEN) {
- brushes_in();
- }
- else {
- brushes_out();
- }
-#endif
- break;
- default:
- break;
- }
- return 0;
-}
-
-void set_game_color(uint8_t col)
-{
- if (col == BLUE)
- robot.team_color = BLUE;
- else
- robot.team_color = YELLOW;
-}
-
-int process_msg(uint8_t *buff) {
- switch(buff[1]) {
- case SWITCH_TO_STATUS:
- uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_STATUS);
- wait_flag = true;
- mode_to_go = MODE_STATUS;
- return MODE_STATUS;
- break;
- case SWITCH_TO_CONTROL:
- uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_CONTROL);
- wait_flag = true;
- mode_to_go = MODE_CONTROL;
- return MODE_CONTROL;
- break;
- case SWITCH_TO_STATUS_DONE:
- wait_flag = false;
- mode_to_go = MODE_STATUS;
- return MODE_STATUS;
- break;
- case SWITCH_TO_CONTROL_DONE:
- wait_flag = false;
- mode_to_go = MODE_CONTROL;
- return MODE_CONTROL;
- break;
- case MSG_COLOR:
- printf("msg color: %d\n",buff[2]);
- set_game_color(buff[2]);
- break;
- case MSG_LIFT_CAL:
- printf("msg lift cal\n");
- break;
- /* Eurobot 2009:
- case MSG_PUSHER_IN:
- act_pusher(PUSHER_INSIDE);
- break;
- case MSG_PUSHER_OUT:
- act_pusher(PUSHER_OUTSIDE);
- break;
- case MSG_LIFT_UP:
- printf("msg lift up\n");
- break;
- case MSG_LIFT_DOWN:
- printf("msg lift down\n");
- break;
- */
- case MSG_CMU_INIT:
- printf("msg cmu init\n");
- break;
- case MSG_START_ACT:
- printf("dislay: start\n");
- FSM_SIGNAL(MAIN, EV_START, NULL);
- break;
- case MSG_INIT: /* FIXME: EXIT */
- printf("display: robot_exit\n");
- robot_exit();
- break;
- case MSG_STATUS:
- printf("display: status\n");
- break;
- case MSG_RESTART: /* FIXME: RESTART ALL */
- printf("display: restart\n");
- system("killall ortemanager");
- system("killall cand");
- system("killall hokuyo");
- robot_exit();
- break;
- default:
- break;
- }
- return -1;
-}
-
-
-FSM_STATE_DECL(init);
-FSM_STATE_DECL(control);
-FSM_STATE_DECL(status);
-FSM_STATE_DECL(set_init_mode);
-FSM_STATE_DECL(wait_for_mode_switch);
-
-FSM_STATE(init)
-{
-
- if (FSM_EVENT == EV_ENTRY) {
- msg_waiting = false;
- sercom = robot.sercom;
-#ifdef DISPLAY_DEBUG
- fsm->debug_states = 1;
-#endif
- FSM_TRANSITION(status);
- }
-}
-
-/* FIXME: this state is unused */
-FSM_STATE(set_init_mode)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- DBG("switching mode\n");
- uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_STATUS);
- DBG("mode switched\n");
- FSM_TIMER(500);
- break;
- case EV_TIMER:
- DBG("timer hit\n");
- FSM_TRANSITION(status);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(status)
-{
- uint8_t buff[10];
- uint8_t ret;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_STATUS);
- DBG("entry to status\n");
- FSM_TIMER(TRANS_LEN);
- break;
- case EV_TIMER:
- ret = uoled_send_status(&robot);
- if(ret == -1)
- printf("send failed\n");
- ROBOT_LOCK(disp);
- if(msg_waiting) {
- memcpy(buff, msg, 10);
- msg_waiting = false;
- ROBOT_UNLOCK(disp);
- ret = process_msg(buff);
- switch(ret) {
- case MODE_CONTROL:
-// mode_to_go = MODE_CONTROL;
-// wait_flag = false;
- FSM_TRANSITION(wait_for_mode_switch);
- break;
- default:
- break;
- }
- }
- else {
- ROBOT_UNLOCK(disp);
- }
- FSM_TIMER(TRANS_LEN);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(control)
-{
- uint8_t buff[10];
- int ret;
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- uoled_switch_mode_rep(MODE_CONTROL, CHANGE_MODE_STATUS);
- FSM_TIMER(TRANS_LEN);
- break;
- case EV_TIMER:
- ret = uoled_send_control(&robot);
- ROBOT_LOCK(disp);
- if(msg_waiting) {
- memcpy(buff, msg, 10);
- msg_waiting = false;
- ROBOT_UNLOCK(disp);
- switch(process_msg(buff)) {
- case MODE_STATUS:
-// mode_to_go = MODE_STATUS;
-// wait_flag = false;
- FSM_TRANSITION(wait_for_mode_switch);
- break;
- default:
- break;
- }
- }
- else
- ROBOT_UNLOCK(disp);
- FSM_TIMER(1000);
- break;
- default:
- break;
- }
-}
-
-
-FSM_STATE(wait_for_mode_switch)
-{
- uint8_t buff[10];
-
- switch (FSM_EVENT) {
- case EV_ENTRY:
- FSM_TIMER(500);
- break;
- case EV_TIMER:
- ROBOT_LOCK(disp);
- if(msg_waiting) {
- memcpy(buff, msg, 10);
- msg_waiting = false;
- ROBOT_UNLOCK(disp);
- process_msg(buff);
- }
- else {
- ROBOT_UNLOCK(disp);
- }
- if(wait_flag) {
- uoled_switch_mode_rep(mode_to_go, CHANGE_MODE_STATUS);
- FSM_TIMER(500);
- break;
- }
- else {
- if(mode_to_go==MODE_STATUS)
- FSM_TRANSITION(status);
- else if(mode_to_go==MODE_CONTROL)
- FSM_TRANSITION(control);
- }
- break;
- default:
- break;
- }
-}
-
projects / eurobot / public.git / commitdiff