diff options
| -rw-r--r-- | client/ui_dirc.c | 49 |
1 files changed, 33 insertions, 16 deletions
diff --git a/client/ui_dirc.c b/client/ui_dirc.c index cddd2c4..61abaf4 100644 --- a/client/ui_dirc.c +++ b/client/ui_dirc.c @@ -25,26 +25,43 @@ int w2_avg(int* samples, unsigned int sample_count) { return (int) total; } -void w2_ui_dirc() { - static unsigned int idx = 0; - int ch = 0; - unsigned int presses = 0; - - static double drive_l = 0.f; - static int drive_l_avg[W2_DIRC_SPL] = {0}; +#define W2_DIRC_MOTOR_DRIVER(name) \ +int w2_dirc_motor_##name(unsigned int forward, unsigned int backward) {\ + static unsigned int idx = 0;\ + \ + static double drive = 0.f;\ + static int drive_avg[W2_DIRC_SPL] = {0};\ + \ + drive *= W2_DIRC_MOD;\ + drive += W2_DIRC_ADD * forward + -W2_DIRC_ADD * backward;\ + drive = W2_RANGE(-254, drive, 255);\ + \ + idx = (idx + 1) % W2_DIRC_SPL;\ + drive_avg[idx] = (int) W2_RANGE(-254, drive * W2_DIRC_PAD, 255);\ + \ + return w2_avg(drive_avg, W2_DIRC_SPL);\ +} +W2_DIRC_MOTOR_DRIVER(l); +W2_DIRC_MOTOR_DRIVER(r); +void w2_ui_dirc() { + int ch = 0; + unsigned int lb = 0; + unsigned int lf = 0; + unsigned int rb = 0; + unsigned int rf = 0; while ((ch = getch()) != -1) { - if (ch == 'a') presses++; + if (ch == 'a' || ch == 's') lb++; + if (ch == 'q' || ch == 'w') lf++; + if (ch == 'd' || ch == 's') rb++; + if (ch == 'e' || ch == 'w') rf++; } - drive_l *= W2_DIRC_MOD; - drive_l += W2_DIRC_ADD * presses; - drive_l = W2_MIN(100, drive_l); - - idx = (idx + 1) % W2_DIRC_SPL; - drive_l_avg[idx] = (int) W2_MIN(100, drive_l * W2_DIRC_PAD); + int drive_l = w2_dirc_motor_l(lf, lb); + int drive_r = w2_dirc_motor_r(rf, rb); - mvaddstr(4, 0, "drive: "); - w2_ui_bar_graph(w2_avg(drive_l_avg, W2_DIRC_SPL)); + char temp[32] = {0}; + sprintf(temp, "l: %04i, r: %04i", drive_l, drive_r); + mvaddstr(4, 0, temp); } |