disp: final version

diff --git a/src/disp-4dgl/control_screen.4dg b/src/disp-4dgl/control_screen.4dg
index 338c3cb6571983c94726338d75634805ab81138e..afd781c43137532411965bae1ea9daa5d2dd843e 100644 (file)
--- a/src/disp-4dgl/control_screen.4dg
+++ b/src/disp-4dgl/control_screen.4dg
@@ -351,7 +351,21 @@ endfunc
 \r
 func process_msg()\r
     if(msg_rcvd[0]==200)\r
-        load_status();\r
+        if(msg_rcvd[2]==1)\r
+            load_status();\r
+        endif\r
+    endif\r
+    if(msg_rcvd[0]=='c')\r
+        if(msg_rcvd[1]==COLOR_RED)\r
+            current_color := COLOR_RED;\r
+            button_color := RED;\r
+        else\r
+            if(msg_rcvd[1]==COLOR_GREEN)\r
+                current_color := COLOR_GREEN;\r
+                button_color := GREEN;\r
+            endif\r
+        endif\r
+        color_bt(UP);\r
     endif\r
 endfunc\r
 \r
@@ -370,7 +384,7 @@ func get_msg()
         else\r
             break;\r
         endif\r
-    forever\r
+    until(idx==MSG_BUFF_LEN);\r
 \r
     msg_len := idx;\r
     com_Init(msg_buff, MSG_BUFF_LEN, ':');\r
@@ -414,3 +428,4 @@ endfunc
 \r
 \r
 \r
+\r

diff --git a/src/disp-4dgl/status_screen.4dg b/src/disp-4dgl/status_screen.4dg
index 4a3d8ee479ba025ec3ea6ebad92fd5762116f164..8902ff4af140028a259d9f94d863710769e67282 100644 (file)
--- a/src/disp-4dgl/status_screen.4dg
+++ b/src/disp-4dgl/status_screen.4dg
@@ -571,7 +571,9 @@ func process_msg()
         \r
     endif\r
     if(msg_rcvd[0]==200)\r
-        load_control();\r
+        if(msg_rcvd[2]==2)\r
+            load_control();\r
+        endif\r
     endif\r
         
 endfunc
@@ -684,3 +686,7 @@ endfunc
 \r
 \r
 \r
+\r
+\r
+\r
+\r

diff --git a/src/robofsm/fsmdisplay.c b/src/robofsm/fsmdisplay.c
index d62f5adcf9ac6f126ce6eaa44b913a730d41b204..24a6c6e22147d63b82e549613c3a58222c54f605 100644 (file)
--- a/src/robofsm/fsmdisplay.c
+++ b/src/robofsm/fsmdisplay.c
@@ -59,10 +59,10 @@ void serial_comm(int status)
                memcpy(msg,buff,10);
                msg_waiting = true;
                ROBOT_UNLOCK(disp);
-/*     printf("received serial msg: \n");
-       for(i=0;i<nr;i++)
-               printf("%02x ",msg[i]);
-       printf("\n");*/
+//     printf("received serial msg: \n");
+//     for(i=0;i<nr;i++)
+//             printf("%d ",msg[i]);
+//     printf("\n");
        }
        
        
