suzy plotter

stepper.h

#define MICROSTEPS 8
#define PERIOD_PER_MICROSTEP_200 2

class Stepper {
  bool current_direction;
  uint8_t stp, dir, en;
  uint16_t period;
  int steps_per_rev, current_step;
public:
  Stepper(uint8_t step_pin, uint8_t dir_pin, uint8_t en_pin, uint16_t steps) {
    stp = step_pin;
    dir = dir_pin;
    en = en_pin;
    steps_per_rev = steps * MICROSTEPS;
    period = PERIOD_PER_MICROSTEP_200 * 1600 / steps_per_rev;
    current_step = 0;
  }
  void init();
  void disable() { digitalWrite(en, HIGH); };
  void enable() { digitalWrite(en, LOW); };
  void direction(bool);
  uint16_t step();
  void step(int);
  uint16_t get_steps_per_rev() { return steps_per_rev; };
  uint16_t get_min_step_period() { return period; };
  int get_current_step() { return current_step; };
  void reset_step_count() { current_step = 0; };
};

void Stepper::init() {
  pinMode(stp, OUTPUT);
  pinMode(dir, OUTPUT);
  pinMode(en, OUTPUT);
  disable();
  direction(true);
}

void Stepper::direction(bool d) {
  digitalWrite(dir, d);
  current_direction = d;
}

uint16_t Stepper::step() {
  digitalWrite(stp, HIGH);
  delayMicroseconds(1);
  digitalWrite(stp, LOW);
  // update awareness of current position
  current_step += current_direction ? 1 : -1;
  // ...but don't allow it to wind up (reset to zero after a full rotation any direction)
  while (current_step > steps_per_rev) current_step -= steps_per_rev;
  while (current_step < -steps_per_rev) current_step += steps_per_rev;
  return period;
}

void Stepper::step(int n) {
  direction(n >= 0);
  for (size_t s = 0; s < abs(n); s++) {
    delay(step());
  }
}

suzy_plotter.ino

#include <Servo.h>
#include "stepper.h"

#define SR_STEP 16
#define SR_DIR 14
#define SR_EN 15

#define SL_STEP A2
#define SL_DIR A1
#define SL_EN A0

#define PEN_PIN 10
#define PEN_STEP_TIME 4
#define PED_DAB_DWELL 42
#define PEN_RAISED 120
#define PEN_UP 138
#define PEN_DOWN 156

#define BUTTON_PIN 2
#define STATUS_PIN 9

// log commands
#define L_DEBUG 'd'
#define L_INFO  'i'
#define L_WARN  'w'
#define L_ERROR 'e'

// control commands
#define C_NOP   '\n' //[x] (empty lines are noops)
#define C_HOME  'h'  //[x] (raise, move home and disarm. just reset state if already disarmed.)
#define C_ARM   '!'  //[x] (energize steppers)
#define C_DIS   'o'  //[x] (disarm but stay here)
#define C_UP    '^'  //[x] (raise pen, stay armed, and stay here)
#define C_DOWN  'v'  //[x] (pen down)
#define C_DOT   '.'  //[x] (make a dot here)
#define C_MOV   '>'  //[x] , left_target_theta, right_target_theta (rad)
#define C_STEP  '#'  //[x] , left_delta_steps, right_delta_steps (rad)
#define C_BOUND 'b'  //[ ] (trace a lissajous-ish pattern to estimate drawing bounds

// state / status
#define S_ACK  'y'  // [, message]
#define S_NACK 'n'  // [, message]
#define S_PROG '#'  // , progress (float, 0–1) (TODO within move_together)
#define S_CTS  'C'  // (clear to send next command)


#define log(level, m...) do { Serial.write(level); Serial.print('\t'); Serial.println(m); } while (0)
#define ack(m...) do { Serial.write(S_ACK); Serial.print('\t'); Serial.println(m); } while (0)
#define nack(m...) do { Serial.write(S_NACK); Serial.print('\t'); Serial.println(m); } while (0)

Stepper s_r(SR_STEP, SR_DIR, SR_EN, 50);
Stepper s_l(SL_STEP, SL_DIR, SL_EN, 200);

Servo pen_servo;
int pen_position = PEN_RAISED;
volatile bool armed = false;

void pen_to(int pos) {
  while (pen_position > pos) {
    pen_position -= 1;
    pen_servo.write(pen_position);
    delay(PEN_STEP_TIME);
  }
  while (pen_position < pos) {
    pen_position += 1;
    pen_servo.write(pen_position);
    delay(PEN_STEP_TIME);
  }
}

