diff options
author | UnavailableDev <69792062+UnavailableDev@users.noreply.github.com> | 2023-06-06 12:00:58 +0200 |
---|---|---|
committer | UnavailableDev <69792062+UnavailableDev@users.noreply.github.com> | 2023-06-06 12:00:58 +0200 |
commit | 077069b82a34bd5e0d51509eb751df9b44a5f6d3 (patch) | |
tree | 1753f1266c2837282c7a1f7ce8137161c85a1a0a /zumo/control.cpp | |
parent | f2335307a26a8ab3e18d8990d2640a8de4cbd0e4 (diff) | |
parent | 0f764db3c3595e863a4949c67592451c7d65a2cf (diff) |
Merge branch 'master' of https://github.com/unavailabledev/avans-dui
Diffstat (limited to 'zumo/control.cpp')
-rw-r--r-- | zumo/control.cpp | 43 |
1 files changed, 43 insertions, 0 deletions
diff --git a/zumo/control.cpp b/zumo/control.cpp new file mode 100644 index 0000000..778969a --- /dev/null +++ b/zumo/control.cpp @@ -0,0 +1,43 @@ +#include <Zumo32U4Motors.h> +#include <Arduino.h> +#include <Wire.h> + +#include "protocol.h" +#include "control.h" + +#define DUI_SPEED_MOD 96.0f +#define DUI_MOTOR_DIFF 0.6f + +void apply_state(dui_state_t *state) { + float motor_l = 0.5f * state->speed * (+1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD; + float motor_r = 0.5f * state->speed * (-1.f * state->steer * DUI_MOTOR_DIFF - DUI_MOTOR_DIFF + 2) * state->speed_mod * DUI_SPEED_MOD; + + Serial.print(motor_l); + Serial.print(" "); + Serial.print(motor_r); + + Serial.println(""); + + Zumo32U4Motors::setLeftSpeed((int16_t) motor_l); + Zumo32U4Motors::setRightSpeed((int16_t) motor_r); + + // TODO: print sign on OLED screen +} + +inline bool rx() { return !digitalRead(DUI_PINOUT_NICLA_RX); } + +unsigned char uart_read() { + if (rx() == true) return 0x00; // return immediately if line is idle + + delayMicroseconds(1200); // wait out start bit + + unsigned char byte = 0x00; + for (unsigned int i = 0; i < 8; i++) { + byte = (byte << 1) | rx(); + delayMicroseconds(1000); + } + + delayMicroseconds(1000); // wait out stop bit + + return byte; +} |