@@ -71,7 +71,7 @@ void serial_comm(int status)
 int set_actuators(uint8_t actuator)
 {
        switch(actuator) {
-               case BRUSH_SERVOS:
+//             case BRUSH_SERVOS:
 #if 0
                        if(robot.orte.servos.brush_left==BRUSH_LEFT_OPEN) {
                                brushes_in();
@@ -96,16 +96,17 @@ void set_game_color(uint8_t col)
 }
 
 int process_msg(uint8_t *buff) {
-//     printf("rcvd msg id: %d\n",msg[1]);
        switch(buff[1]) {
                case SWITCH_TO_STATUS:
                        uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_STATUS);
                        wait_flag = true;
+                       mode_to_go = MODE_STATUS;
                        return MODE_STATUS;
                        break;
                case SWITCH_TO_CONTROL:
                        uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_CONTROL);
                        wait_flag = true;
+                       mode_to_go = MODE_CONTROL;
                        return MODE_CONTROL;
                        break;
                case SWITCH_TO_STATUS_DONE:
@@ -122,9 +123,6 @@ int process_msg(uint8_t *buff) {
                        printf("msg color: %d\n",buff[2]);
                        set_game_color(buff[2]);
                        break;
-               case GET_READY:
-                       robot_exit();
-                       break;
                case MSG_LIFT_CAL:
                        printf("msg lift cal\n");
                        break;
@@ -152,10 +150,11 @@ int process_msg(uint8_t *buff) {
                case MSG_STATUS:
                        printf("msg status\n");
                        break;
-               case ROBOT_RESTART:
-                       system("killall ortemanager");
-                       system("killall cand");
-                       robot_exit();
+               case MSG_RESTART:
+                       printf("msg restart\n");
+//                     system("killall ortemanager");
+//                     system("killall cand");
+//                     robot_exit();
                        break;
                default:
                        break;
@@ -204,10 +203,10 @@ FSM_STATE(status)
 {
        uint8_t buff[10];
        uint8_t ret;
-       static int cycle = 0;
 
        switch (FSM_EVENT) {
                case EV_ENTRY:
+                       uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_STATUS);
                        DBG("entry to status\n");
                        FSM_TIMER(TRANS_LEN);
                        break;
@@ -244,12 +243,15 @@ FSM_STATE(status)
 FSM_STATE(control)
 {
        uint8_t buff[10];
+       int ret;
 
        switch (FSM_EVENT) {
                case EV_ENTRY:
+                       uoled_switch_mode_rep(MODE_CONTROL, CHANGE_MODE_STATUS);
                        FSM_TIMER(TRANS_LEN);
                        break;
                case EV_TIMER:
+                       ret = uoled_send_control(&robot);
                        ROBOT_LOCK(disp);
                        if(msg_waiting) {
                                memcpy(buff, msg, 10);
@@ -286,7 +288,6 @@ FSM_STATE(wait_for_mode_switch)
                case EV_TIMER:
                        ROBOT_LOCK(disp);
                        if(msg_waiting) {
-                               printf("msg waiting\n");
                                memcpy(buff, msg, 10);
                                msg_waiting = false;
                                ROBOT_UNLOCK(disp);
@@ -296,7 +297,7 @@ FSM_STATE(wait_for_mode_switch)
                                ROBOT_UNLOCK(disp);
                        }
                        if(wait_flag) {
-                               uoled_switch_mode_rep(MODE_STATUS, CHANGE_MODE_STATUS);
+                               uoled_switch_mode_rep(mode_to_go, CHANGE_MODE_STATUS);
                                FSM_TIMER(500);
                                break;
                        }

diff --git a/src/uoled/oledlib.h b/src/uoled/oledlib.h
index f253cce4b2bded8b40808df96c28980ecf0edbd6..3f545f7b74d62b5c2a4dd7075f18032c552cc669 100644 (file)
--- a/src/uoled/oledlib.h
+++ b/src/uoled/oledlib.h
@@ -23,7 +23,7 @@
 #define VOLTAGE_MSG 'V'
 #define FSM_STATE_MSG 'F'
 #define HW_STATUS_MSG 163
-#define COLOR_MSG 164
+#define COLOR_MSG 'c'
 #define POSITION_MSG 'P'
 #define MOVE_FSM_STATE_MSG 'M'
 #define LIFT_MSG 'L'
@@ -38,7 +38,7 @@
 #define MSG_CMU_INIT 156
 #define MSG_START_ACT 157
 #define MSG_COLOR 158
-#define MSG_RESTART 159
+#define MSG_RESTART 162
 #define MSG_INIT 160
 #define MSG_STATUS 161
 
@@ -60,14 +60,6 @@
 #define MODE_STATUS 1
 #define MODE_CONTROL 2
 
-#define BRUSH_SERVOS 1
-#define DEPOSITE_BALLS 2
-#define FDOOR_SERVO 3
-#define BDOOR_SERVO 4
-#define BRUSH_DRIVES 5
-#define LASER 6
-#define BAGR_DRIVE 7
-
 #define HW_STATUS_OK 1
 #define HW_STATUS_FAILED 0
 #define HW_STATUS_WARNING 2

diff --git a/src/uoled/uoled.c b/src/uoled/uoled.c
index d285d0f3df8aaa079264fa062be6ffad98932477..70edac7f3bbeeaaebd8febaad48375a223defacc 100644 (file)
--- a/src/uoled/uoled.c
+++ b/src/uoled/uoled.c
@@ -24,7 +24,6 @@
 /** Used to control verbosity */
 #define DEBUG
 #endif
-#define DEBUG
 
 static struct sercom_data sercom;
 
@@ -67,7 +66,7 @@ int uoled_write_cmd(uint8_t *buff, int size)
 int uoled_switch_mode_rep(uint8_t mode, uint8_t status) 
 {
        uint8_t ret;
-       uint8_t msg[102];
+       uint8_t msg[105];
 
        memset(msg, ' ', sizeof(msg));
        msg[0] = ':';
@@ -154,6 +153,31 @@ int uoled_send_state(const char *name, int len, uint8_t type)
        return 0;
 }
 
+int uoled_send_control(struct robot *state)
+{
+       int ret;
+       uint8_t msg[105];
+       int idx = 0;
+
+       msg[0] = ':';
+       msg[1] = ' ';
+
+       idx+=2;
+
+       ret = oled_set_color(msg+idx, sizeof(msg)-idx, state->team_color);
+       if(ret==-1)
+               return -1;
+       else
+               idx += ret;
+
+       ret = uoled_write_cmd(msg, 105);
+       if(ret)
+               return -1;
+
+       return 0;
+
+}
+
 int uoled_send_status(struct robot *state)
 {
        int ret;

diff --git a/src/uoled/uoled.h b/src/uoled/uoled.h
index fe7e5af71bcf6012ab2ca01002d6ae0fba002afe..cd19f1311f94787031a68dd8440f139bd5663cdb 100644 (file)
--- a/src/uoled/uoled.h
+++ b/src/uoled/uoled.h
@@ -125,6 +125,7 @@ int uoled_set_color(uint8_t color);
  */
 int uoled_send_hw_status(uint8_t *status);
 int uoled_send_status(struct robot *state);
+int uoled_send_control(struct robot *state);
 #ifdef __cplusplus
 }
 #endif