summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--client/commands.c33
-rw-r--r--client/commands.h8
-rw-r--r--client/ui_dirc.c69
-rw-r--r--robot/hypervisor.c2
-rw-r--r--robot/main.c1
-rw-r--r--robot/mode_dirc.c3
-rw-r--r--robot/modes.c10
-rw-r--r--robot/sercomm.c5
-rw-r--r--robot/sim.c2
-rw-r--r--robot/sim.h2
-rw-r--r--shared/util.c2
-rw-r--r--shared/util.h1
12 files changed, 88 insertions, 50 deletions
diff --git a/client/commands.c b/client/commands.c
index 18373f6..778e9c1 100644
--- a/client/commands.c
+++ b/client/commands.c
@@ -15,42 +15,41 @@ void w2_send_bin(w2_s_bin *data) {
}
}
-w2_s_bin *w2_send_mode(w2_e_mode mode) {
- W2_CREATE_MSG_BIN(w2_s_cmd_mode_rx, msg, msg_bin);
-
- msg->opcode = W2_CMD_MODE | W2_CMDDIR_RX;
- msg->mode = mode;
+void w2_send_info() {
+ W2_CREATE_MSG_BIN(w2_s_cmd_info_rx, msg, msg_bin);
+ msg->opcode = W2_CMD_INFO | W2_CMDDIR_RX;
w2_send_bin(msg_bin);
free(msg_bin);
- return NULL;
}
-w2_s_bin *w2_send_dirc(uint16_t left, uint16_t right) {
+void w2_send_dirc(uint16_t left, uint16_t right) {
W2_CREATE_MSG_BIN(w2_s_cmd_dirc_rx, msg, msg_bin);
msg->opcode = W2_CMD_DIRC | W2_CMDDIR_RX;
msg->left = w2_bin_hton16(left);
msg->right = w2_bin_hton16(right);
- w2_send_bin(msg_bin);
- free(msg_bin);
- return NULL;
-}
-w2_s_bin *w2_send_info() {
- W2_CREATE_MSG_BIN(w2_s_cmd_info_rx, msg, msg_bin);
- msg->opcode = W2_CMD_INFO | W2_CMDDIR_RX;
w2_send_bin(msg_bin);
free(msg_bin);
- return NULL;
}
-w2_s_bin *w2_send_ping() {
+void w2_send_ping() {
+ g_w2_state.ping_id = (uint8_t)rand();
W2_CREATE_MSG_BIN(w2_s_cmd_ping_rx, msg, msg_bin);
msg->opcode = W2_CMD_PING | W2_CMDDIR_RX;
msg->id = g_w2_state.ping_id;
+
w2_send_bin(msg_bin);
free(msg_bin);
w2_timer_start(W2_TIMER_PING);
- return NULL;
+}
+
+void w2_send_mode(w2_e_mode mode) {
+ W2_CREATE_MSG_BIN(w2_s_cmd_mode_rx, msg, msg_bin);
+ msg->opcode = W2_CMD_MODE | W2_CMDDIR_RX;
+ msg->mode = mode;
+
+ w2_send_bin(msg_bin);
+ free(msg_bin);
}
diff --git a/client/commands.h b/client/commands.h
index cb01189..5a12424 100644
--- a/client/commands.h
+++ b/client/commands.h
@@ -4,7 +4,7 @@
#include "../shared/modes.h"
#include "serial.h"
-w2_s_bin *w2_send_mode(w2_e_mode mode);
-w2_s_bin *w2_send_dirc(uint16_t left, uint16_t right);
-w2_s_bin *w2_send_info();
-w2_s_bin *w2_send_ping();
+void w2_send_info();
+void w2_send_ping();
+void w2_send_mode(w2_e_mode mode);
+void w2_send_dirc(uint16_t left, uint16_t right);
diff --git a/client/ui_dirc.c b/client/ui_dirc.c
index dcd4423..7720a4c 100644
--- a/client/ui_dirc.c
+++ b/client/ui_dirc.c
@@ -3,21 +3,16 @@
#include "commands.h"
#include "ui.h"
-void w2_ui_bar_graph(unsigned int percent) {
- unsigned int width = g_w2_ui_width - 7;
- char bar[width];
- for (unsigned int i = 0; i < width - 2; i++) {
- bar[i + 1] = i > (width - 2) * percent / 100 ? ' ' : '*';
- }
- bar[0] = '|';
- bar[width - 1] = '|';
- mvaddnstr(4, 7, bar, width);
-}
-
+/** decay modifier */
#define W2_DIRC_MOD ((double)0.95)
-#define W2_DIRC_ADD ((double)13.0)
-#define W2_DIRC_PAD ((double)1.10)
-#define W2_DIRC_SPL ((unsigned int)20)
+/** add value per key press */
+#define W2_DIRC_ADD ((double)17.0)
+/** padding */
+#define W2_DIRC_PAD ((double)3.00)
+/** average samples */
+#define W2_DIRC_SPL ((unsigned int)14)
+/** steering padding */
+#define W2_DIRC_STP ((double)0.2)
int w2_avg(int *samples, unsigned int sample_count) {
double total = 0;
@@ -49,6 +44,46 @@ W2_DIRC_MOTOR_DRIVER(r);
void w2_ui_dirc_init() { w2_send_mode(W2_M_DIRC); }
+void w2_ui_bar_graph(unsigned int y, unsigned int x, unsigned int width, double value) {
+ char temp[width];
+ temp[0] = '|';
+ temp[width - 1] = '|';
+ for (unsigned int i = 0; i < width - 2; i++) temp[i + 1] = i < width * value ? '*' : ' ';
+
+ mvaddnstr(y, x, temp, width);
+}
+
+void w2_ui_bar_graph_pm(unsigned int y, unsigned int x, unsigned int width, double value) {
+ char temp[width];
+ temp[0] = '|';
+ temp[width - 1] = '|';
+ width -= 2;
+ unsigned int hw = width / 2;
+ if (value >= 0) {
+ for (unsigned int i = 0; i < width; i++)
+ temp[i + 1] = i < hw ? ' ' : (i - hw) < (hw * value) ? '*' : ' ';
+ } else {
+ for (unsigned int i = 0; i < width; i++)
+ temp[i + 1] = i < hw ? ' ' : (i - hw) < (hw * value) ? '*' : ' ';
+ }
+
+ mvaddnstr(y, x, temp, width);
+}
+
+void w2_ui_dirc_paint(int left, int right) {
+ mvaddstr(4, 0, "left drive: ");
+ w2_ui_bar_graph_pm(4, 13, g_w2_ui_width - 14, (double)left / 255);
+ mvaddstr(5, 0, "right drive: ");
+ w2_ui_bar_graph_pm(5, 13, g_w2_ui_width - 14, (double)right / 255);
+
+ mvaddstr(7, 0,
+ " controls:\n"
+ "\n"
+ " <q> <w> <e> forward\n"
+ " <a> <s> <d> backward\n"
+ "left both right\n");
+}
+
void w2_ui_dirc(bool first) {
if (first) w2_ui_dirc_init();
int ch = 0;
@@ -66,9 +101,9 @@ void w2_ui_dirc(bool first) {
int drive_l = w2_dirc_motor_l(lf, lb);
int drive_r = w2_dirc_motor_r(rf, rb);
- char temp[32] = {0};
- sprintf(temp, "l: %04i, r: %04i", drive_l, drive_r);
- mvaddstr(4, 0, temp);
+ drive_l += drive_r * W2_DIRC_STP;
+ drive_r += drive_l * W2_DIRC_STP;
w2_send_dirc(drive_l, drive_r);
+ w2_ui_dirc_paint(drive_l, drive_r);
}
diff --git a/robot/hypervisor.c b/robot/hypervisor.c
index 4f994a8..478d3a5 100644
--- a/robot/hypervisor.c
+++ b/robot/hypervisor.c
@@ -27,7 +27,7 @@ void w2_hypervisor_main() {
unsigned long sercomm_time = get_ms();
w2_errcatch_main();
unsigned long errcatch_time = get_ms() - sercomm_time;
- w2_io_main();
+ // w2_io_main();
unsigned long io_time = get_ms() - errcatch_time;
w2_modes_main();
unsigned long mode_time = get_ms() - io_time;
diff --git a/robot/main.c b/robot/main.c
index b0302bd..2f15e97 100644
--- a/robot/main.c
+++ b/robot/main.c
@@ -5,7 +5,6 @@
#include "sim.h"
#endif
-#include <unistd.h>
int main() {
#ifdef W2_SIM
w2_sim_setup();
diff --git a/robot/mode_dirc.c b/robot/mode_dirc.c
index 9ed9fef..5988816 100644
--- a/robot/mode_dirc.c
+++ b/robot/mode_dirc.c
@@ -1,10 +1,13 @@
#include "mode_dirc.h"
#include "io.h"
+#include "orangutan_shim.h"
+
int16_t g_w2_mode_dirc_motor_l = 0;
int16_t g_w2_mode_dirc_motor_r = 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;
}
diff --git a/robot/modes.c b/robot/modes.c
index f73a809..7decf47 100644
--- a/robot/modes.c
+++ b/robot/modes.c
@@ -46,13 +46,11 @@ void w2_modes_switch(w2_e_mode new_mode, bool replace) {
}
// forward mode change to sercomm
- size_t msg_size = sizeof(w2_s_cmd_mode_tx);
- w2_s_cmd_mode_tx *msg = malloc(msg_size);
- msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX;
- msg->mode = (uint8_t)new_mode;
- w2_s_bin *msg_bin = w2_bin_s_alloc(msg_size, (uint8_t *)msg);
+ W2_CREATE_MSG_BIN(w2_s_cmd_mode_tx, msg, msg_bin);
+ msg->opcode = W2_CMD_MODE | W2_CMDDIR_TX;
+ msg->mode = new_mode;
+
w2_sercomm_append_msg(msg_bin);
- free(msg);
free(msg_bin);
}
diff --git a/robot/sercomm.c b/robot/sercomm.c
index deef39f..519568d 100644
--- a/robot/sercomm.c
+++ b/robot/sercomm.c
@@ -37,10 +37,11 @@ void w2_sercomm_main() {
#ifdef W2_SIM
w2_sim_print_serial(data);
#endif
- serial_send("\xff", 1);
+ serial_send_blocking("\xff", 1);
for (uint8_t i = 0; i < data->bytes; i++) {
uint8_t byte = data->data[i];
- byte == 0xff ? serial_send("\xff\xff", 2) : serial_send((char *)&byte, 1);
+ byte == 0xff ? serial_send_blocking("\xff\xff", 2)
+ : serial_send_blocking((char *)&byte, 1);
}
g_w2_sercomm_offset = (g_w2_sercomm_offset + 1) % W2_SERCOMM_BUFFER_SIZE;
}
diff --git a/robot/sim.c b/robot/sim.c
index 5956fbb..9cce12f 100644
--- a/robot/sim.c
+++ b/robot/sim.c
@@ -74,7 +74,7 @@ void serial_set_baud_rate(unsigned int rate) {
simprintfunc("serial_set_baud_rate", "%u", rate);
}
-void serial_send(char* message, unsigned int length) {
+void serial_send_blocking(char* message, unsigned int length) {
for (unsigned int byte = 0; byte < length; byte++)
putc(message[byte] & 0xff, stdout);
fflush(stdout);
diff --git a/robot/sim.h b/robot/sim.h
index 9e250da..9d73585 100644
--- a/robot/sim.h
+++ b/robot/sim.h
@@ -71,7 +71,7 @@ void green_led(unsigned char on); // NOLINT
void clear(); // NOLINT
void play(const char *melody); // NOLINT
void serial_set_baud_rate(unsigned int rate); // NOLINT
-void serial_send(char *message, unsigned int length); // NOLINT
+void serial_send_blocking(char *message, unsigned int length); // NOLINT
void serial_receive_ring(char *buffer, unsigned char size); // NOLINT
unsigned char serial_get_received_bytes(); // NOLINT
void set_motors(int left, int right); // NOLINT
diff --git a/shared/util.c b/shared/util.c
index 55f3491..68503e8 100644
--- a/shared/util.c
+++ b/shared/util.c
@@ -4,3 +4,5 @@ unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_m
return (unsigned long)((((double)(current_avg)) * ((double)(1.f - W2_EMA_WEIGHT))) +
(((double)(new_meas)) * ((double)(W2_EMA_WEIGHT))));
}
+
+int w2_sign(int n) { return (n > 0) - (n < 0); }
diff --git a/shared/util.h b/shared/util.h
index 465e043..230c3e4 100644
--- a/shared/util.h
+++ b/shared/util.h
@@ -7,3 +7,4 @@
#define W2_RANGE(min, val, max) W2_MIN(max, W2_MAX(val, min))
unsigned long w2_util_exp_mov_avg(unsigned long current_avg, unsigned long new_meas);
+int w2_sign(int n);