bool arm() {
  s_l.enable();
  s_r.enable();
  armed = true;
  log(L_INFO, "steppers armed");
  pen_up();
  log(L_INFO, "pen ready");
  digitalWrite(STATUS_PIN, HIGH);
}

bool disarm() {
  s_l.disable();
  s_r.disable();
  armed = false;
  log(L_INFO, "steppers disarmed");
  pen_up();
  log(L_INFO, "pen raised");
  digitalWrite(STATUS_PIN, LOW);
}

void pen_down() {
  pen_to(PEN_DOWN);
  delay(PED_DAB_DWELL);
}

void pen_up() {
  pen_to(armed ? PEN_UP : PEN_RAISED);
}

void pen_dab() {
  pen_down();
  pen_up();
}

uint32_t _last_button_toggle = 0;
void HANDLE_BUTTON() {
  uint32_t now = millis();
  if (now - _last_button_toggle < 50) return;
  if (armed) {
    disarm();
  } else {
    arm();
  }
}

void setup() {
  s_r.init();
  s_l.init();
  pen_servo.attach(PEN_PIN);
  pen_to(PEN_UP);

  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(STATUS_PIN, OUTPUT);

  attachInterrupt(digitalPinToInterrupt(2), HANDLE_BUTTON, FALLING);
  disarm();

  Serial.begin(115200);
  while (!Serial);
  log(L_INFO, "hello");
}

bool expect(char c) {
  char * buff;
  uint8_t n = Serial.readBytes(buff, 1);
  if (n == 0) {
    log(L_ERROR, F("expected a char; no data recieved"));
    return false;
  }
  if (*buff != c) {
    log(L_ERROR, F("expected char did not match"));
    return false;
  }
  return true;
}

bool cmd_eol() {
  if (!expect('\n')) {
    if (Serial.find('\n')) {
      log(L_WARN, F("found garbage before \\n for this command."));
    } else {
      log(L_ERROR, F("could not find \\n to terminate command. might still be in a bad state."));
    }
    return false;
  }
  return true;
}

/** 
 * Strict serial float parser that knows how to fail, sort of 
 * 
 * Note that there is some kind of implicit max float length for parseFloat, shorter than
 * constants like PI in arduino.h. I can't find it documented. ¯\_(ツ)_/¯
 */
bool get_float(float * out) {
  uint32_t t0 = millis();
  *out = Serial.parseFloat(SKIP_NONE);
  // giant terrible hack because parseFloat returns 0.00 when it fails >:(
  if (millis() - t0 > 950) {
    // assume a timeout if it takes at least 0.95s
    log(L_WARN, F("assuming invalid float based on timeout -- arduino lib doesn't really provide a good way to know"));
    return false;
  }
  return true;
}

/** 
 * Strict serial int parser that knows how to fail, sort of 
 * 
 * should work for the 4-byte signed long range of integers
 */
bool get_int(int * out) {
  uint32_t t0 = millis();
  *out = Serial.parseInt(SKIP_NONE);
  // giant terrible hack because parseFloat returns 0.00 when it fails >:(
  if (millis() - t0 > 950) {
    // assume a timeout if it takes at least 0.95s
    log(L_WARN, F("assuming invalid int (0) based on timeout -- arduino lib doesn't really provide a good way to know"));
    return false;
  }
  return true;
}

int steps_from_here(Stepper * s, float target_theta) {
  if (fabs(target_theta) > TWO_PI) {
    log(L_WARN, F("received target theta greater than +/- 2*PI -- folding it into normal range"));
    while (target_theta > TWO_PI) target_theta -= TWO_PI;
    while (target_theta < -TWO_PI) target_theta += TWO_PI;
  }
  int target_step = round(target_theta / TWO_PI  * s->get_steps_per_rev());
  int current_step = s->get_current_step();
  int half_rev = s->get_steps_per_rev() / 2;
  int diff = target_step - current_step;
  log(L_DEBUG, F("step diff:"));
  log(L_DEBUG, diff);
  if (abs(diff) > half_rev) {
    // go the other way around
    log(L_DEBUG, F("crossing zero to take the shorter path"));
    if (diff > 0) {
      log(L_DEBUG, F("subbing from full positive"));
      diff = -(s->get_steps_per_rev() - diff);
    } else {
      log(L_DEBUG, F("adding from full negative"));
      diff = s->get_steps_per_rev() + diff;
    }
  }
  log(L_DEBUG, diff);
  return diff;
}

