Bendy Bot Arduino code

bendybot.ino

// BendyBot
// By Tommy Goode

#include <Stepper.h>
#include <LiquidCrystal.h>

#define    STEPS 200
#define AIN1_PIN  11
#define AIN2_PIN  12
#define BIN1_PIN   9
#define BIN2_PIN   8
#define STBY_PIN  10

#define LCD_COLS  20
#define LCD_ROWS   2
#define LCD_RS  1
#define LCD_RW  2
#define LCD_EN  3
#define LCD_D4  4
#define LCD_D5  5
#define LCD_D6  6
#define LCD_D7  7

#define SWITCH_PIN      13
#define SWITCH_PAUSED HIGH
#define SWITCH_RUNNING LOW

// Set up Stepper object
Stepper stepper(STEPS, AIN1_PIN, AIN2_PIN, BIN1_PIN, BIN2_PIN);

// Set up the LCD object
LiquidCrystal lcd(LCD_RS, LCD_RW, LCD_EN, LCD_D4, LCD_D5, LCD_D6, LCD_D7);

double rpm = 20;
double rotationAngle = 130;
double numberOfSteps;

double bentTime = 0.25;
double straightTime = 0.15;

int switchState;
String pausedMessage  = "Paused!";
String runningMessage = "Bending!";
int count = 0;
double startTime = 0;
double pausedSeconds = 0;

void setup() {
  int serialWaitSeconds = 2;
  Serial.begin(9600);
  while (!Serial && serialWaitSeconds-- > 0) {
    ; // wait for serial port to connect. Needed for native USB port only
    delay(1000);
  }
  delay(1000);
  Serial.println("Begin BendyBot!");
 
  // Stepper
  pinMode(STBY_PIN, OUTPUT);
  digitalWrite(STBY_PIN, HIGH);
  stepper.setSpeed(rpm);
  numberOfSteps = rotationAngle/360.0 * STEPS;

  // Pause switch
  pinMode(SWITCH_PIN, INPUT_PULLUP);

  // LCD
  lcd.begin(LCD_COLS, LCD_ROWS);
  delay(1000);
  lcd.clear();
  lcd.blink();
  lcd.print("BendyBot!");

  lcd.print("  3..");
  delay(1000);
  lcd.print("2..");
  delay(1000);
  lcd.print("1..");
  delay(1000);

  startTime = millis();
}
 
void loop() {
  // Read pause switch first to decide if we should bend or not
  switchState = digitalRead(SWITCH_PIN);
  Serial.println(switchState);
  updateDisplay();
  if (switchState == SWITCH_PAUSED) {
    digitalWrite(STBY_PIN, LOW);
    delay(1000);
    pausedSeconds++;
    return;
  }
  
  digitalWrite(STBY_PIN, HIGH);
  step(numberOfSteps);
  updateDisplay();

  Serial.print("Sleeping ");
  Serial.print(bentTime);
  Serial.println(" seconds.");
  delay(bentTime * 1000);
  updateDisplay();

  step(-numberOfSteps);
  count++;
  updateDisplay();
  
  Serial.print("Sleeping ");
  Serial.print(straightTime);
  Serial.println(" seconds.");
  delay(straightTime * 1000);
}

void step(int numberOfSteps) {
  Serial.print("Stepping ");
  Serial.print(numberOfSteps);
  Serial.println(" steps");
  stepper.step(numberOfSteps);
  Serial.println("Sent step command.");
}

void updateDisplay() {
  int xPos = 0;
  lcd.clear();
  lcd.print("BendyBot!");

  if (switchState == SWITCH_PAUSED) {
    xPos = LCD_COLS - (pausedMessage.length());
    lcd.setCursor(xPos, 0);
    lcd.print(pausedMessage);
  }
  else {
    xPos = LCD_COLS - (runningMessage.length());
    lcd.setCursor(xPos, 0);
    lcd.print(runningMessage);
  }
  
  lcd.setCursor(0, 1);
  lcd.print("Count: ");
  lcd.print(count);

  double runtime = round((millis() - startTime)/1000) - pausedSeconds;

  // To right-align, get number of digits, add one for "s", then backtrack from width
  xPos = LCD_COLS - (numberOfDigits(runtime)+1);
  lcd.setCursor(xPos, 1);
  lcd.print(runtime);
  lcd.print("s");
}

int numberOfDigits(int number) {
  int numberOfDigits = 0;

  if (number < 0) {
    number = -number;
    numberOfDigits++;
  }
  else if (number == 0) {
    numberOfDigits = 1;
  }

  if (number != 0) {
    numberOfDigits += log10(number)+1;
  }

  return numberOfDigits;
}