summaryrefslogtreecommitdiff
path: root/robot
diff options
context:
space:
mode:
Diffstat (limited to 'robot')
-rw-r--r--robot/bin.c34
-rw-r--r--robot/bin.h39
-rw-r--r--robot/errcatch.c3
-rw-r--r--robot/errcatch.h4
-rw-r--r--robot/hypervisor.c4
-rw-r--r--robot/makefile2
-rw-r--r--robot/sercomm.c48
-rw-r--r--robot/sercomm.h23
-rw-r--r--robot/setup.c27
-rw-r--r--robot/sim.h3
10 files changed, 168 insertions, 19 deletions
diff --git a/robot/bin.c b/robot/bin.c
index 4242edc..a2c91a4 100644
--- a/robot/bin.c
+++ b/robot/bin.c
@@ -1,3 +1,5 @@
+#include <stdlib.h>
+
#include "bin.h"
#define W2_ENDIAN_LITTLE (1)
@@ -12,8 +14,40 @@
#define _BYTE_2 ((uint32_t)(0xff << (_SHIFT_2B)))
#define _BYTE_3 ((uint32_t)(0xff << (_SHIFT_3B)))
+const uint8_t W2_STRUCT_T_SIZES[] = {sizeof(uint8_t), sizeof(uint16_t), sizeof(uint32_t)};
+
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wshift-count-overflow"
+w2_s_bin *w2_bin_from_uint8_t(uint8_t data) {
+ size_t size = 1;
+ w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size);
+ ret->bytes = size;
+ ret->data[0] = data;
+ return ret;
+}
+
+w2_s_bin *w2_bin_from_uint16_t(uint16_t data) {
+ size_t size = 2;
+ w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size);
+ data = w2_bin_hton16(data);
+ ret->bytes = size;
+ ret->data[0] = (data & _BYTE_1) >> _SHIFT_1B;
+ ret->data[1] = (data & _BYTE_0) >> _SHIFT_0B;
+ return ret;
+}
+
+w2_s_bin *w2_bin_from_uint32_t(uint32_t data) {
+ size_t size = 4;
+ w2_s_bin *ret = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * size);
+ data = w2_bin_hton32(data);
+ ret->bytes = size;
+ ret->data[0] = (data & _BYTE_3) >> _SHIFT_3B;
+ ret->data[1] = (data & _BYTE_2) >> _SHIFT_2B;
+ ret->data[2] = (data & _BYTE_1) >> _SHIFT_1B;
+ ret->data[3] = (data & _BYTE_0) >> _SHIFT_0B;
+ return ret;
+}
+
uint32_t w2_bin_hton32(uint32_t h32) {
if (g_w2_endianness == W2_ENDIAN_BIG) return h32;
return ((h32 & _BYTE_0) << _SHIFT_3B) | ((h32 & _BYTE_1) << _SHIFT_1B) |
diff --git a/robot/bin.h b/robot/bin.h
index ccde132..1c9b951 100644
--- a/robot/bin.h
+++ b/robot/bin.h
@@ -1,19 +1,46 @@
#pragma once
+/**
+ * helper file for binary data
+ *
+ * - fix endianness with functions inspired by UNIX arpa/inet.h
+ * - convert uint16_t and uint32_t to w2_s_bin
+ */
#include <stdint.h>
+extern uint8_t g_w2_endianness;
+
+#define W2_T_UINT8_T (0)
+#define W2_T_UINT16_T (1)
+#define W2_T_UINT32_T (2)
+
+enum w2_e_struct_types {
+ W2_ST_UINT8_T,
+ W2_ST_UINT16_T,
+ W2_ST_UINT32_T,
+};
+
+extern const uint8_t W2_STRUCT_T_SIZES[];
+
typedef struct {
uint16_t bytes;
uint8_t data[];
} w2_s_bin;
-extern uint8_t g_w2_endianness;
+typedef struct {
+ enum w2_e_struct_types type;
+ uint16_t length;
+ const uint8_t *data;
+} w2_s_struct_property;
-/**
- * helper file for binary data
- *
- * - fix endianness with functions inspired by UNIX arpa/inet.h
- */
+typedef struct {
+ uint16_t length;
+ w2_s_struct_property *properties[];
+} w2_s_property_list;
+
+w2_s_bin *w2_bin_from_uint8_t(uint8_t data);
+w2_s_bin *w2_bin_from_uint16_t(uint16_t data);
+w2_s_bin *w2_bin_from_uint32_t(uint32_t data);
/** convert 32-bit value from host endian to network (big-endian) */
uint32_t w2_bin_hton32(uint32_t h32);
diff --git a/robot/errcatch.c b/robot/errcatch.c
index f66b2a1..82da820 100644
--- a/robot/errcatch.c
+++ b/robot/errcatch.c
@@ -1,4 +1,3 @@
-#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -31,7 +30,7 @@ void w2_errcatch_main() {
}
w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message) {
- w2_s_error *error = calloc(sizeof(w2_s_error) + length, 1);
+ w2_s_error *error = malloc(sizeof(w2_s_error) + length);
memcpy(error, &(w2_s_error const){.code = code, .message_length = length}, sizeof(w2_s_error));
strncpy(error->message, message, length);
diff --git a/robot/errcatch.h b/robot/errcatch.h
index c6edcfb..f0e25d9 100644
--- a/robot/errcatch.h
+++ b/robot/errcatch.h
@@ -39,6 +39,8 @@ enum w2_e_errorcodes {
W2_E_WARN_ERR_BUFFER_FULL = 0x04 | W2_E_TYPE_WARN,
/** line lost, trying to calibrate */
W2_E_WARN_LINE_LOST = 0x05 | W2_E_TYPE_WARN,
+ /** serial buffer full, gets thrown on next cycle */
+ W2_E_WARN_SERCOMM_BUFFER_FULL = 0x06 | W2_E_TYPE_WARN,
};
/**
@@ -75,6 +77,6 @@ void w2_errcatch_throw_msg(enum w2_e_errorcodes code, uint16_t length, const cha
/**
* allocate and initialize error struct
*
- * TODO: doesn't handle null pointers from calloc
+ * TODO: doesn't handle null pointers from malloc
*/
w2_s_error *w2_alloc_error(enum w2_e_errorcodes code, uint16_t length, const char *message);
diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index c558de8..c08a24c 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -7,7 +7,7 @@
void w2_hypervisor_main() {
#ifdef W2_SIM
- siminfo("cycle start\n");
+ if (DBG_ENABLE_CYCLEINFO) siminfo("cycle start\n");
#endif
time_reset();
@@ -24,7 +24,7 @@ void w2_hypervisor_main() {
if (mode_time > W2_MAX_MODULE_CYCLE_MS) w2_errcatch_throw(W2_E_WARN_CYCLE_EXPIRED);
#ifdef W2_SIM
- siminfo("cycle end\n");
+ if (DBG_ENABLE_CYCLEINFO) siminfo("cycle end\n");
usleep(100e3);
#endif
}
diff --git a/robot/makefile b/robot/makefile
index ce57090..53010dd 100644
--- a/robot/makefile
+++ b/robot/makefile
@@ -14,7 +14,7 @@ SOURCES := $(filter-out sim.c, $(wildcard *.c))
HEADERS := $(filter-out sim.h, $(wildcard *.h))
# simulation
-# SIM = true
+SIM = true
CFLAGS += $(if $(SIM), -DW2_SIM, -mcall-prologues -mmcu=$(MCU))
LDFLAGS += $(if $(SIM), , -lpololu_$(DEVICE))
PREFIX := $(if $(SIM), , avr-)
diff --git a/robot/sercomm.c b/robot/sercomm.c
index a0eed3a..e845b1a 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -1,3 +1,49 @@
+#include <stdlib.h>
+#include <string.h>
+
+#include "bin.h"
+#include "orangutan_shim.h"
#include "sercomm.h"
-void w2_sercomm_main() {}
+w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE] = {};
+uint8_t g_w2_sercomm_index = 0;
+uint8_t g_w2_sercomm_offset = 0;
+uint8_t g_w2_sercomm_buffer_full = 0;
+
+void w2_sercomm_main() {
+#ifdef W2_SIM
+ simprintfunc("w2_sercomm_main", "");
+#endif
+ // send data
+ while (g_w2_sercomm_index != g_w2_sercomm_offset) {
+#ifdef W2_SIM
+ simprint("line 0");
+ w2_s_bin *data = g_w2_sercomm_buffer[g_w2_sercomm_index];
+ simprint("line 1");
+
+ simprintf("bytes: %i\n", data->bytes);
+ char* data_cast = malloc(data->bytes);
+ simprint("line 2");
+ memcpy(data_cast, data->data, data->bytes);
+ simprint("line 3");
+ serial_send(data_cast, data->bytes);
+ simprint("line 4");
+ g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE;
+ simprint("line 5");
+#endif
+ }
+}
+
+void w2_sercomm_append_msg(w2_s_bin *data) {
+#ifdef W2_SIM
+ simprintfunc("w2_sercomm_append_msg", "");
+#endif
+ uint8_t next_index = (g_w2_sercomm_index + 1) % W2_SERCOMM_BUFFER_SIZE;
+ g_w2_sercomm_buffer_full = next_index == g_w2_sercomm_offset;
+ free(g_w2_sercomm_buffer[g_w2_sercomm_index]);
+ w2_s_bin *data_copy = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * data->bytes);
+ memcpy(&data_copy->bytes, data->data, data->bytes);
+ g_w2_sercomm_buffer[g_w2_sercomm_index] = data_copy;
+ if (g_w2_sercomm_buffer_full) return;
+ g_w2_sercomm_index = next_index;
+}
diff --git a/robot/sercomm.h b/robot/sercomm.h
index d960d0d..dd17af8 100644
--- a/robot/sercomm.h
+++ b/robot/sercomm.h
@@ -7,28 +7,42 @@
#define W2_CMDDIR_TX (1)
enum w2_e_serial_commands {
+ /** ping command */
W2_CMD_PING = 0x00,
+ /** exception command */
W2_CMD_EXPT = 0x02,
+ /** mode command */
W2_CMD_MODE = 0x04,
+ /** speed command */
W2_CMD_SPED = 0x06,
+ /** direct control command */
W2_CMD_DIRC = 0x08,
+ /** coordinate command */
W2_CMD_CORD = 0x0a,
+ /** backorder modify command */
W2_CMD_BOMD = 0x0c,
+ /** soft reset command */
W2_CMD_SRES = 0x0e,
+ /** map config command */
W2_CMD_MCFG = 0x10,
+ /** sensor data command */
W2_CMD_SENS = 0x12,
+ /** info command */
W2_CMD_INFO = 0x14,
+ /** display control command */
W2_CMD_DISP = 0x16,
+ /** play midi command */
W2_CMD_PLAY = 0x18,
+ /** control leds command */
W2_CMD_CLED = 0x1a,
};
/** sercomm ring buffer */
extern w2_s_bin *g_w2_sercomm_buffer[W2_SERCOMM_BUFFER_SIZE];
/** stores head of ring buffer */
-extern uint8_t g_w2_error_index;
+extern uint8_t g_w2_sercomm_index;
/** stores start of ring buffer */
-extern uint8_t g_w2_error_offset;
+extern uint8_t g_w2_sercomm_offset;
/**
* serial pc-robot communication module
@@ -38,10 +52,9 @@ extern uint8_t g_w2_error_offset;
*/
void w2_sercomm_main();
-void w2_sercomm_append_msg(w2_s_bin data);
+void w2_sercomm_append_msg(w2_s_bin *data);
-w2_s_bin w2_sercomm_rx_generic();
-w2_s_bin w2_sercomm_tx_generic();
+w2_s_bin w2_sercomm_struct_pack(w2_s_property_list generic_struct);
w2_s_bin w2_sercomm_rx_ping();
w2_s_bin w2_sercomm_tx_ping();
diff --git a/robot/setup.c b/robot/setup.c
index d040286..59a6404 100644
--- a/robot/setup.c
+++ b/robot/setup.c
@@ -27,4 +27,31 @@ void w2_setup_main() {
// indicate startup done
play("L50 c>c");
+
+#ifdef W2_SIM
+ #include "sercomm.h"
+ w2_s_bin *data = malloc(sizeof(w2_s_bin) + sizeof(uint8_t) * 20);
+ data->bytes = 20;
+ data->data[0] = 0x00;
+ data->data[1] = 0x01;
+ data->data[2] = 0x00;
+ data->data[3] = 0x01;
+ data->data[4] = 0x00;
+ data->data[5] = 0x01;
+ data->data[6] = 0x00;
+ data->data[7] = 0x01;
+ data->data[8] = 0xde;
+ data->data[9] = 0xad;
+ data->data[10] = 0xbe;
+ data->data[11] = 0xef;
+ data->data[12] = 0xff;
+ data->data[13] = 0xff;
+ data->data[14] = 0x00;
+ data->data[15] = 0x00;
+ data->data[16] = 0xff;
+ data->data[17] = 0xff;
+ data->data[18] = 0x00;
+ data->data[19] = 0x69;
+ w2_sercomm_append_msg(data);
+#endif
}
diff --git a/robot/sim.h b/robot/sim.h
index 9c80ecf..c872dda 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -5,9 +5,10 @@
#include <unistd.h>
// debug fine-tuning
-#define DBG_ENABLE_PRINTFUNC (0)
+#define DBG_ENABLE_PRINTFUNC (1)
#define DBG_ENABLE_SIMWARN (1)
#define DBG_ENABLE_SIMINFO (1)
+#define DBG_ENABLE_CYCLEINFO (0)
// debug print options
#define DBG_BYTES_PER_LINE 16