139 lines
3.0 KiB
Arduino
139 lines
3.0 KiB
Arduino
#include <SoftwareSerial.h>
|
|
#include <Servo.h>
|
|
|
|
SoftwareSerial BT(3, 2); // RX, TX
|
|
|
|
const byte N = 4;
|
|
const byte PINS[N] = {4, 5, 6, 7};
|
|
const int HOME_POSE[N] = {50, 0, 140, 10};
|
|
const int LIMITS[N][2] = {{0, 180}, {0, 180}, {0, 180}, {0, 180}};
|
|
const unsigned long TICK_MS = 15;
|
|
const long MAX_MS = 10000;
|
|
|
|
Servo servos[N];
|
|
int cur[N];
|
|
|
|
struct LineBuf {
|
|
char buf[48];
|
|
byte len;
|
|
};
|
|
|
|
LineBuf btLine, usbLine;
|
|
|
|
bool readLine(Stream &s, LineBuf &l) {
|
|
while (s.available()) {
|
|
char c = s.read();
|
|
if (c == '\r') continue;
|
|
if (c == '\n') {
|
|
l.buf[l.len] = 0;
|
|
l.len = 0;
|
|
return true;
|
|
}
|
|
if (l.len < sizeof(l.buf) - 1) l.buf[l.len++] = c;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool stopRequested() {
|
|
if (readLine(BT, btLine) && toupper(btLine.buf[0]) == 'S') return true;
|
|
if (readLine(Serial, usbLine) && toupper(usbLine.buf[0]) == 'S') return true;
|
|
return false;
|
|
}
|
|
|
|
bool moveTo(const int target[N], unsigned long ms) {
|
|
int start[N];
|
|
for (byte i = 0; i < N; i++) start[i] = cur[i];
|
|
|
|
unsigned long t0 = millis();
|
|
while (true) {
|
|
unsigned long t = millis() - t0;
|
|
float p = (ms == 0 || t >= ms) ? 1.0 : (float)t / ms;
|
|
float eased = p * p * (3 - 2 * p);
|
|
|
|
for (byte i = 0; i < N; i++) {
|
|
int a = round(start[i] + (target[i] - start[i]) * eased);
|
|
if (a != cur[i]) {
|
|
servos[i].write(a);
|
|
cur[i] = a;
|
|
}
|
|
}
|
|
if (p >= 1.0) return true;
|
|
|
|
unsigned long next = millis() + TICK_MS;
|
|
while ((long)(next - millis()) > 0) {
|
|
if (stopRequested()) return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
void report(Stream &out, const char *word) {
|
|
out.print(word);
|
|
for (byte i = 0; i < N; i++) {
|
|
out.print(' ');
|
|
out.print(cur[i]);
|
|
}
|
|
out.println();
|
|
}
|
|
|
|
bool parseMove(char *p, int target[N], long &ms) {
|
|
char *end;
|
|
for (byte i = 0; i < N; i++) {
|
|
long v = strtol(p, &end, 10);
|
|
if (end == p) return false;
|
|
target[i] = constrain(v, LIMITS[i][0], LIMITS[i][1]);
|
|
p = end;
|
|
}
|
|
ms = strtol(p, &end, 10);
|
|
if (end == p) return false;
|
|
ms = constrain(ms, 0, MAX_MS);
|
|
return true;
|
|
}
|
|
|
|
void handle(char *line, Stream &out) {
|
|
switch (toupper(line[0])) {
|
|
case 'P':
|
|
out.println("PONG");
|
|
break;
|
|
case 'S':
|
|
report(out, "STOPPED");
|
|
break;
|
|
case 'H':
|
|
report(out, moveTo(HOME_POSE, 1000) ? "OK" : "STOPPED");
|
|
break;
|
|
case 'M': {
|
|
int target[N];
|
|
long ms;
|
|
if (!parseMove(line + 1, target, ms)) {
|
|
out.println("ERR bad move");
|
|
break;
|
|
}
|
|
report(out, moveTo(target, ms) ? "OK" : "STOPPED");
|
|
break;
|
|
}
|
|
case 0:
|
|
break;
|
|
default:
|
|
out.println("ERR unknown");
|
|
}
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
BT.begin(9600);
|
|
|
|
for (byte i = 0; i < N; i++) {
|
|
servos[i].attach(PINS[i]);
|
|
servos[i].write(HOME_POSE[i]);
|
|
cur[i] = HOME_POSE[i];
|
|
}
|
|
delay(1200);
|
|
|
|
Serial.println("READY");
|
|
BT.println("READY");
|
|
}
|
|
|
|
void loop() {
|
|
if (readLine(BT, btLine)) handle(btLine.buf, BT);
|
|
if (readLine(Serial, usbLine)) handle(usbLine.buf, Serial);
|
|
}
|