DIY Gigapan using servos

gigapan_lcd_KEYS_2316.ino

#include <Servo.h>
#include <LiquidCrystal.h>


#define PIN_SERVO_SHUTTER   6
#define PIN_SERVO_TILT  10
#define PIN_SERVO_PAN   9
#define PIN_BEEP   8

#define SHUTTER_START_POINT   35
#define SHUTTER_SHOOT_POINT   50

Servo servoPan;
Servo servoTilt;
Servo servoShutter;


int currentPan = 90;
int currentTilt = 90;

int tiltStart = 120;
int tiltEnd = 60;
int tiltMoveStep = 5;
int tiltStep = 20;

int panStart = 120;
int panEnd = 60;
int panMoveStep = 5;
int panStep = 20;

char keystate = 0;
long menuPaint = 0;
long debounceTimer = 0;

String modeDispLine = "Main Menu";

enum keys { KEY_ENTER, KEY_UP, KEY_DOWN, KEY_LEFT, KEY_RIGHT, KEY_NONE, KEY_RESET };

char *mainMenuOptions[] = { 
  "mMove Camera", "tShoot One", "pAuto Panorama", "iStop Motion", "cConfigure Ends", "aConfigure Step" };

int numberOfMainMenuOptions = sizeof(mainMenuOptions) / sizeof(char *);
int currentMainMenuOption = 0;

LiquidCrystal lcd(12, 11, 5, 4, 3, 2);


void setup() {
  lcd.begin(16, 2);
  lcd.print("PanoBot Booting");
  
  Serial.begin(9600);
  Serial.println("PanoBot Booting...");

  // attach the servos one at a time
  Serial.println("Attaching Servo Shutter");
  servoShutter.attach(PIN_SERVO_SHUTTER);
  servoShutter.write(SHUTTER_START_POINT);
  delay(300);
  //
  Serial.println("Attaching Servo Tilt");
  servoTilt.attach(PIN_SERVO_TILT);
  servoTilt.write(currentTilt);
  delay(300);
  //
  Serial.println("Attaching Servo Pan");
  servoPan.attach(PIN_SERVO_PAN);
  servoPan.write(currentPan);
  delay(500);
  
  pinMode(PIN_BEEP, OUTPUT);
  digitalWrite(PIN_BEEP, HIGH);
  delay(10);
  digitalWrite(PIN_BEEP, LOW);
  
  
  Serial.println("PanoBot Ready:");
}

void loop() {


  char *mainMenuOption = mainMenuOptions[currentMainMenuOption];

  // up/down/select <option_name>
if (millis() > menuPaint ) {
  modeDispLine = mainMenuOption+1;
  updateScreen();
  menuPaint = millis() + 5000;
}
  int key = getKey();

  if (key == KEY_ENTER) {
    switch (mainMenuOption[0]) {
    case 'm': // move camera manually
//      Serial.println("Moving Camera:");
      optionMenuMoveCamera();
      break;
    case 't': // shoot a picture
//      Serial.println("PanoBot T:");
      modeDispLine = "Shooting";
      updateScreen();
      snap();
      menuPaint = millis();
      break;
    case 'p': // pano
//      Serial.println("PanoBot P:");
      Serial.println(tiltStart);
      Serial.println(tiltEnd);
      Serial.println(tiltStep);
      Serial.println(panStart);
      Serial.println(panEnd);
      Serial.println(panStep);
      gogopan();
      menuPaint = millis();
      break;
    case 'i': // stop motion
      Serial.println("UNDER CONSTRUCTION");
      break;
    case 'c': // configure Ends
//      Serial.println("PanoBot C:");
      optionMenuConfigure();
      menuPaint = millis();
      break;
    case 'a': // configure Step
//      Serial.println("PanoBot A:");
      optionMenuConfigureStep();
      menuPaint = millis();
      break;
    default:
      break;
    }
  } 
  else {
    if (key == KEY_UP) {
      currentMainMenuOption--;
      if (currentMainMenuOption < 0){
        currentMainMenuOption = numberOfMainMenuOptions-1;
      }
          mainMenuOption = mainMenuOptions[currentMainMenuOption];
      modeDispLine = mainMenuOption+1;
      updateScreen();
      menuPaint = millis() + 5000;    }
    else if (key == KEY_DOWN) {
      currentMainMenuOption++;
      if (currentMainMenuOption >= numberOfMainMenuOptions){
        currentMainMenuOption = 0;
      }
          mainMenuOption = mainMenuOptions[currentMainMenuOption];
      modeDispLine = mainMenuOption+1;
      updateScreen();
      menuPaint = millis() + 5000;
    }
  }


}


