CastHelper.ino

#include <SPI.h>
#include <LiquidCrystal.h>
#include <Servo.h>
#include "HX711.h"

#define PIN_L_EXTRUDE 5
#define PIN_R_EXTRUDE 2
#define PIN_TARE 4
#define PIN_RESET 3
#define PIN_DIAL A0

#define MAX_EXTRUDE 30 // gram

#define SERVO_CLOSE_ANGLE 135
#define SERVO_OPEN_ANGLE 45

#define OVERSHOOT 15

LiquidCrystal lcd(10);
HX711 scale(6, 7);
Servo servoLeft;
Servo servoRight;

enum RuningState { WAITING, EXTRUDE_LEFT, EXTRUDE_RIGHT};

enum RuningState runingState = WAITING;
long goalWeight = 0;

unsigned long lastTime;
unsigned long currModeTime;

void setup() {
  lcd.begin(16, 2);

  pinMode(PIN_L_EXTRUDE, INPUT_PULLUP);
  pinMode(PIN_R_EXTRUDE, INPUT_PULLUP);
  pinMode(PIN_TARE, INPUT_PULLUP);
  pinMode(PIN_RESET, INPUT_PULLUP);
  pinMode(PIN_DIAL, INPUT);

  servoLeft.attach(8);
  servoRight.attach(9);

  scale.set_scale(46.2f);
  scale.tare();

  lastTime = millis();
}

void loop() {
  unsigned long interval;
  {
    unsigned long t = millis();
    interval = t - lastTime;
    lastTime = t;
  }
  currModeTime += interval;

  long currWeight = scale.get_units(2);
  lcd.setCursor(4, 0);
  printLcd(currWeight, 3, 2);

  if (!digitalRead(PIN_RESET)) {
    runingState = WAITING;
    currModeTime = 0;
    lcd.setCursor(0, 0);
    lcd.print("     RESET     ");
    delay(500);
    return;
  }

  {
    int n = (millis() / 500) % 3;
    lcd.setCursor(0, 0);
    if (runingState == EXTRUDE_LEFT) {
      for (int i = 0; i < 3; i++) {
        lcd.print(i <= n ? "." : " ");
      }
    } else {
      lcd.print("   ");
    }
  }
  {
    int n = (millis() / 500) % 3;
    lcd.setCursor(12, 0);
    if (runingState == EXTRUDE_RIGHT) {
      for (int i = 0; i < 3; i++) {
        lcd.print(i <= n ? "." : " ");
      }
    } else {
      lcd.print("   ");
    }
  }

  int servoLeftValue = SERVO_CLOSE_ANGLE;
  int servoRightValue = SERVO_CLOSE_ANGLE;
  if (runingState == EXTRUDE_LEFT) {
    servoLeftValue = SERVO_OPEN_ANGLE;
    if (currWeight >= goalWeight) {
      runingState = WAITING;
      currModeTime = 0;
    }
  } else if (runingState == EXTRUDE_RIGHT) {
    servoRightValue = SERVO_OPEN_ANGLE;
    if (currWeight >= goalWeight) {
      runingState = WAITING;
      currModeTime = 0;
    }
  } else {
    servoLeftValue = SERVO_CLOSE_ANGLE + (currModeTime < 2000 ? OVERSHOOT : 0);
    servoRightValue = SERVO_CLOSE_ANGLE + (currModeTime < 2000 ? OVERSHOOT : 0);
    if (!digitalRead(PIN_TARE)) {
      scale.tare();
    }
    long w = (MAX_EXTRUDE * 100 * (long)analogRead(PIN_DIAL)) / 1023;
    lcd.setCursor(4, 1);
    printLcd(w, 3, 2);
    goalWeight = currWeight + w;
    if (!digitalRead(PIN_TARE)) {
      scale.tare();
    } else if (!digitalRead(PIN_L_EXTRUDE)) {
      runingState = EXTRUDE_LEFT;
      currModeTime = 0;
    } else if (!digitalRead(PIN_R_EXTRUDE)) {
      runingState = EXTRUDE_RIGHT;
      currModeTime = 0;
    }
  }
  servoLeft.write(servoLeftValue);
  servoRight.write(servoRightValue);

  delay(100);
}

void printLcd(long value, int integralPart, int decimalPart) {
  long ti = pow(10, integralPart);
  long td = pow(10, decimalPart);
  bool negative = (value < 0);
  {
    ti -= 1;
    if (negative) {
      ti = ti / 10;
    }
    long vi = value / td;
    long avi = abs(vi);
    ti /= 10;
    while (ti >= avi && ti > 0) {
      lcd.print(" ");
      ti /= 10;
    }
    if (negative) {
      lcd.print("-");
    }
    lcd.print(avi);
  }
  if (td > 1) {
    lcd.print(".");
    long avd = abs(value % td);
    td -= 1;
    td /= 10;
    while (td >= avd && td > 0) {
      lcd.print("0");
      td /= 10;
    }
    lcd.print(avd);
  }
}