fsmpickup.c

#define FSM_PICKUP
#include "robot.h"
#include <fsm.h>
#include "serva.h"

FSM_STATE_DECL(start_pickup);
FSM_STATE_DECL(pickup_wait);
FSM_STATE_DECL(blbost);

FSM_STATE(start_pickup) {
        switch (FSM_EVENT) {
                case EV_PICKUP_START:
                        releaseFront();
                        frontDoorDown();
                        FSM_TRANSITION(blbost);
                break;
//              case EV_TIMER:
//                      frontTransporterForward();
//                      FSM_TRANSITION(pickup_wait);
//                      break;
                default: break;
        }
}

FSM_STATE(blbost) {
        FSM_TIMER(2000);
        
        switch (FSM_EVENT) {
                case EV_TIMER:
                        frontTransporterForward();
                        FSM_TRANSITION(pickup_wait);
                        break;
                default: break;
        }
        
}

FSM_STATE_DECL(pickup_free);
FSM_STATE_DECL(pickup_wait_for_success);
FSM_STATE(pickup_wait) {
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        break;
                case EV_PICKUP_TRY:
                        frontDoorUp();
                        FSM_TRANSITION(pickup_wait_for_success);
                        break;
                case EV_PICKUP_FREE:
                        FSM_TRANSITION(pickup_free);
                        break;
                        
                default: break;
        }
}

FSM_STATE(pickup_free) {
        FSM_TIMEOUT(1000);
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                if(robot.IRsensors.back != 0);
                
                break;


                default:break;
        }
}


FSM_STATE(pickup_wait_for_success) {
        FSM_TIMEOUT(1000);
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        break;
                case EV_TIMEOUT:
                        if(robot.frontDoorAngle.state>0x220) {
                                FSM_SIGNAL(MAIN, EV_PICKUP_DONE);
                        }
                        else    FSM_SIGNAL(MAIN, EV_PICKUP_FAILED);
                        frontDoorDown();
                        FSM_TRANSITION(pickup_wait);
                        break;
                default: break;
        }
}





FSM_STATE(pickup_ready) {
        FSM_TIMEOUT(300);
        ROBOT_LOCK(sharpsWaste);
        robot.waste_cnt = 0;
        if(robot.sharpsWaste.short1 > 850) robot.waste_cnt++;
        if(robot.sharpsWaste.short2 > 850) robot.waste_cnt++;
        if(robot.sharpsWaste.short3 > 850) robot.waste_cnt++;
        if(robot.sharpsWaste.short4 > 850) robot.waste_cnt++;
        
        printf("waste_cnt=%d\n",robot.waste_cnt);
        
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                case EV_TIMEOUT:
                        printf("EVENT_STATE_ENTERED - pickup ready\n");
                        if(robot.waste_cnt > 1) robot.bottle_under_belt = 1;
                        else if(robot.waste_cnt==1) {
                                frontTransporterBackward();
                                robot.bottle_under_belt = 0;
                                FSM_TIMER(2000);
                                break;  
                        }
                        else robot.bottle_under_belt = 0;
                        break;
                case EV_TIMER:
                        printf("EV_TIMER - pickup ready\n");
                        frontTransporterForward();
                        break;
                default:
                        DBG_PRINT_EVENT("Unhandled event:");
        }       
        
        if (robot.bottle_under_belt) {          //FIXME Jirkovi se to nelibi, at to laskave nekdo opravi
                frontTransporterForward();
                robot.bottle_under_belt = 0;
                ROBOT_UNLOCK(sharpsWaste);
//              FSM_TRANSITION(try_pickup);
        } else {
                ROBOT_UNLOCK(sharpsWaste);
//              frontTransporterForward();
        }
        
/*      if (robot.bottle_under_belt) {
                robot.bottle_under_belt = 0;
                ROBOT_UNLOCK();
                FSM_TRANSITION(try_pickup);
        } else {
                ROBOT_UNLOCK();
                frontTransporterForward();
        }*/
}

FSM_STATE_DECL(finish_pickup);
FSM_STATE_DECL(jam);
FSM_STATE(try_pickup) {
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        frontDoorUp();
                        FSM_TIMER(500);
                        break;
                case EV_TIMER:
                        if (/* TODO robot.frontDoorAngle*/ 0x221 < 0x220)
                                FSM_TRANSITION(jam);
                        else
                                FSM_TRANSITION(finish_pickup);
                        break;
                case EV_TRASH_GOES_UP:
                        FSM_TRANSITION(finish_pickup);
                        break;
                default:
                        DBG_PRINT_EVENT("Unhandled event");
        }
}

FSM_STATE(finish_pickup) {
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        printf("front door down - finnish pickup\n");
                        frontDoorDown();
                        FSM_TIMER(1000);
                        break;
                case EV_TIMER:
                        //robot.trash_count++;
                        //frontTransporterStop();
//                      fsm_signal(FSM(MOT),EV_FINISHED_PICKUP);
                        FSM_TRANSITION(pickup_ready);
                default: break;
        }
}

FSM_STATE(jam) {
        switch (FSM_EVENT) {
                case EV_STATE_ENTERED:
                        frontDoorDown();
                        frontTransporterBackward();
                        FSM_TIMER(1500);
                        //fsm_signal(FSM(MAIN), EV_JAM);
                        break;
                case EV_TIMER:
                        FSM_TRANSITION(pickup_ready);
                        break;
                default: break;
        }
}

---