Useless Box – The Code

// Written by Dean Edis (IMadeAThing.co.uk)
// The code is provided as example code only.
#include <Servo.h>

enum {
  STATE_IDLE,           // Nothing happening. Switch is off.
  STATE_SWITCHED,       // User just flipped switch.
  STATE_TURNOFF_SIMPLE, // Turn off - Simple, quick, switch off.
};

Servo lidServo, fingerServo;
float fingerPosition = 0.0f; // 0.0 (inside) to 1.0 (pushing switch)
float lidPosition = 0.0f; // 0.0 (closed) to 1.0 (open)
float servoTicksPerLoop = 0.0004f;

int switchPin = 2;

int currentState = STATE_IDLE;

void initServo(Servo&amp; s, int pin) {
  s.attach(pin);
}

bool isSwitchOn() {
  return digitalRead(switchPin) == HIGH;
}

void setState(int newState) {
  currentState = newState;
}

void moveFingerTo(float&amp; position) {
  if (position < 0)
    position = 0;
  else if (position > 1.0)
    position = 1.0;

  const int fingerMin = 67;
  const int fingerMax = 13;
  fingerServo.write(map(position * 1024, 0, 1024, fingerMin, fingerMax));
}

// Move finger from current position back into rest position.
void retractFinger() {
  fingerPosition -= servoTicksPerLoop;
  moveFingerTo(fingerPosition);
}

// Move finger from current position to maximum extension.
void extendFinger() {
  fingerPosition += servoTicksPerLoop;
  moveFingerTo(fingerPosition);
}

void moveLidTo(float&amp; position) {
  if (position < 0)
    position = 0;
  else if (position > 1.0)
    position = 1.0;

  const int lidMin = 133;
  const int lidMax = 110;
  lidServo.write(map(position * 1024, 0, 1024, lidMin, lidMax));
}

// Move lid from current position back into rest position.
void closeLid() {
  lidPosition -= servoTicksPerLoop;
  moveLidTo(lidPosition);
}

// Move lid from current position to open position.
void openLid() {
  lidPosition += servoTicksPerLoop;
  moveLidTo(lidPosition);
}

// Abort any current action and move servos to rest positions.
void goToSleep() {
  // Retract finger first...
  if (fingerPosition > 0) {
    retractFinger();
    return;
  }
  
  // ... then close lid.
  closeLid();
}

// Move finger and lid to turn off switch.
void turnOff() {
  // Open lid fully.
  if (lidPosition < 1.0) {
    openLid();
    return;
  }
  
  // ...then move finger to hit the switch.
  extendFinger();
}

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

  // Init servos.
  float startPos = 0.0;
  initServo(lidServo, 9);
  moveLidTo(startPos);
  initServo(fingerServo, 10);
  moveFingerTo(startPos);
  
  // Init switch.
  pinMode(switchPin, INPUT_PULLUP);
  
  delay(2000);
}

void loop() {
  switch (currentState)
  {
    case STATE_IDLE:
      if (isSwitchOn())
        setState(STATE_SWITCHED);
      else
        goToSleep();
      break;
      
    case STATE_SWITCHED:
      setState(STATE_TURNOFF_SIMPLE);
      break;
      
    case STATE_TURNOFF_SIMPLE:
      if (isSwitchOn())
        turnOff();
      else
        setState(STATE_IDLE);
      break;
  }
}