//#inherit "4DGL-System.fnc"
\r
#constant MSG_TERM '~'\r
-#constant MSG_BUFF_LEN 30\r
+#constant MSG_BUFF_LEN 79\r
#constant MSG_POWER 'V'\r
#constant MSG_POSITION 'P'\r
+#constant MSG_FSM_STATE 'F'\r
+#constant MSG_MOVE_FSM_STATE 'M'\r
+\r
+#constant MSG_STATUS 'S'\r
// Indices to the voltage_status array
#constant VOLTAGE33 0
/*Voltage branch OK indication*/
#constant VOLTAGE_OK 1\r
-\r
-#constant ACK 255
var msg_buff[MSG_BUFF_LEN]; // serial RX buffer\r
var msg_rcvd[MSG_BUFF_LEN]; // serial RX buffer
var game_color;
var fsm_state[20];
-var fsm_state_len;
+var fsm_state_len;\r
+var move_fsm_state[20];\r
+var move_fsm_state_len;\r
+
+func init()\r
+ var idx;\r
+ \r
+ game_color:=BLACK;\r
+ \r
+ idx:=0;\r
+ repeat\r
+ position[idx]:=' ';\r
+ idx++;\r
+ until(idx == 8);\r
+ idx:=0;\r
+ repeat\r
+ voltage33[idx]:=' ';\r
+ idx++;\r
+ until(idx == 4);\r
+ idx:=0;\r
+ repeat\r
+ voltage50[idx]:=' ';\r
+ idx++;\r
+ until(idx == 4);\r
+ idx:=0;\r
+ repeat\r
+ voltage80[idx]:=' ';\r
+ idx++;\r
+ until(idx == 4);\r
+ idx:=0;\r
+ repeat\r
+ voltageBAT[idx]:=' ';\r
+ idx++;\r
+ until(idx == 5);\r
+endfunc\r
+\r
func clear_screen()
gfx_Rectangle(0, 0, 239, 319, BLACK);
func update_fsm_state()
var idx;
- var state_p;
gfx_Rectangle(0, 70, 239, 99, BLACK);
txt_Set(FONT_SIZE, FONT4);
txt_MoveCursor(5, 0); // move the cursor to line 4, column 5
return 0;
endfunc
+func update_move_fsm_state()\r
+ var idx;\r
+ gfx_Rectangle(0, 99, 239, 112, BLACK);\r
+ txt_Set(FONT_SIZE, FONT4);\r
+ txt_MoveCursor(6, 0); // move the cursor to line 4, column 5\r
+ txt_Set(TEXT_COLOUR, WHITE);\r
+ idx:=0;\r
+ repeat\r
+ putch(move_fsm_state[idx]);\r
+ idx++;\r
+ until(idx==move_fsm_state_len)\r
+ return 0;\r
+endfunc\r
+\r
func draw_status()
clear_screen();
/*Process the message received on serial line*/
func process_msg()\r
+ var idx;\r
+ if(msg_rcvd[0]==MSG_STATUS)\r
+ voltage33[0]:=msg_rcvd[2];\r
+ voltage33[1]:=msg_rcvd[3];\r
+ voltage33[2]:=msg_rcvd[4];\r
+ voltage33[3]:=msg_rcvd[5];\r
+ \r
+ voltage50[0]:=msg_rcvd[6];\r
+ voltage50[1]:=msg_rcvd[7];\r
+ voltage50[2]:=msg_rcvd[8];\r
+ voltage50[3]:=msg_rcvd[9];\r
+ \r
+ voltage80[0]:=msg_rcvd[10];\r
+ voltage80[1]:=msg_rcvd[11];\r
+ voltage80[2]:=msg_rcvd[12];\r
+ voltage80[3]:=msg_rcvd[13];\r
+ \r
+ voltageBAT[0]:=msg_rcvd[14];\r
+ voltageBAT[1]:=msg_rcvd[15];\r
+ voltageBAT[2]:=msg_rcvd[16];\r
+ voltageBAT[3]:=msg_rcvd[17];\r
+ voltageBAT[4]:=msg_rcvd[18];\r
+ \r
+ position[0]:=msg_rcvd[21];\r
+ position[1]:=msg_rcvd[22];\r
+ position[2]:=msg_rcvd[23];\r
+ position[3]:=msg_rcvd[24];\r
+ position[4]:=msg_rcvd[25];\r
+ position[5]:=msg_rcvd[26];\r
+ position[6]:=msg_rcvd[27];\r
+ position[7]:=msg_rcvd[28];\r
+ \r
+ draw_voltage33();\r
+ draw_voltage50();\r
+ draw_voltage80();\r
+ draw_voltageBAT();\r
+ draw_position();\r
+ endif\r
+ \r
if(msg_rcvd[0]==MSG_POWER) //
voltage33[0]:=msg_rcvd[1];
voltage33[1]:=msg_rcvd[2];
\r
draw_position();\r
endif\r
-
+ \r
+ if(msg_rcvd[0]==MSG_FSM_STATE)\r
+ \r
+ idx:=1;\r
+ repeat\r
+ fsm_state[idx-1] := msg_rcvd[idx];\r
+ idx++;\r
+ until(msg_rcvd[idx]==MSG_TERM)\r
+ fsm_state_len := idx-1;\r
+ update_fsm_state();\r
+ putstr("STATE");\r
+ endif\r
+ \r
+ if(msg_rcvd[0]==MSG_MOVE_FSM_STATE)\r
+ idx:=1;\r
+ repeat\r
+ move_fsm_state[idx-1] := msg_rcvd[idx];\r
+ idx++;\r
+ until(msg_rcvd[idx]==MSG_TERM)\r
+ move_fsm_state_len := idx-1;\r
+ update_move_fsm_state();\r
+ endif\r
+
endfunc
func get_msg()
idx++;
else
break;
- endif
- until((in==MSG_TERM)||(idx==MSG_BUFF_LEN))
+ endif\r
+ forever
+// until((in==MSG_TERM)||(idx==MSG_BUFF_LEN))
msg_len := idx;\r
-// com_Init(msg_buff, 40, ':');
+ com_Init(msg_buff, 40, ':');
process_msg();
- com_Init(msg_buff, MSG_BUFF_LEN, ':');
return 0;
endfunc
func main()
- setbaud(6); // 2400 baud
+ setbaud(6); // 9600 baud
com_Init(msg_buff, MSG_BUFF_LEN, ':');
touch_Set(TOUCH_ENABLE); // enable TS
touch_Set(TOUCH_REGIONDEFAULT); // reset touch area to fullscreen\r
\r
+ init();\r
+ \r
draw_status();
-// update_fsm_state();
repeat // the main application loop
// touchscreen();\r
if(com_Full()==1)\r
forever
endfunc
\r
+\r
#include <string.h>
#include <oledlib.h>
-#define TRANS_LEN 200
+#define TRANS_LEN 300
#ifdef DISPLAY_DEBUG
#define DBG(format, ...) printf(format, ##__VA_ARGS__)
switch (FSM_EVENT) {
case EV_ENTRY:
DBG("entry to status\n");
-// uoled_set_color(robot.team_color);
-// uoled_send_position(&robot.orte.est_pos);
FSM_TIMER(TRANS_LEN);
break;
case EV_TIMER:
-// uoled_send_state(robot.act_fsm_state_name, strlen(robot.act_fsm_state_name), FSM_STATE_MSG);
-// uoled_send_state(robot.move_fsm_state_name, strlen(robot.move_fsm_state_name), MOVE_FSM_STATE_MSG);
- switch(cycle) {
- case 0:
- uoled_send_voltage(&robot.orte.pwr_voltage);
- break;
- case 1:
- uoled_send_position(&robot.orte.est_pos);
- break;
- }
- cycle++;
- if(cycle > 1)
- cycle = 0;
-// uoled_send_hw_status(robot.hw_status);
+ ret = uoled_send_status(&robot);
+ if(ret == -1)
+ printf("send failed\n");
ROBOT_LOCK(disp);
if(msg_waiting) {
memcpy(buff, msg, 10);
#include <fsm.h>
#include <robot_config.h>
-#define FSM_STATE_NAME_MAX_LEN 20
+#define FSM_STATE_NAME_MAX_LEN 21
/* Select robot color. If we are GREEN, no coordinate transformation will
* be performed. */
robot.fsm.main.transition_callback = trans_callback;
robot.fsm.motion.transition_callback = move_trans_callback;
+ sprintf(robot.act_fsm_state_name, "state_name");
+ sprintf(robot.move_fsm_state_name, "state_name_2");
+
rv = robot_start();
if (rv) error(1, errno, "robot_start() returned %d\n", rv);
if(buff_size < HW_STATUS_MSG_SIZE)
return -1;
- buff[0] = MSG_START;
- buff[1] = HW_STATUS_MSG;
- buff[2] = status[0];
- buff[3] = status[1];
- buff[4] = status[2];
- buff[5] = status[3];
- buff[6] = status[4];
- buff[7] = status[5];
- buff[8] = MSG_TERM;
- return 0;
+ buff[0] = HW_STATUS_MSG;
+ buff[1] = status[0];
+ buff[2] = status[1];
+ buff[3] = status[2];
+ buff[4] = status[3];
+ buff[5] = status[4];
+ buff[6] = status[5];
+ buff[7] = MSG_TERM;
+ return HW_STATUS_MSG_SIZE;
}
int oled_set_color(uint8_t *buff, int buff_size, uint8_t color)
if(buff_size < COLOR_MSG_SIZE)
return -1;
- buff[0] = MSG_START;
- buff[1] = COLOR_MSG;
- buff[2] = color;
- buff[3] = MSG_TERM;
- return 0;
+ buff[0] = COLOR_MSG;
+ buff[1] = color;
+ buff[2] = MSG_TERM;
+ return COLOR_MSG_SIZE;
}
int oled_switch_mode(uint8_t *buff, int buff_size, uint8_t mode, uint8_t status)
if(buff_size < SWITCH_MODE_MSG_SIZE)
return -1;
- *buff = MSG_START;
- *(buff+1) = CHANGE_MODE_REP;
- *(buff+2) = status;
- *(buff+3) = mode;
- *(buff+4) = MSG_TERM;
+ *(buff+0) = CHANGE_MODE_REP;
+ *(buff+1) = status;
+ *(buff+2) = mode;
+ *(buff+3) = MSG_TERM;
- return 0;
+ return SWITCH_MODE_MSG_SIZE;
}
int oled_send_voltage(uint8_t *buff, int buff_size, struct pwr_voltage_type *volt)
if(buff_size < VOLTAGE_MSG_SIZE)
return -1;
- buff[0] = MSG_START;
- buff[1] = ' ';
+ buff[0] = VOLTAGE_MSG;
- buff[2] = VOLTAGE_MSG;
+ sprintf((char*)(buff+1),"%1.2f",volt->voltage33);
- sprintf((char*)(buff+3),"%1.2f",volt->voltage33);
-
- sprintf((char*)(buff+7),"%1.2f",volt->voltage50);
+ sprintf((char*)(buff+5),"%1.2f",volt->voltage50);
- sprintf((char*)(buff+11),"%1.2f",volt->voltage80);
+ sprintf((char*)(buff+9),"%1.2f",volt->voltage80);
- sprintf((char*)(buff+15),"%2.2f",volt->voltageBAT);
+ sprintf((char*)(buff+13),"%2.2f",volt->voltageBAT);
- *(buff+20) = MSG_TERM;
+ *(buff+18) = MSG_TERM;
- return 0;
+ return VOLTAGE_MSG_SIZE;
}
int oled_send_position(uint8_t *buff, int buff_size, struct est_pos_type *pos)
if(buff_size < POSITION_MSG_SIZE)
return -1;
- buff[0] = MSG_START;
- buff[1] = ' ';
-
- buff[2] = POSITION_MSG;
+ buff[0] = POSITION_MSG;
- sprintf((char*)(buff+3),"%1.2f",pos->x);
+ sprintf((char*)(buff+1),"%1.2f",pos->x);
- sprintf((char*)(buff+7),"%1.2f",pos->y);
+ sprintf((char*)(buff+5),"%1.2f",pos->y);
- sprintf((char*)(buff+11),"%1.2f",pos->phi);
+ sprintf((char*)(buff+9),"%1.2f",pos->phi);
- *(buff+15) = MSG_TERM;
+ *(buff+13) = MSG_TERM;
- return 0;
+ return POSITION_MSG_SIZE;
}
+
int oled_send_fsm_state(uint8_t *buff, int buff_size, const char *name, int len, uint8_t type)
{
- int i;
-
- if(buff_size > FSM_STATE_MSG_SIZE)
+ if(len > FSM_STATE_MSG_SIZE)
return -1;
-
- buff[0] = MSG_START;
-
- buff[1] = type;
- for(i=0; i<len; i++)
- buff[2+i] = name[i];
- buff[2+i] = MSG_TERM;
- return 0;
+ memset(buff, ' ', FSM_STATE_MSG_SIZE);
+ buff[0] = type;
+ strncpy((char*)buff+1, name, len);
+ buff[FSM_STATE_MSG_SIZE-1] = MSG_TERM;
+
+ return FSM_STATE_MSG_SIZE;
}
#define MSG_SIZE 31
#define MSG_TERM '~'
-#define HW_STATUS_MSG_SIZE 10
-#define SWITCH_MODE_MSG_SIZE 6
-#define VOLTAGE_MSG_SIZE 21
-#define FSM_STATE_MSG_SIZE 41
-#define POSITION_MSG_SIZE 16
-#define COLOR_MSG_SIZE 5
+#define HW_STATUS_MSG_SIZE 8
+#define SWITCH_MODE_MSG_SIZE 4
+#define VOLTAGE_MSG_SIZE 19
+#define FSM_STATE_MSG_SIZE 22
+#define POSITION_MSG_SIZE 14
+#define COLOR_MSG_SIZE 3
#define VOLTAGE_MSG 'V'
-#define FSM_STATE_MSG 161
-#define BALLS_MSG 162
+#define FSM_STATE_MSG 'F'
#define HW_STATUS_MSG 163
#define COLOR_MSG 164
#define POSITION_MSG 'P'
-#define MOVE_FSM_STATE_MSG 166
+#define MOVE_FSM_STATE_MSG 'M'
#define ACTUATORS 180
#define GET_READY 181
#define SWITCH_TO_CONTROL_DONE 186
#define SET_COLOR 187
#define SET_MODE 188
-#define CAR_LEFT 189
-#define CAR_RIGHT 190
#define CHANGE_MODE_REP 200
#define CHANGE_MODE_REQ 201
#include <stdio.h>
#include <robottype.h>
#include <robottype.h>
+#include <robot.h>
#include "uoled.h"
#include "uoled_config.h"
int uoled_write_cmd(uint8_t *buff, int size)
{
int ret;
- char outbuff[MSG_SIZE];
+ char outbuff[size];
-#ifdef DEBUG
+//#if 1
int i;
- printf("sending command: ");
+ printf("sending command (%d bytes): ", size);
for(i=0; i<size; i++)
- printf("%d ",buff[i]);
+ printf("%c",buff[i]);
printf("\n");
-#endif
- memset(outbuff, 0, MSG_SIZE);
+//#endif
+ memset(outbuff, 0, size);
memcpy(outbuff, buff, size);
- ret = write(sercom.fd, outbuff, MSG_SIZE);
- if(ret != MSG_SIZE) {
- printf("uoled: WRITE FAILED!!!!!!!!!!!!!!!\n");
+ ret = write(sercom.fd, outbuff, size);
+ if(ret != size) {
+ printf("uoled: WRITE FAILED!\n");
return -1;
}
oled_send_fsm_state(msg, len+3, name, len, type);
- ret = uoled_write_cmd(msg, len+3);
+ ret = uoled_write_cmd(msg, len+4);
if(ret)
return -1;
return 0;
}
+int uoled_send_status(struct robot *state)
+{
+ int ret, i;
+ uint8_t msg[100];
+ int idx = 0;
+
+ msg[0] = MSG_START;
+ msg[1] = ' ';
+ msg[2] = 'S';
+
+ idx += 3;
+
+ ret = oled_send_voltage(msg+idx, sizeof(msg)-idx, &(state->orte.pwr_voltage));
+ if(ret==-1)
+ return -1;
+ else
+ idx += ret;
+ ret = oled_send_position(msg+idx, sizeof(msg)-idx, &(state->orte.est_pos));
+ if(ret==-1)
+ return -1;
+ else
+ idx += ret;
+
+ ret = oled_send_fsm_state(msg+idx, sizeof(msg)-idx, (state->act_fsm_state_name),
+ strlen(state->act_fsm_state_name), FSM_STATE_MSG);
+ if(ret==-1)
+ return -1;
+ else
+ idx += ret;
+
+ ret = oled_send_fsm_state(msg+idx, sizeof(msg)-idx, (state->move_fsm_state_name),
+ strlen(state->move_fsm_state_name), MOVE_FSM_STATE_MSG);
+ if(ret==-1)
+ return -1;
+ else
+ idx += ret;
+
+ ret = uoled_write_cmd(msg, idx);
+ if(ret)
+ return -1;
+
+ return 0;
+}
* status of individual HW unit (first element is the box in upper left corner).
*/
int uoled_send_hw_status(uint8_t *status);
+int uoled_send_status(struct robot *state);
#ifdef __cplusplus
}
#endif
projects / eurobot / public.git / commitdiff