So we need to simulate a microcontroller program that interacts with something and communicates with something. Since my final project involves a sprinkler head moving in a 2D plane, I want to simulate a program that reads a shape from somewhere and controls 2 servos (one per axis) that draw the shape. Ideally I would be able to read the servos’ positions and plot the graph they actually make in some display.

So I looked at Wokwi for simulation and the Arduino UNO R3 as the board, initially. One problem that came up is that the servo that comes with Wokwi doesn’t have a pin to read its current position.

I looked at Tinkercad to see if its servo had that 4th pin, but it doesn’t, so I went back to Wokwi, especially since it had the SD card peripheral.

To start, I added 2 servos (servo X and servo Y) and made them wipe. A clip of the simulation follows:

The initial test simulation in Wokwi: 2 servos wiping

Then I implemented reading from an SD card. The program reads a shape text file inspired by SVG paths. Each line contains a movement code letter followed by some number of values in \([0, 1]\), separated by spaces. The specification is as follows:

Code # Values Description
M 2 Move to the (x, y) pair specified as fast as possible.
L 2 Draw a line to the (x, y) pair specified.
C 6 Draw a Bezier cubic. The first and second (x, y) pairs are control points. The last one is the final position.

For now, the file must end in a newline. An example shape file and its simulation follows:

M 0.9 0.5
C 0.9 0.7 0.7 0.9 0.5 0.9
C 0.3 0.9 0.1 0.7 0.1 0.5
C 0.1 0.3 0.3 0.1 0.5 0.1
C 0.7 0.1 0.9 0.3 0.9 0.5
L 0.1 0.5
The shape file.
The servos following the shape on the left

So then I added a 32×32 LED display to show where the sprinkler head would be if the servos were actually controlling one instead of just spinning around. There’s probably a lot of cheating with electronics involved since Wokwi doesn’t understand things like current and power and stuff. I also added a couple of convenience code letters:

Code # Values Description
H 1 Draw a horizontal line to the x position specified.
V 1 Draw a vertical line to the y position specified.

Anyway, here’s the new shape file and simulation:

M 0.05 0.95
H 0.45
M 0.25 0.95
V 0.55
M 0.95 0.95
H 0.55
V 0.55
H 0.95
M 0.55 0.75
H 0.85
M 0.45 0.45
C 0.10 0.45 0.05 0.40 0.05 0.35
C 0.05 0.25 0.45 0.25 0.45 0.15
C 0.45 0.10 0.40 0.05 0.05 0.05
M 0.55 0.45
H 0.95
M 0.75 0.45
V 0.05
The shape file.
The servos following the shape on the left, drawing the word "TEST"

The code, which can be found here, is as follows:

#include <Servo.h>
#include <SD.h>
#include <MD_MAX72xx.h>

const int CS_PIN = 10;

Servo servo_x;
Servo servo_y;
int pos = 0;
unsigned long micro_time = 0;
unsigned long wipe_time = 0;
const unsigned long PERIOD = 800uL * 1000;

const int LED_SIDE_LENGTH = 32;
MD_MAX72XX mx = MD_MAX72XX(MD_MAX72XX::PAROLA_HW, 9, sq(LED_SIDE_LENGTH / 8));

struct ShapeTracer {
  bool init(const char* filename, Servo* servo_x, Servo* servo_y) {
    if (shape_exists)
      shape.close();

    shape = SD.open(filename);
    shape_exists = true;
    if (!shape) {
      shape_exists = false;
      Serial.println("error opening file!");
      return false;
    }

    servos[0] = servo_x;
    servos[1] = servo_y;
    for (int i = 0; i < 2; ++i)
      next_pos[i] = servos[i]->read() / 180.0f;
    finished = true;
    return true;
  }

  void readFloats(float* buffer, int num_floats) {
    for (int i = 0; i < num_floats; ++i)
      buffer[i] = shape.parseFloat();
  }

  static float cubicInterp(float x0, float x1, float x2, float x3, float t) {
    return (x0*(1-t) + x1*3*t)*(1-t)*(1-t) + (x2*3*(1-t) + x3*t)*t*t;
  }

  static float lineLength(float* prev_pos, float* next_pos) {
    return sqrt(sq(next_pos[0] - prev_pos[0]) + sq(next_pos[1] - prev_pos[1]));
  }

  static float cubicLength(float* prev_pos, float* inter_pos, float* next_pos) {
    float prev[2] { prev_pos[0], prev_pos[1] };
    float next[2];
    float len = 0.0;
    for (int i = 0; i < CURVE_SUBDIVISION; ++i) {
      for (int j = 0; j < 2; ++j)
        next[j] = cubicInterp(prev_pos[j], inter_pos[j], inter_pos[2 + j], next_pos[j],
          float(i + 1) / CURVE_SUBDIVISION);
      len += lineLength(prev, next);
      for (int j = 0; j < 2; ++j)
        prev[j] = next[j];
    }
    return len;
  }