int getKey() {
int key;

  if ((debounceTimer < millis()) && (analogRead(A0) > 400)) {
    delay(50);
    if (analogRead(A0) > 400) {
    key = KEY_RESET;
    debounceTimer = millis() + 500;
    return key;
    }
  }
  if ((debounceTimer < millis()) && (analogRead(A1) > 400)) {
    delay(50);
    if (analogRead(A1) > 400) {
    key = KEY_LEFT;
    debounceTimer = millis() + 500;
    return key;
    }
  }
  if ((debounceTimer < millis()) && (analogRead(A2) > 400)) {

    delay(50);
    if (analogRead(A2) > 400) {
    key = KEY_DOWN;
    debounceTimer = millis() + 500;
    return key;
    }
  }
  if ((debounceTimer < millis()) && (analogRead(A3) > 400)) {

    delay(50);
    if (analogRead(A3) > 400) {
    key = KEY_UP;
    debounceTimer = millis() + 500;
    return key;
    }
  }
  if ((debounceTimer < millis()) && (analogRead(A4) > 400)) {

    delay(50);
    if (analogRead(A4) > 400) {
    key = KEY_RIGHT;
    debounceTimer = millis() + 500;
    return key;
    }
  }
  if ((debounceTimer < millis()) && (analogRead(A5) > 400)) {

    delay(50);
    if (analogRead(A5) > 400) {
    key = KEY_ENTER;
    debounceTimer = millis() + 500;
    return key;
    }
  }



  if (Serial.available() > 0) {
    char ch = Serial.read();
//    Serial.println(ch, HEX);
    if(ch == 0x72)
    {
      key = KEY_RESET;
//      Serial.println("R");
    }
    if(ch == 0xD)
    {
//      Serial.println("Enter");
      key = KEY_ENTER;
      }
    else if(ch == 0x1B)
    {
      keystate = 0x1B;
      key = KEY_NONE;
    }
    else if(ch == 0x5B)
    {
      if(keystate == 0x1B) {
        keystate = 0x5B;
        key = KEY_NONE;
      } else {
        keystate = 0;
        key = KEY_NONE;
      }
    }
    else if(keystate == 0x5B)
    {
      if(ch == 0x43) // Right arrow key
      {
//        Serial.println("Right");
        key = KEY_RIGHT;
      }
      else if(ch == 0x44) // Left arrow key
      {
//        Serial.println("Left");
        key = KEY_LEFT;
      }
      else if(ch == 0x42) // Down arrow key
      {
//        Serial.println("Down");
        key = KEY_DOWN;
      }
      else if(ch == 0x41) // Up arrow key
      {
//        Serial.println("Up");
        key = KEY_UP;
      }
      else
      {
        keystate = 0;
        key = KEY_NONE;
      }
    }
  } else {
    key = KEY_NONE;
  }
  return key;
}


void optionMenuMoveCamera() {
  modeDispLine = "Move-Ent to Exit";
  updateScreen();
  manualMoveCamera();
}

void optionMenuConfigure() {
  modeDispLine = "Set End Pos 1";
  updateScreen();
  manualMoveCamera();
  int pos1Pan = servoPan.read();
  int pos1Tilt = servoTilt.read();
  modeDispLine = "Set End Pos 2";
  updateScreen();
  manualMoveCamera();
  int pos2Pan = servoPan.read();
  int pos2Tilt = servoTilt.read();
  if(pos1Pan < pos2Pan) {
    panStart = pos2Pan;
    panEnd = pos1Pan;
  } else if(pos1Pan > pos2Pan) {
    panStart = pos1Pan;
    panEnd = pos2Pan;
  }
  if(pos1Tilt < pos2Tilt) {
    tiltStart = pos2Tilt;
    tiltEnd = pos1Tilt;
  } else if(pos1Tilt > pos2Tilt) {
    tiltStart = pos1Tilt;
    tiltEnd = pos2Tilt;
  }

}

void optionMenuConfigureStep() {
  modeDispLine = "Set Step Pos 1";
  updateScreen();
  manualMoveCamera();
  int pos1Pan = servoPan.read();
  int pos1Tilt = servoTilt.read();
  modeDispLine = "Set Step Pos 2";
  updateScreen();
  manualMoveCamera();
  int pos2Pan = servoPan.read();
  int pos2Tilt = servoTilt.read();
  if(pos1Pan > pos2Pan) {
    panStep = pos1Pan - pos2Pan;
  } else if(pos2Pan > pos1Pan) {
    panStep = pos2Pan - pos1Pan;
  }
  if(pos1Tilt > pos2Tilt) {
    tiltStep = pos1Tilt - pos2Tilt;
  } else if(pos2Tilt > pos1Tilt) {
    tiltStep = pos2Tilt - pos1Tilt;
  }
}

