fsmdisplay: functions for new buttons

diff --git a/src/robofsm/eb2008/fsmdisplay.cc b/src/robofsm/eb2008/fsmdisplay.cc
index 6613873f2ac88c46b4b01f53360cfbbc991ce90e..da5be00f1f583478f196cff9e90e7cc826de3998 100644 (file)
--- a/src/robofsm/eb2008/fsmdisplay.cc
+++ b/src/robofsm/eb2008/fsmdisplay.cc
@@ -32,6 +32,11 @@ bool msg_waiting;
 bool wait_flag;
 uint8_t mode_to_go;
 
+enum {
+       LEFT=0,
+       RIGHT
+}
+
 void serial_comm(int status)
 {
        uint8_t buff[10];
@@ -124,8 +129,39 @@ int set_actuators(uint8_t actuator)
        return 0;
 }
 
+int set_game_color(uint8_t col)
+{
+       return 0;
+}
+
+int move_carousel(uint8_t dir)
+{
+       uint8_t act_car_pos, new_car_pos;
+       ROBOT_LOCK(carousel);
+       act_car_pos = robot.orte.drives.carousel_pos;
+       ROBOT_UNLOCK(carousel);
+       switch(dir) {
+               case LEFT:
+                       if(act_car_pos==0)
+                               new_car_pos = 4;
+                       else
+                               new_car_pos = --act_car_pos;
+                       break;
+               case RIGHT:
+                       if(act_car_pos==4)
+                               new_car_pos = 0;
+                       else
+                               new_car_pos = ++act_car_pos;
+                       break;
+               default:
+                       break;
+       }
+       ROBOT_LOCK(carousel);
+       robot.orte.drives.carousel_pos = new_car_pos;
+       ROBOT_UNLOCK(carousel);
+}
+
 int process_msg(uint8_t *buff) {
-       printf("buff[1]=%d\n",buff[1]);
        switch(buff[1]) {
                case SWITCH_TO_STATUS:
                        uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_STATUS);
@@ -148,6 +184,15 @@ int process_msg(uint8_t *buff) {
                case ACTUATORS:
                        set_actuators(buff[2]);
                        break;
+               case SET_COLOR:
+                       set_game_color(buff[2]);
+                       break;
+               case CAR_LEFT:
+                       move_carousel(LEFT);
+                       break;
+               case CAR_RIGHT:
+                       move_carousel(RIGHT);
+                       break;
                default:
                        break;
        }