Factory 3: Robot arm, on an Arduino Uno

// Factory 3: Robot arm. A servo swings lumps from one belt to another.
//
// Lumps arrive on belt A, on the left. The arm turns to belt A, closes its
// claw on a lump, swings across, opens the claw over belt B and goes back for
// the next one. The servo on pin 5 is the arm, and the picture on the screen
// is drawn from the same angle that the servo is told, so the two always agree.
//
// Wiring, board to parts:
//   TFT   CS -> pin 10   DC -> pin 9   SCK -> pin 13   MOSI -> pin 11
//         RST and LED -> 5 V, VCC -> 5 V, GND -> GND
//   Servo: signal -> pin 5, red -> 5 V, brown -> GND
//   Speed knob (10k pot): middle leg -> A0, outer legs to 5 V and GND
//   Grab knob  (10k pot): middle leg -> A1, outer legs to 5 V and GND
//   Status LED: pin 3 through 220 ohm to GND   Piezo: pin 6 to GND
//
// # What this teaches
//
// 1. Timing you can tune. Belt A offers a lump for only a short window, then
//    it falls off the end and is counted as MISSED. The arm has to do a whole
//    round trip in less time than lumps arrive. The SPEED knob sets how fast
//    the arm turns, from 50 to 400 degrees a second. Too slow and lumps are
//    missed. The turn is worked out from the milliseconds that really went by,
//    so a slow frame on the screen never slows the arm.
// 2. A minimum time. A claw does not close instantly. The GRAB knob sets how
//    long the arm waits with its claw on a lump, and anything under CLAW_MS
//    means the lump slips out of the claw (SLIPS). Too long just wastes time.
//    Tuning is finding the setting that is fast enough and still safe.
// 3. A state machine again, now with four states and a clock for each. It is
//    the same idea as the drill: loop() asks which state we are in, does that
//    state's work, and changes state only when its condition is met.
//
// The belts here move one tile at a time on a timer, which is simpler than the
// smooth belt of the last project and good enough to show the arm's timing.

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

const uint8_t TFT_CS = 10;
const uint8_t TFT_DC = 9;
const uint8_t TFT_SCK = 13;
const uint8_t TFT_MOSI = 11;
const uint8_t SERVO_PIN = 5;
const uint8_t SPEED_KNOB = A0;
const uint8_t GRAB_KNOB = A1;
const uint8_t LED_PIN = 3;
const uint8_t PIEZO_PIN = 6;

SoftSpi spi(TFT_SCK, TFT_MOSI, TFT_CS);
Ili9341 tft(spi, TFT_DC);
Servo servo;

const int16_t TILE = 16;
const int16_t WORLD_X = 8;
const int16_t WORLD_Y = 8;
const uint16_t STEEL = 0x8c71;
const uint16_t BELT = 0x31a6;
const uint16_t ORE = 0xfc60;
const uint16_t DARK = 0x10a2;
const uint16_t ARM = ILI9341_CYAN;

// The belts are both on row 5. Belt A runs columns 0 to 4 and its last tile is
// where the arm picks up. Belt B runs columns 10 to 13 and its first tile is
// where the arm drops.
const uint8_t ROW = 5;
const uint8_t PICK_COL = 4;
const uint8_t DROP_COL = 10;
const uint8_t B_LAST = 13;

// The arm turns about the middle of tile (7, 6). Angle 0 points right, 90 up and
// 180 left, so 150 reaches belt A's pickup tile and 30 reaches belt B's.
const int16_t PIVOT_X = WORLD_X + 7 * TILE + 8;
const int16_t PIVOT_Y = WORLD_Y + 6 * TILE + 8;
const uint8_t PICK_ANGLE = 150;
const uint8_t DROP_ANGLE = 30;
const uint8_t PARK_ANGLE = 90;

const unsigned long ARRIVE_MS = 2400;   // a new lump on belt A this often
const unsigned long STEP_MS = 350;      // belts move a tile this often
const unsigned long WINDOW_MS = 1700;   // a lump waits this long at the end of belt A
const unsigned long CLAW_MS = 300;      // the claw needs this long to close
const unsigned long RELEASE_MS = 200;
const unsigned long SERVO_MS = 15;      // the arm is moved at most every 15 ms
long turnCarry = 0;                     // degrees * 1000 turned but not yet written

enum ArmState { GO_PICK, GRAB, GO_DROP, RELEASE };

ArmState state = GO_PICK;
int angle = PARK_ANGLE;
int drawnAngle = -1;
int speedStep = 3;                // degrees per step, from the speed knob
unsigned long dwell = 500;        // grab time in ms, from the grab knob
unsigned long stateSince = 0;
bool holding = false;
bool aLump[PICK_COL + 1];         // a lump on each tile of belt A
bool bLump[B_LAST - DROP_COL + 1];
unsigned long waitedSince = 0;    // when the lump at the pickup tile arrived
unsigned long lastArrive = 0;
unsigned long lastStep = 0;
unsigned long lastServo = 0;
unsigned int moved = 0, missed = 0, slips = 0;
int shownSpeed = -1;
int shownDwell = -1;

