fsmdet.c

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

#define FSM_DETECT
#include "robot.h"
#include "fsm.h"
#include "fsmmain.h"
#include "movehelper.h"
#include "serva.h"
#include <math.h>

#define WASTE_STACK_EMPTYm              0xe0
#define WASTE_INNER_STACK_BOTTLE1       0x1e
#define WASTE_INNER_STACK_BOTTLE2       0x0f

#define CLOSE_LIMIT 850
#define FAR_LIMIT 650

#define VMAX 0.5;
//#define JOYSTICK_NAV 1
#define R 0.15

void evaluate_distance(uint16_t value, uint8_t *result) {
        if((value > CLOSE_LIMIT)) *result = CLOSE;
        else if((value >= FAR_LIMIT)&&(value < CLOSE_LIMIT)) *result = FAR;
        else if(value < FAR_LIMIT) *result = NOTHING;
}

FSM_STATE_DECL(det_start);
FSM_STATE_DECL(det_scan);

FSM_STATE(det_start) {
        FSM_TIMEOUT(100);
//      if(robot.start) {
                FSM_SIGNAL(MAIN, EV_START);
                FSM_TRANSITION(det_scan);
//      }
}

FSM_STATE(det_scan) {

        FSM_TIMEOUT(100);
//      ROBOT_LOCK();
        
        robot.waste_cnt = 0;
/*      if(robot.sharpsWaste.short1 > VERY_FAR_LIMIT) robot.waste_cnt++;
        if(robot.sharpsWaste.short2 > VERY_FAR_LIMIT) robot.waste_cnt++;
        if(robot.sharpsWaste.short3 > VERY_FAR_LIMIT) robot.waste_cnt++;
        if(robot.sharpsWaste.short4 > VERY_FAR_LIMIT) robot.waste_cnt++;*/
        
//      printf("waste_cnt=%d\n",robot.waste_cnt);
        
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        break;
                case EV_TIMEOUT:
                        //printf("EVENT_STATE_ENTERED - pickup ready\n");
                        if(robot.waste_cnt) {
                                evaluate_distance(robot.sharpsWaste.short1, &robot.bott_sens.sens1);
                                evaluate_distance(robot.sharpsWaste.short2, &robot.bott_sens.sens2);
                                evaluate_distance(robot.sharpsWaste.short3, &robot.bott_sens.sens3);
                                evaluate_distance(robot.sharpsWaste.short4, &robot.bott_sens.sens4);
                                
                                FSM_SIGNAL(MAIN, EV_DETECTED_BOTTLE);
                                break;
                        }
                        
                // parsing joy buttons

                        if(robot.joy.button1 == 1) innerTransporterStop();
                        if(robot.joy.button2 == 1) frontTransporterStop();
                        if(robot.joy.button3 == 1) frontDoorDown();
                        if(robot.joy.button4 == 1) frontTransporterBackward();
                        if(robot.joy.button5 == 1) frontDoorUp();
                        if(robot.joy.button6 == 1) frontTransporterForward();
                        if(robot.joy.button7 == 1) innerDoorDown();
                        if(robot.joy.button8 == 1) innerDoorUp();
                        if(robot.joy.button9 == 1) backDoorDown();
                        if(robot.joy.button10 == 1) backDoorUp();
                        if(robot.joy.button11 == 1) innerTransporterLeft();
                        if(robot.joy.button12 == 1) innerTransporterRight();

#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 >= 1)                // If lenght si more than 1 is used unit circle
                        {
                                angle = tan2(robot.joy.axisX/robot.joy.axisY);  // computing new speed and omega
                                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:");
        }       
        
//      ROBOT_UNLOCK();
}

/*
                // definovat nove ev na joy
        EV_JOY_B0
        EV_JOY_B1               
        EV_JOY_B2
        EV_JOY_B0
        EV_JOY_B4
        EV_JOY_B5
        EV_JOY_B6
        EV_JOY_B7
        EV_JOY_B8
        EV_JOY_B9
        EV_JOY_B10
        EV_JOY_B11



*/

---