Leopold the Lizard Logic

leopold.ino

/**
 * Name: leopold.ino
 * Desc: Logic for running Leopold the Lizard
 * Auth: Jessica Ebert, Sam Pottinger, DJ Sutton
**/

#include <Servo.h>

#define NECK_SERVO_PIN 5
#define MAIN_LOOP_DELAY 15
#define LEFT_EYE_LIGHT_SENSOR_PIN 1
#define RIGHT_EYE_LIGHT_SENSOR_PIN 0
#define LIGHT_SENSOR_DIFF_TOLLERANCE 30
#define DANCE_DIFF_THRESHOLD 200
#define STOP_SINGING_THRESHOLD 565
#define CORRECTION_STEP_SIZE 1
#define MAX_HEAD_ANGLE 180
#define MIN_HEAD_ANGLE 0
#define LEFT_ADJ_TICKS 0
#define RIGHT_ADJ_TICKS 0
#define LAZY_ITERATIONS 5
#define DANCE_THRESHOLD 580
#define NUM_DANCE_SERVOS 4
#define FRONT_LEFT_SERVO_INDEX 0
#define FRONT_RIGHT_SERVO_INDEX 1
#define BACK_LEFT_SERVO_INDEX 2
#define BACK_RIGHT_SERVO_INDEX 3
#define BACK_LEFT_SERVO_INDEX 2
#define BACK_RIGHT_SERVO_INDEX 3
#define FRONT_LEFT_SERVO_PORT 6
#define FRONT_RIGHT_SERVO_PORT 9
#define BACK_LEFT_SERVO_PORT 10
#define BACK_RIGHT_SERVO_PORT 11
#define MIN_DANCE_ANGLE 80
#define MAX_DANCE_ANGLE 160
#define DANCE_FORWARD 6
#define DANCE_BACKWARD -6
#define SINGING_PIN 13
#define STARTING_DANCE_POS 100

Servo neckServo; // Servo to control left right neck motion
Servo danceServos[NUM_DANCE_SERVOS];
int currentPos = 0;
int numItrSameDir = 0;
int dancePos;
int danceDir;
boolean dancing;

void setup()
{
  // Assign neck servo
  neckServo.attach(NECK_SERVO_PIN);
  
  // Assign servos for wiggling
  danceServos[FRONT_LEFT_SERVO_INDEX].attach(FRONT_LEFT_SERVO_PORT);
  danceServos[FRONT_RIGHT_SERVO_INDEX].attach(FRONT_RIGHT_SERVO_PORT);
  danceServos[BACK_LEFT_SERVO_INDEX].attach(BACK_LEFT_SERVO_PORT);
  danceServos[BACK_RIGHT_SERVO_INDEX].attach(BACK_RIGHT_SERVO_PORT);

  // Set initial state
  dancePos = STARTING_DANCE_POS;
  danceDir = DANCE_FORWARD;
  dancing = false;

  // Setup greeting card pin
  pinMode(SINGING_PIN, OUTPUT);
  digitalWrite(SINGING_PIN, LOW);
}

void loop()
{
  int leftEyeLightVal;
  int rightEyeLightVal;
  int deltaPos;
  int lsDiff;
  int i;
  
  // Slow down crazy fast iteration
  delay(MAIN_LOOP_DELAY);
  
  // Read light sensor values
  leftEyeLightVal = analogRead(LEFT_EYE_LIGHT_SENSOR_PIN) + LEFT_ADJ_TICKS;
  rightEyeLightVal = analogRead(RIGHT_EYE_LIGHT_SENSOR_PIN) + RIGHT_ADJ_TICKS;
  
  // Determine difference in light sensor vals
  lsDiff = leftEyeLightVal - rightEyeLightVal;
  
  // Determine in which direction the servo should move
  if(abs(lsDiff) < DANCE_DIFF_THRESHOLD && leftEyeLightVal > DANCE_THRESHOLD)
    dancing = true;
  else if(abs(lsDiff) < DANCE_DIFF_THRESHOLD && leftEyeLightVal < STOP_SINGING_THRESHOLD)
    dancing = false;
  
  // Update dance servos if dancing
  if(dancing)
  {
    digitalWrite(SINGING_PIN, HIGH); 
    if(dancePos > MAX_DANCE_ANGLE)
      danceDir = DANCE_BACKWARD;
    else if(dancePos < MIN_DANCE_ANGLE)
      danceDir = DANCE_FORWARD;
    
    dancePos += danceDir;
    
    for(i=0; i<NUM_DANCE_SERVOS; i++)
      danceServos[i].write(dancePos);
  }
  else
    digitalWrite(SINGING_PIN, LOW); // Set the pin for the greeting card low
   
  // Calculate head position
  if(abs(lsDiff) > LIGHT_SENSOR_DIFF_TOLLERANCE)
  {
    if(lsDiff < 0)
      deltaPos = -CORRECTION_STEP_SIZE;
    else
      deltaPos = CORRECTION_STEP_SIZE;
  }
  
  // Update head position
  currentPos += deltaPos;
  if(currentPos > MAX_HEAD_ANGLE)
    currentPos = MAX_HEAD_ANGLE;
  else if(currentPos < MIN_HEAD_ANGLE)
    currentPos = MIN_HEAD_ANGLE;
  neckServo.write(currentPos);
  
}