// cos(angle) * 100 for 0, 5, 10 ... 90 degrees. The arm is drawn from this, so
// the sketch needs no floating point.
const int8_t COS5[19] = {100, 100, 98, 97, 94, 91, 87, 82, 77, 71, 64, 57, 50, 42, 34, 26, 17, 9, 0};

static int cosx(int a) {  // a in 0..180
  int sign = 1;
  if (a > 90) {
    a = 180 - a;
    sign = -1;
  }
  int i = a / 5;
  int frac = a % 5;
  int v = COS5[i] + ((COS5[i + 1 > 18 ? 18 : i + 1] - COS5[i]) * frac) / 5;
  return sign * v;
}
static int sinx(int a) { return cosx(90 - a < 0 ? a - 90 : 90 - a); }

static int16_t tileX(uint8_t col) { return (int16_t)(WORLD_X + col * TILE); }
static int16_t tileY(uint8_t row) { return (int16_t)(WORLD_Y + row * TILE); }

static void drawLump(int16_t cx, int16_t cy) {
  tft.fillRect((int16_t)(cx - 4), (int16_t)(cy - 4), 8, 8, ORE);
}

static void drawTile(uint8_t col, bool lump) {
  tft.fillRect(tileX(col), tileY(ROW), TILE, TILE, BELT);
  tft.fillRect(tileX(col), (int16_t)(tileY(ROW) + 7), TILE, 2, DARK);
  if (lump) {
    drawLump((int16_t)(tileX(col) + 8), (int16_t)(tileY(ROW) + 8));
  }
}

bool armDirty = true;  // the arm must be drawn again (it moved, or a tile under it was)

// Painting a belt tile that the arm hangs over would wipe the hand off the
// glass, so those two tiles ask for the arm to be drawn again afterwards.
static void drawBeltA(uint8_t col) {
  drawTile(col, aLump[col]);
  armDirty = armDirty || col == PICK_COL;
}
static void drawBeltB(uint8_t col) {
  drawTile(col, bLump[col - DROP_COL]);
  armDirty = armDirty || col == DROP_COL;
}

// The arm is a row of squares from the pivot out to the hand. Drawing the same
// squares in black takes it away again, and then the two belt tiles it may have
// crossed are painted back.
static void drawArm(int a, bool on) {
  for (int i = 1; i <= 4; i++) {
    int r = i * 12;
    int16_t x = (int16_t)(PIVOT_X + (r * cosx(a)) / 100);
    int16_t y = (int16_t)(PIVOT_Y - (r * sinx(a)) / 100);
    int16_t size = (int16_t)(i == 4 ? 10 : 6);
    tft.fillRect((int16_t)(x - size / 2), (int16_t)(y - size / 2), size, size,
                 on ? (i == 4 ? ILI9341_WHITE : ARM) : ILI9341_BLACK);
    if (i == 4 && on && holding) {
      drawLump(x, y);
    }
  }
}

static void showArm() {
  if (drawnAngle >= 0) {
    drawArm(drawnAngle, false);
  }
  drawBeltA(PICK_COL);
  drawBeltB(DROP_COL);
  drawArm(angle, true);
  drawnAngle = angle;
  armDirty = false;
}

static void drawBase() {
  tft.fillRect(tileX(7), tileY(6), TILE, TILE, STEEL);
  tft.fillRect((int16_t)(tileX(7) + 3), (int16_t)(tileY(6) + 3), TILE - 6, TILE - 6, DARK);
}

static void drawLabels() {
  tft.text(8, 176, "MOVED", ILI9341_WHITE, ILI9341_BLACK, 2);
  tft.text(8, 198, "MISSED", ILI9341_WHITE, ILI9341_BLACK, 2);
  tft.text(8, 220, "SLIPS", ILI9341_WHITE, ILI9341_BLACK, 2);
  tft.text(136, 176, "SPD", ILI9341_WHITE, ILI9341_BLACK, 2);
  tft.text(136, 198, "GRAB", ILI9341_WHITE, ILI9341_BLACK, 2);
}

static void drawNumbers() {
  tft.number(8 + 7 * 12, 176, moved, ILI9341_YELLOW, ILI9341_BLACK, 2, 3);
  tft.number(8 + 7 * 12, 198, missed, ILI9341_RED, ILI9341_BLACK, 2, 3);
  tft.number(8 + 7 * 12, 220, slips, ILI9341_ORANGE, ILI9341_BLACK, 2, 3);
}

static void drawSettings() {
  tft.number(136 + 5 * 12, 176, speedStep, ILI9341_CYAN, ILI9341_BLACK, 2, 1);
  tft.number(136 + 5 * 12 - 12, 198, (long)dwell, ILI9341_CYAN, ILI9341_BLACK, 2, 4);
}

static void report() {
  Serial.print("moved ");
  Serial.print(moved);
  Serial.print(" missed ");
  Serial.print(missed);
  Serial.print(" slips ");
  Serial.println(slips);
}

