Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #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);
- }
Add Comment
Please, Sign In to add comment