Robot exits when start plug is plugged in after competition start

[eurobot/public.git] / src / robofsm / robot_orte.c

diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c
index 2735cea1453421dbe7f4edce87f726900bab67d1..18df45280e9c93c579c95eb9469737645f049a1e 100644 (file)
--- a/src/robofsm/robot_orte.c
+++ b/src/robofsm/robot_orte.c
@@ -173,13 +173,13 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
                case NEW_DATA:
                        /* Stupid way of controlling the robot by
                         * pluggin in and out start connector. */
-                       if (instance->start) {
-                               robot.hw_status[STATUS_STA] = HW_STATUS_WARNING;
-                       } else {
-                               robot.hw_status[STATUS_STA] = HW_STATUS_OK;
-                       }
                        switch (robot.state) {
                                case POWER_ON:
+                                       if (instance->start) {
+                                               robot.hw_status[STATUS_STA] = HW_STATUS_WARNING;
+                                       } else {
+                                               robot.hw_status[STATUS_STA] = HW_STATUS_OK;
+                                       }
                                        if (!instance->start) {
                                                robot.state = START_PLUGGED_IN;
                                                printf("START_PLUGGED_IN\n");
@@ -187,6 +187,7 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
                                        break;
 
                                case START_PLUGGED_IN:
+                                       robot.hw_status[STATUS_STA] = HW_STATUS_OK;
                                        /* Competition starts when plugged out */
                                        if (instance->start) {
                                                FSM_SIGNAL(MAIN, EV_START, NULL);
@@ -195,12 +196,16 @@ void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
                                        }
                                        break;
 
-                               case COMPETITION_STARTED:
+                               case COMPETITION_STARTED: {
                                        /* Subsequent plug-in stops the robot */
+                                       static int num = 0;
                                        if (!instance->start) {
-                                               //robot_exit();
+                                               robot.hw_status[STATUS_STA] = HW_STATUS_WARNING;
+                                               if (++num == 10)
+                                                       robot_exit();
                                        }
                                        break;
+                               }
                        }
                        last_instance = *instance;
                        break;