static void setState(ArmState next) {
  state = next;
  stateSince = millis();
  digitalWrite(LED_PIN, next == GRAB || next == RELEASE ? HIGH : LOW);
}

// Turn the arm toward `target` for `elapsed` milliseconds; true once it is
// there. The knob's value is in units of 50 degrees a second.
static bool turnToward(int target, unsigned long elapsed) {
  if (angle != target) {
    turnCarry += (long)speedStep * 50L * (long)elapsed;   // degrees * 1000
    int whole = (int)(turnCarry / 1000L);
    turnCarry -= (long)whole * 1000L;
    if (angle < target) {
      angle = angle + whole > target ? target : angle + whole;
    } else {
      angle = angle - whole < target ? target : angle - whole;
    }
    servo.write(angle);
  }
  return angle == target;
}

static void updateBelts(unsigned long now) {
  if (now - lastArrive >= ARRIVE_MS) {
    lastArrive = now;
    if (!aLump[0]) {
      aLump[0] = true;
      drawBeltA(0);
    }
  }
  if (now - lastStep < STEP_MS) {
    return;
  }
  lastStep = now;
  // Belt A: the lump at the pickup tile falls off if it has waited too long.
  if (aLump[PICK_COL] && now - waitedSince >= WINDOW_MS && !(state == GRAB && holding)) {
    aLump[PICK_COL] = false;
    missed++;
    drawBeltA(PICK_COL);
    drawNumbers();
    report();
    tone(PIEZO_PIN, 200, 100);
  }
  for (int c = PICK_COL - 1; c >= 0; c--) {
    if (aLump[c] && !aLump[c + 1]) {
      aLump[c] = false;
      aLump[c + 1] = true;
      drawBeltA((uint8_t)c);
      drawBeltA((uint8_t)(c + 1));
      if (c + 1 == PICK_COL) {
        waitedSince = now;
      }
    }
  }
  // Belt B: lumps ride off the end and out of sight.
  uint8_t n = B_LAST - DROP_COL;
  if (bLump[n]) {
    bLump[n] = false;
    drawBeltB(B_LAST);
  }
  for (int c = n - 1; c >= 0; c--) {
    if (bLump[c]) {
      bLump[c] = false;
      bLump[c + 1] = true;
      drawBeltB((uint8_t)(DROP_COL + c));
      drawBeltB((uint8_t)(DROP_COL + c + 1));
    }
  }
}

void setup() {
  Serial.begin(9600);
  pinMode(LED_PIN, OUTPUT);
  servo.attach(SERVO_PIN);
  servo.write(angle);
  tft.begin();
  tft.rect(WORLD_X - 3, WORLD_Y - 3, 14 * TILE + 6, 10 * TILE + 6, 0x4a49, 2);
  for (uint8_t c = 0; c <= PICK_COL; c++) {
    drawBeltA(c);
  }
  for (uint8_t c = DROP_COL; c <= B_LAST; c++) {
    drawBeltB(c);
  }
  drawBase();
  drawLabels();
  drawNumbers();
  showArm();
  report();
  lastArrive = millis();
  setState(GO_PICK);
}

void loop() {
  unsigned long now = millis();
  updateBelts(now);

  if (now - lastServo < SERVO_MS) {
    return;
  }
  unsigned long elapsed = now - lastServo;
  lastServo = now;

  // The knobs. SPEED is 1..8 (times 50 degrees a second). GRAB is 100..900 ms.
  speedStep = 1 + (analogRead(SPEED_KNOB) * 8) / 1024;
  dwell = 100 + ((unsigned long)analogRead(GRAB_KNOB) * 800UL) / 1024UL;
  if ((int)speedStep != shownSpeed || (int)dwell / 10 != shownDwell) {
    shownSpeed = speedStep;
    shownDwell = (int)dwell / 10;
    drawSettings();
  }

  switch (state) {
    case GO_PICK:
      if (turnToward(PICK_ANGLE, elapsed) && aLump[PICK_COL]) {
        setState(GRAB);
      }
      break;
    case GRAB:
      if (now - stateSince >= dwell) {
        if (dwell >= CLAW_MS && aLump[PICK_COL]) {
          aLump[PICK_COL] = false;     // the lump comes off the belt into the claw
          holding = true;
          armDirty = true;
          setState(GO_DROP);
        } else {
          slips++;                     // the claw opened before it had closed
          drawNumbers();
          report();
          tone(PIEZO_PIN, 300, 80);
          setState(GO_PICK);
        }
      }
      break;
    case GO_DROP:
      if (turnToward(DROP_ANGLE, elapsed)) {
        setState(RELEASE);
      }
      break;
    case RELEASE:
      if (now - stateSince >= RELEASE_MS) {
        holding = false;
        armDirty = true;
        bLump[0] = true;
        moved++;
        drawBeltB(DROP_COL);
        drawNumbers();
        report();
        tone(PIEZO_PIN, 1000, 40);
        setState(GO_PICK);
      }
      break;
  }
  if (armDirty || angle != drawnAngle) {
    showArm();
  }
}