void move_together(Stepper * left, int left_steps, Stepper * right, int right_steps) {
  left->direction(left_steps > 0);
  right->direction(right_steps > 0);
  left_steps = abs(left_steps);
  right_steps = abs(right_steps);
  uint16_t left_min_time = left_steps * left->get_min_step_period();
  uint16_t right_min_time = right_steps * right->get_min_step_period();
  uint16_t min_time = max(left_min_time, right_min_time);
  uint16_t left_stepped = 0;
  uint16_t right_stepped = 0;
  uint32_t t0 = millis();
  while(left_stepped < left_steps || right_stepped < right_steps) {
    uint32_t dt = millis() - t0;
    if (dt * left_steps / min_time > left_stepped && left_stepped < left_steps) {
      left->step();
      left_stepped += 1;
    }
    if (dt * right_steps / min_time > right_stepped && right_stepped < right_steps) {
      right->step();
      right_stepped += 1;
    }
  }
}

bool handle_cmd(byte cmd) {switch (cmd) {
case C_DOT:
  if (!cmd_eol()) goto bad_cmd;
  ack("dot");
  pen_dab();
  return true;

case C_ARM:
  if (!cmd_eol()) goto bad_cmd;
  ack("arm");
  arm();
  return true;

case C_DIS:
  if (!cmd_eol()) goto bad_cmd;
  ack("disarm");
  disarm();
  return true;

case C_UP:
  if (!cmd_eol()) goto bad_cmd;
  ack("up");
  pen_up();
  return true;

case C_DOWN:
  if (!cmd_eol()) goto bad_cmd;
  if (!armed) {
    log(L_WARN, F("putting pen down while not armed"));
  }
  ack("down");
  pen_down();
  return true;

case C_HOME:
  if (!cmd_eol()) goto bad_cmd;
  ack("home");
  if (armed) {
    log(L_DEBUG, F("pen up & move home"));
    pen_up();
    move_together(&s_l, steps_from_here(&s_l, 0), &s_r, steps_from_here(&s_r, 0));
    disarm();
  } else {
    log(L_DEBUG, F("resetting counters to home"));
    s_l.reset_step_count();
    s_r.reset_step_count();
  }
  return true;

case C_MOV: {
  float left_theta, right_theta;
  if (!get_float(&left_theta)) {
    log(L_ERROR, F("did not find valid float for left theta"));
    goto bad_cmd;
  }
  if (!expect(',')) {
    log(L_ERROR, F("expected comma to separate left theta from right"));
    goto bad_cmd;
  }
  if (!get_float(&right_theta)) {
    log(L_ERROR, F("did not find valid float for right theta"));
    goto bad_cmd;
  }
  if (!cmd_eol()) goto bad_cmd;
  if (!armed) {
    log(L_ERROR, F("must be armed to move"));
    goto bad_cmd;
  }
  ack("mov");
  log(L_DEBUG, F("== Left steps =="));
  int left_steps = steps_from_here(&s_l, left_theta);
  log(L_DEBUG, F("== Right steps =="));
  int right_steps = steps_from_here(&s_r, right_theta);
  move_together(&s_l, left_steps, &s_r, right_steps);
  return true;
}

case C_STEP: {
  int left_steps, right_steps;
  if (!get_int(&left_steps)) {
    log(L_ERROR, F("did not find a valid int for left steps"));
    goto bad_cmd;
  }
  if (!expect(',')) {
    log(L_ERROR, F("expected comma to separate left and right int steps"));
    goto bad_cmd;
  }
  if (!get_int(&right_steps)) {
    log(L_ERROR, F("did not find a valid int for right steps"));
    goto bad_cmd;
  }
  if (!cmd_eol()) goto bad_cmd;
  if (!armed) {
    log(L_ERROR, F("must be armed to move"));
    goto bad_cmd;
  }
  ack("step");
  move_together(&s_l, left_steps, &s_r, right_steps);
  return true;
}

case C_BOUND: {
  if (!cmd_eol()) goto bad_cmd;
  ack("bounds-ish");
  bool was_armed = armed;
  if (!was_armed) {
    log(L_WARN, F("arming to enable bounds movement"));
    arm();
  }
  uint16_t left_steps = s_l.get_steps_per_rev() * 2;
  uint16_t right_steps = s_r.get_steps_per_rev() * 3;
  move_together(&s_l, left_steps, &s_r, right_steps);
  if (!was_armed) {
    log(L_INFO, F("disarming to restore pre-bounds-movement state"));
    disarm();
  }
  return true;
}

case C_NOP:
  ack("nop");
  return true;

default:
  log(L_WARN, F("ignoring invalid command"));
  log(L_WARN, (char)cmd);
  cmd_eol();
bad_cmd:
  nack((char)cmd);
  return false;
}}


void loop() {
  if (Serial.available()) {
    if (handle_cmd(Serial.read())) {
      Serial.println(S_CTS);
    }
  }
}