void manualMoveCamera() {
  boolean updateTilt = false;
  boolean updatePan = false;


  while (true) {

    int key = getKey();
    if (key == KEY_ENTER) {
      Serial.println("Exit Move");
      return;    // done!
    } else if (key == KEY_UP) {
//    Serial.println("Up");
    int tiltAngle = servoTilt.read();
    if(tiltAngle < 180 - tiltMoveStep)
    {
      tiltAngle = tiltAngle + tiltMoveStep; // step right
    }
    else
    {
      tiltAngle = 180;
    }
    moveServoTo(servoTilt, tiltAngle);
    updateScreen();
      updateTilt = true;
    } else if (key == KEY_DOWN) {
//        Serial.println("Down");
        int tiltAngle = servoTilt.read();
        if(tiltAngle > tiltMoveStep)
        {
          tiltAngle = tiltAngle - tiltMoveStep; // step Down
        }
        else
        {
          tiltAngle = 0;
        }
        moveServoTo(servoTilt, tiltAngle);
        updateScreen();
      updateTilt = true;
    } else if (key == KEY_LEFT) {
//        Serial.println("Left");
        int panAngle = servoPan.read();
        if(panAngle < 180 - panMoveStep)
        {
          panAngle = panAngle + panMoveStep; // step right
        }
        else
        {
          panAngle = 180;
        }
        moveServoTo(servoPan, panAngle);
        updateScreen();
      updatePan = true;
    } else if (key == KEY_RIGHT) {
//        Serial.println("Right");
        int panAngle = servoPan.read();
        if(panAngle > panMoveStep)
        {
          panAngle = panAngle - panMoveStep; // step left
        }
        else
        {
          panAngle = 0;
        }
        moveServoTo(servoPan, panAngle);
        updateScreen();
      updatePan = true;
    }

  }
}


void snap() {
  servoShutter.write(SHUTTER_SHOOT_POINT);
  delay(500);
  servoShutter.write(SHUTTER_START_POINT);
  delay(5000);
}

void moveServoTo(Servo servo, int targetPos) {
  int currentPos = servo.read();  //  get last position (the last write)
  if (targetPos > currentPos) {
    for (int pos=currentPos; pos<=targetPos; pos++) {
      servo.write(pos);
      delay(20);
    }
  }
  else {

    for (int pos=currentPos; pos>=targetPos; pos--) {
      servo.write(pos);
      delay(20);
    }
  }
  delay(50);
}

// function clearAndHome()
// clear the terminal screen and send the cursor home
void terminalClear()
{
Serial.write(27); // ESC
Serial.write("[2J"); // clear screen
Serial.write(27); // ESC
Serial.write("[H"); // cursor to home
Serial.println("PanoBot - v0.19"); // Header
Serial.println("----------------");

}

void updateScreen() {
      int panAngle = servoPan.read();
      int tiltAngle = servoTilt.read();
      panAngle = 180 - panAngle;
      char angleprint[19];
      sprintf(angleprint, "Pan:%3d Tilt:%3d", panAngle, tiltAngle);
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print(angleprint);
      lcd.setCursor(0, 1);
      lcd.print(modeDispLine);
      terminalClear();
      Serial.println(angleprint);
      Serial.println(modeDispLine);
      while (Serial.available() > 0) char nothing = Serial.read();
}

void beep(){
  digitalWrite(PIN_BEEP, HIGH);
  delay(10);
  digitalWrite(PIN_BEEP, LOW);
}

void gogopan() {
Serial.println("PanoBot GO");
  modeDispLine = "PanoBot GO";
  beep();

  for (int tiltPos=tiltStart; tiltPos>=tiltEnd; tiltPos-=tiltStep) {
    moveServoTo(servoTilt, tiltPos);
    delay(200);
    for (int panPos=panStart; panPos>=panEnd; panPos-=panStep) {
      delay(300);
      moveServoTo(servoPan, panPos);
      updateScreen();
//    delay(750);
      snap();
    }
  }
  beep();
  moveServoTo(servoPan, 90);
  moveServoTo(servoTilt, 90);
}