  bool calcNextMotion() {
    finished = false;

    for (int i = 0; i < 2; ++i)
      prev_pos[i] = next_pos[i];

    char move;
    if (!shape.readBytes(&move, 1)) {
      finished = true;
      return false;
    }

    float floats[6];
    switch (move) {
      case 'M':
        movement = MV_MOVE;
        readFloats(next_pos, 2);
        break;

      case 'H':
        movement = MV_LINE;
        readFloats(next_pos, 1);
        line_length = lineLength(prev_pos, next_pos);
        break;

      case 'V':
        movement = MV_LINE;
        readFloats(next_pos + 1, 1);
        line_length = lineLength(prev_pos, next_pos);
        break;

      case 'L':
        movement = MV_LINE;
        readFloats(next_pos, 2);
        line_length = lineLength(prev_pos, next_pos);
        break;

      case 'C':
        movement = MV_CUBIC;
        readFloats(floats, 6);
        for (int i = 0; i < 4; ++i)
          inter_pos[i] = floats[i];
        for (int i = 0; i < 2; ++i) {
          next_pos[i] = floats[4 + i];
          curve_pos_1[i] = prev_pos[i];
        }
        curve_segment = 0;
        curve_time = 0;
        //line_length = cubicLength(prev_pos, inter_pos, next_pos);
        break;
    }

    shape.find('\n');
    time = 0;

    return true;
  }

  bool calcNextCurveSegment() {
    if (curve_segment == CURVE_SUBDIVISION)
      return false;
    time -= curve_time;
    ++curve_segment;
    for (int i = 0; i < 2; ++i) {
      curve_pos_0[i] = curve_pos_1[i];
      curve_pos_1[i] = cubicInterp(prev_pos[i], inter_pos[i], inter_pos[2 + i], next_pos[i],
        float(curve_segment) / CURVE_SUBDIVISION);
      line_length = lineLength(curve_pos_0, curve_pos_1);
      curve_time = (unsigned long)(line_length / speed * 1000000L);
    }
    return true;
  }

  bool mvMove() {
    for (int i = 0; i < 2; ++i)
      servos[i]->write(int(round(next_pos[i] * 180)));
    return time >= MOVE_TIME;
  }

  bool mvLine() {
    float interp = speed * time / 1000000L / line_length;
    for (int i = 0; i < 2; ++i) {
      float pos = prev_pos[i] + (next_pos[i] - prev_pos[i]) * interp;
      servos[i]->write(int(round(pos * 180)));
    }
    return interp >= 1;
  }

  bool mvCubic() {
    while (time >= curve_time) {
      if (!calcNextCurveSegment()) {
        for (int i = 0; i < 2; ++i)
          servos[i]->write(int(round(next_pos[i] * 180)));
        return true;
      }
    }
    float interp = speed * time / 1000000L / line_length;
    for (int i = 0; i < 2; ++i) {
      float pos = curve_pos_0[i] + (curve_pos_1[i] - curve_pos_0[i]) * interp;
      servos[i]->write(int(round(pos * 180)));
    }
    return false;
  }

  // d_time is in microseconds
  void update(unsigned long d_time) {
    if (finished) {
      if (!calcNextMotion())
        return;
    }

    time += d_time;
    switch (movement) {
      case MV_MOVE: finished = mvMove(); break;
      case MV_LINE: finished = mvLine(); break;
      case MV_CUBIC: finished = mvCubic(); break;
    }
  }

  enum MovementType {
    MV_MOVE, // move to the next position and stop
    MV_LINE,  // draw a line to the next position
    MV_CUBIC, // draw a Bézier cubic curve to the next position
  };

  static const unsigned long MOVE_TIME = 250uL * 1000;
  static const float speed = 0.8; // units per second
  static const int CURVE_SUBDIVISION = 64;

  File shape;
  bool shape_exists;
  MovementType movement;
  float prev_pos[2];
  float inter_pos[4];
  float next_pos[2];
  float line_length;
  float curve_pos_0[2]; // current line segment in curve
  float curve_pos_1[2];
  int curve_segment;
  unsigned long curve_time; // time needed for the current curve segment
  Servo* servos[2];
  bool finished;
  unsigned long time; // in microseconds
};

ShapeTracer tracer;

void setup() {
  Serial.begin(115200);

  servo_x.attach(3);
  servo_y.attach(5);

  if (!SD.begin(CS_PIN)) {
    Serial.println("Card initialization failed!");
    while (true);
  }
  
  tracer.init("shape.txt", &servo_x, &servo_y);

  // For dot matrix
  //SPI.beginTransaction(SPISettings(16000000, MSBFIRST, SPI_MODE0));

  mx.begin();
  mx.clear();
  micro_time = micros();
}

void setLed(float x, float y) {
  int pixel_x = min(floor((1 - x) * LED_SIDE_LENGTH), LED_SIDE_LENGTH - 1);
  int pixel_y = min(floor((1 - y) * LED_SIDE_LENGTH), LED_SIDE_LENGTH - 1);
  int row = pixel_y % 8;
  int col = pixel_x % LED_SIDE_LENGTH + (pixel_y / 8 * LED_SIDE_LENGTH);
  mx.setPoint(row, col, true);
}

void loop() {
  long d_time = micros() - micro_time;
  micro_time += d_time;

  tracer.update(d_time);
  setLed(servo_x.read() / 180.0, servo_y.read() / 180.0);
  /*wipe_time += d_time;
  wipe_time %= PERIOD;

  int angle = wipe_time * 9 / (PERIOD / 20) * 2;
  if (angle > 180)
    angle = 360 - angle;

  servo_x.write(angle);
  servo_y.write(angle);*/
}