Servo control terminal over serial port

servo.c

#include <Arduino.h>
#include <Servo.h>

#define SERVO_DELAY 500
#define SERVO_STEP 1
// or can be 1000 and 2

Servo servo_x, servo_y, servo_claw, servo_rotate;
char prompt[25], cmd_buf[32];
int val_x, val_y, val_claw, val_rotate;

void write_servo();
void write_prompt();
void servo_move(Servo &servo, int pos, int step, int ms);
void process_escape(int *val1, int *val2);
void read_command(char *buf, int *val1, int *val2);
void parse_command(char *buf, int *val1, int *val2);


void setup() {
    Serial.begin(9600);
    delay(1000);

    // set start values before attaching servos to avoid setting default values 
    val_x = 30;
    val_y = 135;
    val_claw = 0;
    val_rotate = 90;
    write_servo();

    servo_rotate.attach(11);
    servo_y.attach(10);
    servo_claw.attach(9);
    servo_x.attach(6);
}

/*
 * Smoothly move servo to angle \c pos, by \c step microseconds each \c delay us.
 * @param servo Servo object to move
 * @param pos Target angle
 * @param step Step size
 * @param delay Delay between steps in microseconds
 */
void servo_move(Servo &servo, int pos, int step = SERVO_STEP, int delay = SERVO_DELAY) {
    int start_us = servo.readMicroseconds();
    int end_us = map(pos, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);

    // if servo is not attached, just turn servo to
    // the end value without smooth transition
    if (!servo.attached()) {
        servo.writeMicroseconds(end_us);
        return;
    }
    // if we are already at the end value, return
    if (start_us == end_us) {
        return;
    }

    // determine direction of movement
    if (start_us > end_us) {
        step *= -1;
    }

    // make a smooth transition
    for (int i = start_us; abs(end_us - i) > abs(step); i += step) {
        servo.writeMicroseconds(i);
        delayMicroseconds(delay);
    }

    // make sure we end up at the end value
    if (servo.readMicroseconds() != end_us) {
        servo.writeMicroseconds(end_us);
    }
}

/*
 * Set servo motors to corresponding values.
 */
void write_servo() {
    servo_move(servo_x, val_x, SERVO_STEP, SERVO_DELAY);
    servo_move(servo_y, val_y, SERVO_STEP, SERVO_DELAY);
    servo_rotate.write(val_rotate);
    servo_claw.write(val_claw);
}

/*
 * Write command prompt (with servo positions) to serial port.
 */
void write_prompt() {
    sprintf(prompt, "(%3d %3d %3d %3d) >>> ",
        servo_x.read(), servo_y.read(), servo_rotate.read(), servo_claw.read()
    );
    Serial.write(prompt);
}

/*
 * Process ANSI escape sequence to handle arrow keys on keyboard.
 * Assumes that '\e' character has already been readfrom serial port.
 * More on https://en.wikipedia.org/wiki/ANSI_escape_code#CSI_(Control_Sequence_Introducer)_sequences
 * or https://ru.wikipedia.org/wiki/Управляющие_последовательности_ANSI#CSI_коды
 */
void process_escape(int *val1, int *val2){
    while (!Serial.available()) {}
    char c = Serial.read();  // read '['

    if (c != '[') {
        Serial.print("invalid escape sequence");
        return;
    };

    while (!Serial.available()) {}
    c = Serial.read();  // process arrow keys' escape codes

    switch (c) {
    case 'A':  // up arrow
        *val1 = max(0, *val1 - 3);
        break;

    case 'B':  // down arrow
        *val1 = min(180, *val1 + 3);
        break;

    case 'C':  // right arrow
        *val2 = max(0, *val2 - 3);
        break;

    case 'D':  // left arrow
        *val2 = min(180, *val2 + 3);
        break;

    default:
        Serial.println("unsupported escape sequence");
        break;
    } 
}

/*
 * Read command from serial port to \c buf (until new line).
 * Supports line editing (backspace character) and controlling motors
 * using arrow keys via \c val1 and \c val2 parameters.
 */
void read_command(char *buf, int *val1, int *val2){
    char *ptr = buf;

    while (true) {
        while (!Serial.available()) {}
        char c = Serial.read();

        if (c == '\b') {
            if (ptr <= &buf[0]) {
                continue;
            }
            ptr--;
            Serial.write("\b \b");
            
        } else if (c == '\e') {
            process_escape(val1, val2);
            write_servo();
            
            // more on https://en.wikipedia.org/wiki/ANSI_escape_code#CSI_(Control_Sequence_Introducer)_sequences
            // or https://ru.wikipedia.org/wiki/Управляющие_последовательности_ANSI#CSI_коды
            Serial.write("\e[s");  // saves cursor position
            Serial.write("\r");    // moves cursor to the beginning of the line
            write_prompt();        // updates prompt
            Serial.write("\e[u");  // restores cursorposition 

        } else if (c == '\n') {
            *ptr = '\0';
            break;

        } else if (c == '\r') {
            Serial.write('\r');

        } else if (c == 'r') {
            abort();

        } else {
            *ptr++ = c;
            Serial.write(c);
        }
    }

    Serial.write('\n');
}

/*
 * Parse command from buffer \c buf in format "{value1} {value2}"
 * and store values to \c val1 and \c val2.
 */
void parse_command(char *buf, int *val1, int *val2) {
    // TODO what if buf dosesn't have ' ' in it ?
    char *space = strchr(buf, ' ');
    *space = '\0';

    *val1 = atoi(buf);
    *val2 = atoi(space + 1);
}

#define CONSOLE_MODE 0

/*
 * Main command loop.
 */
void loop() {

#if CONSOLE_MODE == 1
    write_prompt();
    read_command(cmd_buf, &val_claw, &val_rotate);
    parse_command(cmd_buf, &val_x, &val_y);
    write_servo();
#else
    // left
    servo_move(servo_rotate, 0);

    // left grab
    servo_move(servo_claw, 90);
    delay(1000);
    servo_move(servo_x, 93);
    servo_move(servo_y, 110);
    delay(1000);
    servo_move(servo_claw, 0);

    // home
    servo_move(servo_x, 30);
    servo_move(servo_y, 135);

    // right
    servo_move(servo_rotate, 180);
    delay(1000);

    // right release
    servo_move(servo_x, 93);
    servo_move(servo_y, 110);
    delay(1000);
    servo_move(servo_claw, 90);

    // home
    servo_move(servo_x, 30);
    servo_move(servo_y, 135);
    servo_move(servo_claw, 0);
#endif
}