fsmmain.cc

/*
 * mainfsm.c                    07/04/20
 *
 * Finite state machine of the main part.
 *
 * Copyright: (c) 2007 DCE Eurobot Dragon Team
 *            CTU FEE - Department of Control Engineering
 * License: GNU GPL v.2
 */

#define FSM_MAIN
#include "robot.h"
#include <fsm.h>
#include "fsmmain.h"
#include <unistd.h>
#include "fsmmove.h"
#include "serva.h"
#include <math.h>
#include "trgen.h"
#include "movehelper.h"

#define WAIT_TRANSPORTER_FORWARD_MS     1000
#define WAIT_TRANSPORTER_BACKWARD_MS    1000
#define WAIT_TRANSPORTER_STOP_MS        1000

#define WAIT_WASTE_PULLED_IN_MS         100

#define WAIT_DOOR_OPEN_MS               1000
#define WAIT_PREPARE_FLUSHING_MS        1000
#define WAIT_FLUSHING_BOTTLE_MS         1000
#define WAIT_FLUSHING_CAN_MS            1000

enum {
        BOTTLE = 0,
        CAN,
        /* front transporter status */
        FT_STOP,
        FT_FORWARD,
        FT_BACKWARD,
        FD_UP,
        /*  */
        WASTE_NOT_PULLED_IN,
        WASTE_WAIT_PULLING_IN,
        WASTE_PULLED_IN
};

void *wait_for_end(void *arg)
{
        //sleep(90);
        //robot_exit();
        return NULL;
}

int fsm_main_init(struct robo_fsm *fsm, int ret_code);

//#define HOMOLOGACE
//#define SHOW
#define DEMO

FSM_STATE_DECL(plan_go);
FSM_STATE(main_init) {
        pthread_t thid;

        if (/*1 ||*/ FSM_EVENT == EV_START) {
                pthread_create(&thid, NULL, wait_for_end, NULL);
                releaseFront();         // opens robot's front transporter
                frontTransporterForward();
                FSM_SIGNAL(PICKUP, EV_PICKUP_START);


                robot.act_pos.x = (double)ROBOT_AXIS_TO_BACK_MM/1000.0;
                robot.act_pos.y = ((double)PLAYGROUND_HEIGHT_MM - ROBOT_WIDTH_MM/2.0)/1000.0;
                robot.act_pos.phi = -90.0/180*M_PI;
                printf("fsmmain going to plan_go\n");
                FSM_TRANSITION(plan_go);
        }
}


FSM_STATE_DECL(plan_go);



// --------------------------------------------
void avoid_enemy()
{
        robot_trajectory_stop();
        ROBOT_LOCK(act_pos);
        double x = robot.act_pos.x, y = robot.act_pos.y;
        double heading = robot.act_pos.phi;
        ROBOT_UNLOCK(act_pos);
        
        y+=0.3*sin(heading);
        
        robot_trajectory_new(NULL);
        robot_trajectory_add_point(x + 0.5*cos(heading-M_PI/2.0),
                            y + 0.5*sin(heading-M_PI/2.0));
        robot_trajectory_add_point(x + 0.7*cos(heading-M_PI/4.0),
                            y + 0.7*sin(heading-M_PI/4.0));
        robot_trajectory_add_final_point(target_x, target_y, NAN);
}

void follow_bottle()
{
        robot_trajectory_stop();
        
}
        

FSM_STATE(enemy_avoidance)
{
        switch (FSM_EVENT) {
                case EV_DETECTED_BOTTLE:
                        follow_bottle();
                        break;
                case EV_ENEMY_AHEAD:
                        avoid_enemy();
                        break;
                case EV_GOAL_NOT_REACHABLE:
                case EV_MOTION_DONE:
                        SUBFSM_RET();
                default:
                        break;
        }
}


FSM_STATE_DECL(main_wait);

FSM_STATE(plan_go)
{
        struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        tc.maxv *= 1.5;
//                      robot_trajectory_new(&tc, 0);
//                      robot_trajectory_add_point(robot.act_pos.x, robot.act_pos.y);
//                      robot_trajectory_add_point(robot.act_pos.x+1.0, robot.act_pos.y);
//                      robot_trajectory_add_point(robot.act_pos.x+1.0, robot.act_pos.y+1.0);
//                      robot_trajectory_add_point(robot.act_pos.x, robot.act_pos.y+1.0);
//                      robot_trajectory_add_final_point(robot.act_pos.x, robot.act_pos.y);
//                      FSM_TRANSITION(main_wait);
                        break;
                case EV_RETURN:         // sem se dostanu kdyz projedu cestu, nebo najdu nepritele
                        break;
                        default: break;
        }
}

FSM_STATE_DECL(main_follow_bott);

FSM_STATE(main_wait)
{
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        break;
                case EV_DETECTED_BOTTLE:
                        robot_trajectory_stop();
                        FSM_TRANSITION(main_follow_bott);
                        break;
                case EV_GOAL_NOT_REACHABLE:
                case EV_MOTION_DONE:
                        FSM_TRANSITION(plan_go);
                        break;
                default: break;
        }
}

FSM_STATE_DECL(main_grab);
FSM_STATE(main_follow_bott)
{
        FSM_TIMEOUT(200);
        int i = 0;
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                case EV_TIMEOUT:
                        
                        if(robot.bott_sens.sens1==CLOSE)  i++;
                        if(robot.bott_sens.sens2==CLOSE)  i++;
                        if(robot.bott_sens.sens3==CLOSE)  i++;
                        if(robot.bott_sens.sens4==CLOSE)  i++;
                        
                        
                        if(robot.bott_sens.sens1==FAR) robot_send_speed(0.05,0.1);
                        else if(robot.bott_sens.sens4==FAR) robot_send_speed(0.1,0.05);
                        else if(robot.bott_sens.sens2==FAR) robot_send_speed(0.1,0.1);
                        else if(robot.bott_sens.sens3==FAR) robot_send_speed(0.1,0.1);
                        else if(i >1) FSM_TRANSITION(main_grab);
                        else FSM_TRANSITION(plan_go);
                        break;
                        default: break;
        }
}

FSM_STATE(main_grab)
{
        FSM_TIMEOUT(200);
        
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        robot_send_speed(0.0,0.0);
                        FSM_SIGNAL(PICKUP, EV_PICKUP_TRY);
                        break;
                case EV_PICKUP_DONE:
                        FSM_TRANSITION(plan_go);
                        break;
                case EV_PICKUP_FAILED:
                        FSM_TRANSITION(main_follow_bott);
                        break;
                default: break;
        }
}

---