blob: 778969aa4788f2454adb8bfb9b7cfd6ffcdb57c7 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
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;
}
|