Code for Arduino Nano based toy duplicator prop

dupli_jeu.ino

#include <FastLED.h>
#include <Servo.h>

// Pin assignments
#define BUTTON_PIN 2
#define BUTTON_ENABLE 3
#define LED_PIN 4
#define SERVO_PIN 9
#define BELT_PIN 8

// 180 degree servo globals
Servo trapdoorServo;
bool trapdoorIsBusy = false;
int targetServoPos = 140;
unsigned long lastTrapdoorChange = 0;
#define closedPosition 0
#define openPosition 180
#define MAX_STEPS 50

// Continuous rotation servo globals
Servo beltServo;
int beltZero;
bool beltIsBusy = false;
#define BELT_SPEED 5

// Fastled Globals
#define NUM_LEDS 12
#define COLOR_ORDER GRB
#define LED_TYPE WS2812B
CRGB leds[NUM_LEDS];
#define BRIGHTNESS         255
#define FRAMES_PER_SECOND  120

// State machine
byte state = 0; 
byte nextState = 0;
unsigned long nextChangeTimestamp = 0;

void setup() {
  Serial.begin(9600);
  pinMode(BUTTON_PIN, INPUT);
  pinMode(BUTTON_ENABLE, OUTPUT);
  digitalWrite(BUTTON_ENABLE, 1);

  // LEDs config
  FastLED.addLeds<LED_TYPE,LED_PIN,COLOR_ORDER>(leds, NUM_LEDS).setCorrection(TypicalLEDStrip);
  FastLED.setBrightness(BRIGHTNESS);
  FastLED.show();

  // Set trapdoor servo position to closed
  close_door();

}

void close_door() {
  trapdoorIsBusy = true;
  trapdoorServo.attach(SERVO_PIN);
  targetServoPos = closedPosition;
}

void open_door() {
  trapdoorIsBusy = true;
  trapdoorServo.attach(SERVO_PIN);
  targetServoPos = openPosition;
}

void beltForward() {
  beltServo.attach(BELT_PIN);
  beltZero = beltServo.read();
  beltServo.write(beltZero + BELT_SPEED);
  beltIsBusy = true;
}

void beltStop() {
  beltServo.detach();
  beltIsBusy = false;
}

void adjustTrapdoor() {
  if (state == 0 && !trapdoorIsBusy) {
    return;
  }
  int curr = trapdoorServo.read();
  if (millis() - lastTrapdoorChange > 0 && curr != targetServoPos) {
    // map(value, fromLow, fromHigh, toLow, toHigh)
    int diff = abs(curr - targetServoPos);
    int stepSize = map(abs(curr - targetServoPos), 0, 120, 1, MAX_STEPS);
    Serial.print("diff: ");
    Serial.print(diff);
    Serial.print("\t");
    Serial.print("stepSize: ");
    Serial.print(stepSize);
    Serial.print("\t");
    if (curr > targetServoPos) {
      curr -= stepSize;
      trapdoorServo.write(curr);
      lastTrapdoorChange = millis();
    } else if (curr < targetServoPos) {
      curr += stepSize;
      trapdoorServo.write(curr);
      lastTrapdoorChange = millis();
    }
  }
  
  if (curr == targetServoPos && trapdoorIsBusy) {
    trapdoorIsBusy = false;
    trapdoorServo.detach();
  }
  
}

void checkForStateChange() {
  if (state != nextState) {
    if (nextChangeTimestamp < millis()) {
      if (!trapdoorIsBusy) {
        state = nextState;
        nextChangeTimestamp = 0;
        digitalWrite(BUTTON_ENABLE, 1);
      }
    } else {
      digitalWrite(BUTTON_ENABLE, 0);
    }
  }
}

void handleLights() {
  // send the 'leds' array out to the actual LED strip
  fadeToBlackBy( leds, NUM_LEDS, 50);
  FastLED.show();  
  // insert a delay to keep the framerate modest
//  FastLED.delay(1000/FRAMES_PER_SECOND); 
}

void logValues() {
  Serial.print("trapdoorIsBusy ");
  Serial.print(trapdoorIsBusy);
  Serial.print("\tstate ");
  Serial.print(state);
  Serial.print("\tnextState ");
  Serial.print(nextState);
  Serial.print("\ttrapTar ");
  Serial.print(targetServoPos);
  Serial.print("\ttrapCur ");
  Serial.print(trapdoorServo.read());
  Serial.print("\ttrapEN ");
  Serial.print(trapdoorServo.attached());
  
  if (nextChangeTimestamp > millis()) {
    Serial.print("\tnext ");
    Serial.print(nextChangeTimestamp - millis());
  }
  Serial.println();
}


void loop() {
  checkForStateChange();
  
  switch (state) {
    case 0: 
      // machine is idle
      if (digitalRead(BUTTON_PIN)) {
        // open the trap door, and set next state to trap door open
        nextState = 1;
        trapdoorIsBusy = true;
        open_door();
      }
      break;
    case 1:
      // trap door is open
      if (digitalRead(BUTTON_PIN) == 0) {
        // close the trap door, and set the next state to trigger lights
        nextState = 2;
        close_door();
      }
      break;
    case 2:
      // trap door has closed, lights should start
      // On first run, set the duration for the current step
      if (nextChangeTimestamp == 0) {
        nextChangeTimestamp = millis() + 4000;
      }
      nextState = 3;
      juggle();
      break;
    case 3:
      // fade LEDs out over two seconds
      // On first run, set the duration for the current step
      if (nextChangeTimestamp == 0) {
        nextChangeTimestamp = millis() + 1000;
      }
      nextState = 4;
      break;
    case 4:
      if (nextChangeTimestamp == 0) {
        beltForward();
        nextChangeTimestamp = millis() + 16000;
      }
      juggle();
      nextState = 5;
      break;
    case 5:
      beltStop();
      if (nextChangeTimestamp == 0) {
        nextChangeTimestamp = millis() + 2000;
      }
      nextState = 0;
      break;
  }
  adjustTrapdoor();
  handleLights();
  logValues();
}

void juggle() {
  // eight colored dots, weaving in and out of sync with each other
  fadeToBlackBy( leds, NUM_LEDS, 20);
  byte dothue = 0;
  for( int i = 0; i < 8; i++) {
    leds[beatsin16( i+7, 0, NUM_LEDS-1 )] |= CHSV(dothue, 200, 255);
    dothue += 32;
  }
}