summaryrefslogtreecommitdiff
path: root/robot
diff options
context:
space:
mode:
Diffstat (limited to 'robot')
-rw-r--r--robot/errcatch.c10
-rw-r--r--robot/hypervisor.c6
-rw-r--r--robot/mode_dirc.c13
-rw-r--r--robot/mode_dirc.h1
-rw-r--r--robot/sercomm.c20
-rw-r--r--robot/sim.h4
6 files changed, 35 insertions, 19 deletions
diff --git a/robot/errcatch.c b/robot/errcatch.c
index 0f809f9..77fcfd3 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -19,21 +19,21 @@ void w2_errcatch_handle_error(w2_s_error *error) {
default: {
g_w2_error_uncaught = true;
#ifdef W2_SIM
- simwarn("Uncaught/unhandled error found with code 0x%02x\n", error->code);
+ simwarn("Uncaught/unhandled error found with code 0x%02x", error->code);
+ if (error->message_length > 0) fprintf(stderr, " and message \"%.*s\"", error->message_length, error->message);
+ fprintf(stderr, "\n");
#endif
}
}
// forward error to sercomm
- size_t msg_size = sizeof(w2_s_cmd_expt_tx) + sizeof(uint8_t) * error->message_length;
- w2_s_cmd_expt_tx *msg = malloc(msg_size);
+ W2_CREATE_MSG_SIZE_BIN(w2_s_cmd_expt_tx, sizeof(w2_s_cmd_expt_tx) + sizeof(uint8_t) * error->message_length, msg, msg_bin);
msg->opcode = W2_CMD_EXPT | W2_CMDDIR_TX;
msg->error = error->code;
msg->length = error->message_length;
memcpy(msg->message, error->message, error->message_length);
- w2_s_bin *msg_bin = w2_bin_s_alloc(msg_size, (uint8_t *)msg);
+
w2_sercomm_append_msg(msg_bin);
- free(msg);
free(msg_bin);
return;
diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index 478d3a5..efdceec 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -14,6 +14,12 @@ unsigned long g_w2_hypervisor_ema_io_ms = 0;
unsigned long g_w2_hypervisor_ema_mode_ms = 0;
uint64_t g_w2_hypervisor_timers[W2_HYPERVISOR_TIMER_COUNT] = {0};
+unsigned int g_w2_ping_ms = 0;
+uint8_t g_w2_ping_id = 0;
+bool g_w2_ping_received = true;
+bool g_w2_ping_timeout = false;
+bool g_w2_connected = false;
+
void w2_hypervisor_main() {
#ifdef W2_SIM
w2_sim_cycle_begin();
diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c
index 5988816..397d44d 100644
--- a/robot/mode_dirc.c
+++ b/robot/mode_dirc.c
@@ -1,13 +1,18 @@
#include "mode_dirc.h"
#include "io.h"
-
+#include "hypervisor.h"
+#include "modes.h"
+#include "../shared/util.h"
#include "orangutan_shim.h"
int16_t g_w2_mode_dirc_motor_l = 0;
int16_t g_w2_mode_dirc_motor_r = 0;
+double g_w2_mode_dirc_power = 1.0;
void w2_mode_dirc() {
- set_motors(g_w2_mode_dirc_motor_l, g_w2_mode_dirc_motor_r);
- g_w2_io.motor_left.speed = g_w2_mode_dirc_motor_l;
- g_w2_io.motor_right.speed = g_w2_mode_dirc_motor_r;
+ // if (g_w2_connected) g_w2_mode_dirc_power = 1.0;
+ // else g_w2_mode_dirc_power = W2_MAX(0, g_w2_mode_dirc_power - 0.01);
+
+ // if (g_w2_mode_dirc_power == 0.0) w2_modes_call(W2_M_HALT);
+ set_motors(g_w2_mode_dirc_motor_l * g_w2_mode_dirc_power, g_w2_mode_dirc_motor_r * g_w2_mode_dirc_power);
}
diff --git a/robot/mode_dirc.h b/robot/mode_dirc.h
index 12c967a..bf02270 100644
--- a/robot/mode_dirc.h
+++ b/robot/mode_dirc.h
@@ -6,6 +6,7 @@
extern int16_t g_w2_mode_dirc_motor_l;
extern int16_t g_w2_mode_dirc_motor_r;
+extern double g_w2_mode_dirc_power;
/**
* direct control mode
diff --git a/robot/sercomm.c b/robot/sercomm.c
index afde48a..6e191d2 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -20,12 +20,6 @@ char g_w2_serial_buffer[W2_SERIAL_READ_BUFFER_SIZE] = {0};
uint8_t g_w2_serial_buffer_index = 0;
uint8_t g_w2_serial_buffer_head = 0;
-unsigned int g_w2_ping_ms = 0;
-uint8_t g_w2_ping_id = 0;
-bool g_w2_ping_received = true;
-bool g_w2_ping_timeout = false;
-bool g_w2_connected = false;
-
void w2_sercomm_main() {
#ifdef W2_SIM
simprintfunc("w2_sercomm_main", "");
@@ -40,10 +34,11 @@ void w2_sercomm_main() {
// check time-out
if (!g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_TIMEOUT) {
g_w2_ping_timeout = true;
+ g_w2_connected = false;
w2_errcatch_throw(W2_E_WARN_PING_TIMEOUT);
}
// send ping every W2_TIMER_PING ms
- if ((g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > 1000) || g_w2_ping_timeout) {
+ if ((g_w2_ping_received && w2_hypervisor_time_end(W2_TIMER_PING) > W2_PING_FREQUENCY) || g_w2_ping_timeout) {
g_w2_ping_timeout = false;
g_w2_ping_received = false;
g_w2_ping_id = (uint8_t) rand();
@@ -62,7 +57,7 @@ void w2_sercomm_main() {
while (g_w2_sercomm_offset != g_w2_sercomm_index) {
w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_offset];
#ifdef W2_SIM
- w2_sim_print_serial(data);
+ if (DBG_ENABLE_SERIAL) w2_sim_print_serial(data);
#endif
serial_send_blocking("\xff", 1);
for (uint8_t i = 0; i < data->bytes; i++) {
@@ -89,10 +84,19 @@ void w2_sercomm_append_msg(w2_s_bin *data) {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
+#include <stdlib.h>
+#include <string.h>
+
+
void w2_cmd_ping_tx(w2_s_bin *data) {
g_w2_ping_ms = w2_hypervisor_time_end(W2_TIMER_PING);
g_w2_ping_received = true;
g_w2_ping_timeout = false;
+ g_w2_connected = true;
+
+ char buf[32];
+ sprintf(buf, "rec: %i, tim: %i, con: %i", g_w2_ping_received, g_w2_ping_timeout, g_w2_connected);
+ w2_errcatch_throw_msg(0x69, strlen(buf), buf);
}
void w2_cmd_ping_rx(w2_s_bin *data) {
diff --git a/robot/sim.h b/robot/sim.h
index 14f0f74..7d7c091 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -14,9 +14,9 @@
#define DBG_ENABLE_SIMWARN (1)
#define DBG_ENABLE_SIMINFO (1)
#define DBG_ENABLE_CYCLEINFO (0)
-#define DBG_ENABLE_SERIAL (1)
+#define DBG_ENABLE_SERIAL (0)
-#define DBG_CYCLE_DELAY (1e3)
+#define DBG_CYCLE_DELAY (10e3)
#define DBG_MAX_CYCLES (-1)
// debug print options