#include <unistd.h>
#include <stdlib.h>
#include <signal.h>
+#include <stdbool.h>
struct sercom_data* sercom;
int interrupt = 0;
return x->tv_sec < y->tv_sec;
}
-int miliseconds_since(struct timespec *t)
+bool miliseconds_elapsed_since(struct timespec *t, unsigned miliseconds)
{
struct timespec now, diff;
+ unsigned long long elapsed;
+
+ if (t->tv_sec == 0 && t->tv_nsec == 0)
+ return true; /* Always elapsed after program start */
clock_gettime(CLOCK_MONOTONIC, &now);
- timespec_subtract(&diff, t, &now);
- return diff.tv_sec * 1000 + diff.tv_nsec/1000000;
+ timespec_subtract(&diff, &now, t);
+ elapsed = diff.tv_sec * 1000 + diff.tv_nsec/1000000;
+ return elapsed > miliseconds;
}
break;
}
if (status != last_status ||
- miliseconds_since(&last_sent) > 1000) {
- uoled_display_status(PWR, STATUS_OK);
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(PWR, status);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
last_status = status;
}
/* Neni potreba aktualizovat stav na displeji 25x za sekundu */
if (status != last_status ||
- miliseconds_since(&last_sent) > 1000) {
- uoled_display_status(ODO, STATUS_OK);
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(ODO, status);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
last_status = status;
}
-void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
UDE_hw_status_t status = STATUS_FAILED;
static UDE_hw_status_t last_status;
static struct timespec last_sent;
+ struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
+
switch (info->status) {
case NEW_DATA:
- status = STATUS_OK;
+ if (instance->start_condition)
+ status = STATUS_WARNING; /* Start plug must be plugged before competition start */
+ else
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 500)) {
+ uoled_display_status(STA, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_robot_swicthes_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ UDE_hw_status_t status = STATUS_FAILED;
+ static char last_color;
+ static struct timespec last_sent;
+ struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA:
+ if (instance->team_color != last_color ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_color(instance->team_color);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_color = instance->team_color;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+}
+
+void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct motion_status_type *m = (struct motion_status_type *)vinstance;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ if (m->err_left == 0 && m->err_right == 0)
+ status = STATUS_OK;
+ else
+ status = STATUS_WARNING;
break;
case DEADLINE:
status = STATUS_FAILED;
}
/* Neni potreba aktualizovat stav na displeji 25x za sekundu */
if (status != last_status ||
- miliseconds_since(&last_sent) > 1000) {
- uoled_display_status(MOT, STATUS_OK);
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(MOT, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct vidle_status_type *m = (struct vidle_status_type *)vinstance;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ if (m->flags == 0)
+ status = STATUS_OK;
+ else
+ status = STATUS_WARNING;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(VID, status);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
last_status = status;
UDE_hw_status_t status = STATUS_FAILED;
static UDE_hw_status_t last_status;
static struct timespec last_sent;
- //struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+ struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+ char s, c;
switch (info->status) {
case NEW_DATA:
- status = STATUS_OK;
+ if (instance->error == 0)
+ status = STATUS_OK;
+ else
+ status = STATUS_WARNING;
break;
case DEADLINE:
status = STATUS_FAILED;
break;
}
if (status != last_status ||
- miliseconds_since(&last_sent) > 1000) {
- uoled_display_status(CAM, STATUS_OK);
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(CAM, status);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
last_status = status;
+
+ if (status == STATUS_OK) {
+ s = (instance->side >= 0 && instance->side < 10) ?
+ '0'+instance->side : 'E';
+ c = (instance->center >= 0 && instance->center < 10) ?
+ '0'+instance->center : 'E';
+ } else
+ s = c = '-';
+ uoled_display_corns(s, c);
}
void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
break;
}
if (status != last_status ||
- miliseconds_since(&last_sent) > 1000) {
- uoled_display_status(HOK, STATUS_OK);
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(HOK, status);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
last_status = status;
struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
switch (info->status) {
case NEW_DATA:
- uoled_display_position(orte_data->est_pos_best.x, orte_data->est_pos_best.y, orte_data->est_pos_best.phi);
status = STATUS_OK;
break;
case DEADLINE:
break;
}
if (status != last_status ||
- miliseconds_since(&last_sent) > 1000) {
- uoled_display_status(ODO, STATUS_OK);
+ miliseconds_elapsed_since(&last_sent, 100)) {
+ uoled_display_status(APP, status);
+ if (status == STATUS_OK)
+ uoled_display_position(orte_data->est_pos_best.x, orte_data->est_pos_best.y, orte_data->est_pos_best.phi);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
last_status = status;
break;
}
if (strcmp(status,last_status) != 0 ||
- miliseconds_since(&last_sent) > 1000) {
+ miliseconds_elapsed_since(&last_sent, 1000)) {
uoled_display_fsm(FSM_MAIN, status);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
break;
}
if (strcmp(status,last_status) != 0 ||
- miliseconds_since(&last_sent) > 1000) {
+ miliseconds_elapsed_since(&last_sent, 1000)) {
uoled_display_fsm(FSM_ACT, status);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
break;
}
if (strcmp(status,last_status) != 0 ||
- miliseconds_since(&last_sent) > 1000) {
+ miliseconds_elapsed_since(&last_sent, 1000)) {
uoled_display_fsm(FSM_MOVE, status);
clock_gettime(CLOCK_MONOTONIC, &last_sent);
}
}
-
+struct robottype_orte_data orte;
int main(int argc, char *argv[])
{
int ret;
//int size;
- struct robottype_orte_data orte;
char * tty;
signal(SIGINT, sig_handler);
}
sercom = uoled_sercom_init(tty, NULL);
- if (strcmp(tty, "/dev/null") != 0) {
+ if (strcmp(tty, "/dev/null") == 0 || sercom != NULL) {
// :wq
printf("displayd: init serial communication OK\n");
} else {
- printf("displayd: init serial communication failed\n");
- //return 1;
+ fprintf(stderr, "displayd: init serial communication failed\n");
+//#ifdef COMPETITION
+ /* Sleep in order to that not be respawn too fast by init */
+ sleep(3);
+ return 1;
+//#endif
}
- orte.strength = 1;
-
/* orte initialization */
if(robottype_roboorte_init(&orte)) {
- printf("displayd: Roboorte initialization failed! Exiting...\n");
- return -1;
+ fprintf(stderr, "displayd: Roboorte initialization failed! Exiting...\n");
+#ifdef COMPETITION
+ /* Sleep in order to that not be respawn too fast by init */
+ sleep(3);
+#endif
+ return 1;
}
else
printf("displayd: Roboorte OK\n");
/* creating subscribers */
robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, &orte);
- robottype_subscriber_motion_irc_create(&orte, rcv_motion_irc_cb, &orte);
+ robottype_subscriber_motion_status_create(&orte, rcv_motion_status_cb, NULL);
+ robottype_subscriber_vidle_status_create(&orte, rcv_vidle_status_cb, NULL);
robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, &orte);
robottype_subscriber_est_pos_best_create(&orte, rcv_est_pos_best_cb, &orte);
//robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, &orte);
robottype_subscriber_fsm_act_create(&orte, rcv_fsm_act_cb, &orte);
robottype_subscriber_fsm_motion_create(&orte, rcv_fsm_motion_cb, &orte);
+ robottype_subscriber_robot_cmd_create(&orte, rcv_robot_cmd_cb, &orte);
+ robottype_subscriber_robot_switches_create(&orte, rcv_robot_swicthes_cb, &orte);
printf("displayd: subscribers created OK\n");
if (strcmp(tty, "/dev/null") != 0) {
ret = poll(pfd, 1, 500/*ms*/);
if (ret > 0) {
- rcmd = uoled_recieve_cmd();
- orte.display_cmd.command=rcmd;
- if (! rcmd){
- ORTEPublicationSend(orte.publication_display_cmd);
- printf("displayd: publishing command recieved from display [ 0x%2x ]\n", rcmd);
+ ret = uoled_recieve_cmd(&rcmd);
+/* orte.display_cmd.command=rcmd; */
+/* if (! rcmd){ */
+/* ORTEPublicationSend(orte.publication_display_cmd); */
+/* printf("displayd: publishing command recieved from display [ 0x%2x ]\n", rcmd); */
- }
+/* } */
+ }
+ if (ret < 0) {
+ perror("Error: read from display");
+ break;
}
} else {
usleep(500/*ms*/*1000);
printf("displayd: destroying orte instance...\n");
robottype_roboorte_destroy(&orte);
+ if (!interrupt) {
+ printf("Waiting 3 seconds\n");
+ sleep(3);
+ }
+
printf("displayd: finished!\n");
return 0;
}
projects / eurobot / public.git / blobdiff