robofsm: Do not allow start before every system components are ready.

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index beac2ad0e30b22eb8afa692ff06506dcecc71a4a..bd0b7bf2ac14c726e7f265ea65f060843e11208a 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -165,11 +165,11 @@ void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
                        /* wake up motion-control/thread_trajectory_follower */
                        sem_post(&measurement_received);
 
-                       //robot.hw_status[COMPONENT_ODO] = STATUS_OK;
+                       robot.status[COMPONENT_ODOMETRY] = STATUS_OK;
                        break;
                case DEADLINE:
                        robot.indep_odometry_works = false;
-                       //robot.hw_status[COMPONENT_ODO] = STATUS_FAILED;
+                       robot.status[COMPONENT_ODOMETRY] = STATUS_FAILED;
                        DBG("ORTE deadline occurred - odo_data receive\n");
                        break;
        }
@@ -290,8 +290,8 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
                         * pluggin in and out start connector. */
                        switch (robot.start_state) {
                                case POWER_ON:
+                                        robot.status[COMPONENT_START] = STATUS_WARNING;
                                        if (instance->start_condition == START_PLUGGED_IN) {
-                                               robot.status[COMPONENT_START] = STATUS_WARNING;
                                                robot.start_state = START_PLUGGED_IN;
                                                ul_logmsg("START_PLUGGED_IN\n");
                                        }
@@ -299,8 +299,8 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
 
                                case START_PLUGGED_IN:
                                        robot.status[COMPONENT_START] = STATUS_OK;
-                                       /* Competition starts when plugged out */
-                                       if (instance->start_condition == COMPETITION_STARTED) {
+                                       /* Competition starts when plugged out, check component status before start */
+                                       if ((instance->start_condition == COMPETITION_STARTED) && !check_prestart_status()) {
                                                FSM_SIGNAL(MAIN, EV_START, NULL);
                                                robot.start_state = COMPETITION_STARTED;
                                                ul_logmsg("STARTED\n");
@@ -308,10 +308,10 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
                                        break;
 
                                case COMPETITION_STARTED: {
+                                        robot.status[COMPONENT_START] = STATUS_WARNING;
                                        /* Subsequent plug-in stops the robot */
                                        static int num = 0;
                                        if (instance->start_condition == START_PLUGGED_IN) {
-                                               robot.status[COMPONENT_START] = STATUS_WARNING;
                                                if (++num == 10)
                                                        robot_exit();
                                        }
@@ -352,7 +352,6 @@ void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
        }
 }
 
-// FIXME use this for recieve data from target recognition
 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {