Torque angle meter on an Arduino Uno

// Torque angle meter: an MPU6050 gyro tracks how far a wrench has turned a
// torque-to-yield bolt past snug, on a 16x2 I2C LCD with a beeper and lights.
//
//   A4, A5   I2C: the MPU6050 at 0x68 and the LCD backpack at 0x27
//   pin 2    ZERO button (to ground): press at snug torque to start counting
//   pin 3    TARGET button (to ground): steps the target 45, 60, 90, 120, 180
//   pin 5    green LED: at target
//   pin 6    red LED: overshot
//   pin 8    active buzzer: beeps faster in the last 30 degrees, a long tone
//            at target, fast beeps on an overshoot
//
// SAFETY: this is a teaching simulation. Never connect a project like this to
// a running car's safety systems (brakes, airbags, steering, ECU wiring,
// anything that can strand or hurt someone). Power it on the bench from USB,
// or from a fused 12 V battery through a proper automotive-rated regulator
// with load-dump and reverse protection. Always follow the manufacturer's
// torque and angle spec for the bolt; this tool does not replace it.
//
// # The job
//
// Torque-to-yield (TTY) bolts, such as most modern cylinder head bolts, are
// tightened in two stages: a snug torque on a torque wrench, then a set
// extra angle (for example 90 or 120 degrees, sometimes in two steps). The
// angle stretches the bolt into its yield range, which gives a far more even
// clamping force than torque alone, because torque is mostly lost to
// friction. Press ZERO once the bolt is at its snug torque, then turn. If you
// go past the angle, do not loosen and retighten: a TTY bolt is stretched for
// good, so replace it as the manual says.
//
// # Gyro scale and integration
//
// A gyroscope measures how fast it is turning, not where it is. At the
// +/-250 deg/s range (GYRO_CONFIG = 0) the MPU6050 gives 131 counts per
// degree per second, so a wrench turned at 45 deg/s reads 5895. The angle is
// the rate added up over time:
//
//   angle += rate * dt        degrees = (counts / 131) * seconds
//
// The loop reads the gyro every 5 ms (200 Hz, which is also what CONFIG and
// SMPLRT_DIV set the chip to: 1 kHz / (1 + 4)), measures the real dt with
// micros() so a slow pass does not lose angle, and uses the average of this
// rate and the last one (the trapezoid rule). Faster sampling follows a
// change of speed more closely: a turn that starts or stops between two
// samples is off by at most half a sample of motion, 0.1 degree at 45 deg/s.
// There is no floating point on this chip; the sum is kept in counts x
// microseconds, and every 131000 of those is one thousandth of a degree, so
// nothing is ever rounded away.
//
// # Bias and drift, and why you hold still at start-up
//
// A real MPU6050 does not read zero when it is still. Each axis has a
// zero-rate offset (the datasheet allows up to +/-20 deg/s) that changes with
// temperature. Integrated, an offset of only 0.5 deg/s is 30 degrees a
// minute: that is gyro drift. So for the first two seconds the sketch
// averages 400 readings with the tool at rest and subtracts that bias from
// every reading after. If it moves during those two seconds the average would
// include the motion and every later angle would drift, so it checks the
// spread and starts over. Set the wrench down and keep still while it says
// "Hold still". A small dead band (0.2 deg/s) hides what bias is left, and the
// count only ever goes up: backward swings of a ratchet do not turn the bolt,
// so they do not count. Work steadily; a minute-long pull drifts more than a
// five-second one. The simulated part has no bias or noise, so here the bias
// reads 0; on the bench it will not.
//
// # Mounting
//
// The gyro axis that counts is the one along the bolt. In Mokxi the canvas
// turns the board about its X axis when you drag it left and right (that is
// "roll"), so this sketch reads GYRO_XOUT and dragging right is tightening.
// On a real tool the board usually lies flat on the socket adapter with Z
// along the bolt: use GYRO_ZOUT (0x47) and flip TIGHTEN_SIGN if it counts
// the wrong way.

#include <Wire.h>
#include <LiquidCrystal_I2C.h>

const uint8_t MPU = 0x68;
const uint8_t GYRO_REG = 0x43;    // GYRO_XOUT_H; 0x47 is GYRO_ZOUT_H
const int TIGHTEN_SIGN = 1;

const int PIN_ZERO = 2;
const int PIN_TARGET = 3;
const int PIN_GREEN = 5;
const int PIN_RED = 6;
const int PIN_BUZZER = 8;

const long COUNTS_PER_DPS = 131;          // +/-250 deg/s range
const unsigned long SAMPLE_US = 5000;     // 200 Hz
const int CAL_SAMPLES = 400;              // two seconds
const int CAL_SPREAD = 2 * 131;           // more than 2 deg/s of wobble is "moved"
const int DEAD_BAND = 26;                 // about 0.2 deg/s

// Past the target by more than this is an overshoot. Set it from your spec;
// some give a tolerance such as plus or minus 5 degrees.
const long TOLERANCE_MDEG = 5000;
// The beeper starts this far before the target.
const long APPROACH_MDEG = 30000;

const int TARGETS[] = {45, 60, 90, 120, 180};
const int TARGET_COUNT = 5;

LiquidCrystal_I2C lcd(0x27, 16, 2);

enum Mode { CALIBRATING, WAIT_ZERO, TURNING, AT_TARGET, OVERSHOT };
Mode mode = CALIBRATING;

int targetIndex = 2;      // 90 degrees
long bias = 0;            // counts
long angleMdeg = 0;       // thousandths of a degree since ZERO
long remainder = 0;       // counts x microseconds not yet a whole millidegree
long lastRate = 0;        // the last sample's rate, for the trapezoid rule
unsigned long lastSample = 0;

int calCount = 0;
long calSum = 0;
int calMin = 0, calMax = 0;

bool zeroWas = HIGH, targetWas = HIGH;
unsigned long toneUntil = 0;   // the long tone at target
unsigned long alarmUntil = 0;  // the overshoot beeping
unsigned long printedAt = 0;

// What the LCD should show, and what it is showing.
char want[2][17];
char shown[2][17];
int drawAt = 0;

void writeRegister(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(MPU);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

uint8_t readRegister(uint8_t reg) {
  Wire.beginTransmission(MPU);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)1);
  return Wire.read();
}

// The signed rate on the bolt axis, in counts, high byte first.
int readGyro() {
  Wire.beginTransmission(MPU);
  Wire.write(GYRO_REG);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)2);
  int16_t raw = (int16_t)(Wire.read() << 8);
  raw |= Wire.read();
  return raw;
}

long targetMdeg() { return (long)TARGETS[targetIndex] * 1000; }

// "  87.3" style: tenths of a degree, right-aligned in six characters.
void formatTenths(char *out, long mdeg) {
  long tenths = (mdeg + 50) / 100;
  if (tenths > 9999) tenths = 9999;
  char tmp[7];
  int n = 0;
  tmp[n++] = '0' + tenths % 10;
  tmp[n++] = '.';
  long whole = tenths / 10;
  do {
    tmp[n++] = '0' + whole % 10;
    whole /= 10;
  } while (whole > 0);
  int pad = 6 - n;
  for (int i = 0; i < pad; i++) *out++ = ' ';
  while (n > 0) *out++ = tmp[--n];
  *out = 0;
}

void setLine(int row, const char *text) {
  int i = 0;
  for (; i < 16 && text[i]; i++) want[row][i] = text[i];
  for (; i < 16; i++) want[row][i] = ' ';
  want[row][16] = 0;
}

// Write at most one changed character per pass. Each LCD character is several
// I2C transfers, and a whole redraw at once would hold the gyro up for tens of
// milliseconds; one character keeps every sample close to on time.
void drawOne() {
  for (int k = 0; k < 32; k++) {
    int at = (drawAt + k) % 32;
    int row = at / 16, col = at % 16;
    if (want[row][col] != shown[row][col]) {
      lcd.setCursor(col, row);
      lcd.write(want[row][col]);
      shown[row][col] = want[row][col];
      drawAt = (at + 1) % 32;
      return;
    }
  }
}

void startCalibration() {
  mode = CALIBRATING;
  calCount = 0;
  calSum = 0;
  setLine(0, "Calibrating gyro");
  setLine(1, "Hold still...");
}

void zero() {
  angleMdeg = 0;
  remainder = 0;
  toneUntil = 0;
  alarmUntil = 0;
  mode = TURNING;
  Serial.print("ZERO  target ");
  Serial.println(TARGETS[targetIndex]);
}

// Two seconds of readings at rest: their average is the bias.
void calibrate(int raw) {
  if (calCount == 0) {
    calMin = raw;
    calMax = raw;
  }
  if (raw < calMin) calMin = raw;
  if (raw > calMax) calMax = raw;
  calSum += raw;
  calCount++;
  if (calMax - calMin > CAL_SPREAD) {
    Serial.println("moved during calibration, starting over");
    startCalibration();
    return;
  }
  if (calCount == CAL_SAMPLES) {
    // Rounded to the nearest count: half a count is 0.004 deg/s.
    bias = (calSum >= 0 ? calSum + CAL_SAMPLES / 2 : calSum - CAL_SAMPLES / 2) / CAL_SAMPLES;
    Serial.print("bias ");
    Serial.print(bias);
    Serial.println(" counts; snug the bolt, then press ZERO");
    mode = WAIT_ZERO;
  }
}

// The tightening rate in counts, less the bias. Backward and anything inside
// the dead band count as zero.
long tighteningRate(int raw) {
  long rate = ((long)raw - bias) * TIGHTEN_SIGN;
  return rate < DEAD_BAND ? 0 : rate;
}

// Add one sample to the angle, by the trapezoid rule: the average of this
// rate and the last one, times the time between them. That follows a rate
// that is changing better than this rate times the whole interval would.
void integrate(int raw, unsigned long dtUs) {
  long rate = tighteningRate(raw);
  long average = (rate + lastRate) / 2;
  lastRate = rate;
  if (dtUs > 50000) dtUs = 50000;   // a stalled loop never adds a jump
  remainder += average * (long)dtUs;
  const long PER_MDEG = COUNTS_PER_DPS * 1000;   // 131000 count-us per millidegree
  angleMdeg += remainder / PER_MDEG;
  remainder %= PER_MDEG;
}

void updateMode(unsigned long now) {
  if (mode != TURNING && mode != AT_TARGET) return;
  long target = targetMdeg();
  if (angleMdeg > target + TOLERANCE_MDEG) {
    mode = OVERSHOT;
    alarmUntil = now + 1500;
    toneUntil = 0;
    Serial.println("OVERSHOT: do not back off and retighten; replace the bolt per spec");
  } else if (mode == TURNING && angleMdeg >= target) {
    mode = AT_TARGET;
    toneUntil = now + 1000;
    Serial.println("AT TARGET: stop turning");
  }
}

void outputs(unsigned long now) {
  digitalWrite(PIN_GREEN, mode == AT_TARGET ? HIGH : LOW);
  digitalWrite(PIN_RED, mode == OVERSHOT ? HIGH : LOW);
  bool beep = false;
  if (mode == OVERSHOT) {
    beep = now < alarmUntil && (now / 150) % 2 == 0;
  } else if (mode == AT_TARGET) {
    beep = now < toneUntil;
  } else if (mode == TURNING) {
    long left = targetMdeg() - angleMdeg;
    if (left <= APPROACH_MDEG) {
      // 420 ms between beeps 30 degrees out, 60 ms right at the target.
      unsigned long period = 60 + (unsigned long)(left / 1000) * 12;
      beep = now % period < 30;
    }
  }
  digitalWrite(PIN_BUZZER, beep ? HIGH : LOW);
}

void compose() {
  if (mode == CALIBRATING) return;
  char line[17];
  char num[7];
  // Row 0: "Angle   87.3 deg"
  formatTenths(num, angleMdeg);
  int n = 0;
  const char *head = "Angle ";
  while (*head) line[n++] = *head++;
  for (int i = 0; num[i]; i++) line[n++] = num[i];
  line[n++] = ' ';
  line[n++] = 'd';
  line[n++] = 'e';
  line[n++] = 'g';
  line[n] = 0;
  setLine(0, line);
  // Row 1: "Tgt 120 TURN", eight characters for the word
  int t = TARGETS[targetIndex];
  n = 0;
  line[n++] = 'T';
  line[n++] = 'g';
  line[n++] = 't';
  line[n++] = ' ';
  line[n++] = t >= 100 ? '0' + t / 100 : ' ';
  line[n++] = '0' + (t / 10) % 10;
  line[n++] = '0' + t % 10;
  line[n++] = ' ';
  const char *word = "TURN";
  if (mode == WAIT_ZERO) word = "ZERO?";
  if (mode == AT_TARGET) word = "AT TGT";
  if (mode == OVERSHOT) word = "OVERSHOT";
  while (*word && n < 16) line[n++] = *word++;
  line[n] = 0;
  setLine(1, line);
}

const char *modeName() {
  switch (mode) {
    case CALIBRATING: return "CAL";
    case WAIT_ZERO: return "WAIT";
    case TURNING: return "TURN";
    case AT_TARGET: return "AT TARGET";
    default: return "OVERSHOT";
  }
}

void buttons() {
  bool z = digitalRead(PIN_ZERO);
  bool t = digitalRead(PIN_TARGET);
  if (mode != CALIBRATING) {
    if (zeroWas == HIGH && z == LOW) zero();
    if (targetWas == HIGH && t == LOW) {
      targetIndex = (targetIndex + 1) % TARGET_COUNT;
      Serial.print("target ");
      Serial.println(TARGETS[targetIndex]);
      if (mode == AT_TARGET && angleMdeg < targetMdeg()) mode = TURNING;
    }
  }
  zeroWas = z;
  targetWas = t;
}

void setup() {
  pinMode(PIN_ZERO, INPUT_PULLUP);
  pinMode(PIN_TARGET, INPUT_PULLUP);
  pinMode(PIN_GREEN, OUTPUT);
  pinMode(PIN_RED, OUTPUT);
  pinMode(PIN_BUZZER, OUTPUT);
  Serial.begin(115200);
  Serial.println("Torque angle meter");

  lcd.init();
  lcd.backlight();
  for (int r = 0; r < 2; r++) {
    for (int c = 0; c < 16; c++) shown[r][c] = ' ';
    shown[r][16] = 0;
  }

  Wire.beginTransmission(MPU);
  if (Wire.endTransmission() != 0) {
    Serial.println("No MPU6050 at 0x68. Check SDA, SCL and power.");
    lcd.print("No gyro found");
    while (true) delay(100);
  }
  Serial.print("WHO_AM_I 0x");
  Serial.println(readRegister(0x75), HEX);
  writeRegister(0x6B, 0x01);   // PWR_MGMT_1: awake, clocked from the X gyro's PLL
  writeRegister(0x1A, 0x03);   // CONFIG: 44 Hz low-pass, 1 kHz internal rate
  writeRegister(0x19, 4);      // SMPLRT_DIV: 1 kHz / (1 + 4) = 200 Hz
  writeRegister(0x1B, 0x00);   // GYRO_CONFIG: +/-250 deg/s, 131 counts per deg/s

  startCalibration();
  // Draw the "Hold still" screen before the first sample.
  for (int i = 0; i < 32; i++) drawOne();
  lastSample = micros();
}

void loop() {
  unsigned long nowUs = micros();
  unsigned long dt = nowUs - lastSample;
  if (dt < SAMPLE_US) return;
  lastSample = nowUs;

  int raw = readGyro();
  unsigned long now = millis();
  if (mode == CALIBRATING) {
    calibrate(raw);
  } else {
    if (mode == WAIT_ZERO) lastRate = tighteningRate(raw);
    else integrate(raw, dt);
    updateMode(now);
  }
  buttons();
  outputs(now);

  if (now - printedAt >= 250) {
    printedAt = now;
    compose();
    if (mode != CALIBRATING) {
      char num[7];
      formatTenths(num, angleMdeg);
      const char *p = num;
      while (*p == ' ') p++;
      Serial.print("angle ");
      Serial.print(p);
      Serial.print(" / ");
      Serial.print(TARGETS[targetIndex]);
      Serial.print("  ");
      Serial.println(modeName());
    }
  }
  drawOne();
}