SHARE
TWEET

Untitled

a guest Nov 14th, 2017 46 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <Servo.h>
  2. #include <LiquidCrystal.h>
  3.  
  4.  
  5. #define PIN_SERVO_SHUTTER   6
  6. #define PIN_SERVO_TILT  10
  7. #define PIN_SERVO_PAN   9
  8. #define PIN_BEEP   8
  9.  
  10. #define SHUTTER_START_POINT   35
  11. #define SHUTTER_SHOOT_POINT   50
  12.  
  13. Servo servoPan;
  14. Servo servoTilt;
  15. Servo servoShutter;
  16.  
  17.  
  18. int currentPan = 90;
  19. int currentTilt = 90;
  20.  
  21. int tiltStart = 120;
  22. int tiltEnd = 60;
  23. int tiltMoveStep = 5;
  24. int tiltStep = 20;
  25.  
  26. int panStart = 120;
  27. int panEnd = 60;
  28. int panMoveStep = 5;
  29. int panStep = 20;
  30.  
  31. char keystate = 0;
  32. long menuPaint = 0;
  33. long debounceTimer = 0;
  34.  
  35. String modeDispLine = "Main Menu";
  36.  
  37. enum keys { KEY_ENTER, KEY_UP, KEY_DOWN, KEY_LEFT, KEY_RIGHT, KEY_NONE, KEY_RESET };
  38.  
  39. char *mainMenuOptions[] = {
  40.   "mMove Camera", "tShoot One", "pAuto Panorama", "iStop Motion", "cConfigure Ends", "aConfigure Step" };
  41.  
  42. int numberOfMainMenuOptions = sizeof(mainMenuOptions) / sizeof(char *);
  43. int currentMainMenuOption = 0;
  44.  
  45. LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
  46.  
  47.  
  48. void setup() {
  49.   lcd.begin(16, 2);
  50.   lcd.print("PanoBot Booting");
  51.  
  52.   Serial.begin(9600);
  53.   Serial.println("PanoBot Booting...");
  54.  
  55.   // attach the servos one at a time
  56.   Serial.println("Attaching Servo Shutter");
  57.   servoShutter.attach(PIN_SERVO_SHUTTER);
  58.   servoShutter.write(SHUTTER_START_POINT);
  59.   delay(300);
  60.   //
  61.   Serial.println("Attaching Servo Tilt");
  62.   servoTilt.attach(PIN_SERVO_TILT);
  63.   servoTilt.write(currentTilt);
  64.   delay(300);
  65.   //
  66.   Serial.println("Attaching Servo Pan");
  67.   servoPan.attach(PIN_SERVO_PAN);
  68.   servoPan.write(currentPan);
  69.   delay(500);
  70.  
  71.   pinMode(PIN_BEEP, OUTPUT);
  72.   digitalWrite(PIN_BEEP, HIGH);
  73.   delay(10);
  74.   digitalWrite(PIN_BEEP, LOW);
  75.  
  76.  
  77.   Serial.println("PanoBot Ready:");
  78. }
  79.  
  80. void loop() {
  81.  
  82.  
  83.   char *mainMenuOption = mainMenuOptions[currentMainMenuOption];
  84.  
  85.   // up/down/select <option_name>
  86. if (millis() > menuPaint ) {
  87.   modeDispLine = mainMenuOption+1;
  88.   updateScreen();
  89.   menuPaint = millis() + 5000;
  90. }
  91.   int key = getKey();
  92.  
  93.   if (key == KEY_ENTER) {
  94.     switch (mainMenuOption[0]) {
  95.     case 'm': // move camera manually
  96. //      Serial.println("Moving Camera:");
  97.       optionMenuMoveCamera();
  98.       break;
  99.     case 't': // shoot a picture
  100. //      Serial.println("PanoBot T:");
  101.       modeDispLine = "Shooting";
  102.       updateScreen();
  103.       snap();
  104.       menuPaint = millis();
  105.       break;
  106.     case 'p': // pano
  107. //      Serial.println("PanoBot P:");
  108.       Serial.println(tiltStart);
  109.       Serial.println(tiltEnd);
  110.       Serial.println(tiltStep);
  111.       Serial.println(panStart);
  112.       Serial.println(panEnd);
  113.       Serial.println(panStep);
  114.       gogopan();
  115.       menuPaint = millis();
  116.       break;
  117.     case 'i': // stop motion
  118.       Serial.println("UNDER CONSTRUCTION");
  119.       break;
  120.     case 'c': // configure Ends
  121. //      Serial.println("PanoBot C:");
  122.       optionMenuConfigure();
  123.       menuPaint = millis();
  124.       break;
  125.     case 'a': // configure Step
  126. //      Serial.println("PanoBot A:");
  127.       optionMenuConfigureStep();
  128.       menuPaint = millis();
  129.       break;
  130.     default:
  131.       break;
  132.     }
  133.   }
  134.   else {
  135.     if (key == KEY_UP) {
  136.       currentMainMenuOption--;
  137.       if (currentMainMenuOption < 0){
  138.         currentMainMenuOption = numberOfMainMenuOptions-1;
  139.       }
  140.           mainMenuOption = mainMenuOptions[currentMainMenuOption];
  141.       modeDispLine = mainMenuOption+1;
  142.       updateScreen();
  143.       menuPaint = millis() + 5000;    }
  144.     else if (key == KEY_DOWN) {
  145.       currentMainMenuOption++;
  146.       if (currentMainMenuOption >= numberOfMainMenuOptions){
  147.         currentMainMenuOption = 0;
  148.       }
  149.           mainMenuOption = mainMenuOptions[currentMainMenuOption];
  150.       modeDispLine = mainMenuOption+1;
  151.       updateScreen();
  152.       menuPaint = millis() + 5000;
  153.     }
  154.   }
  155.  
  156.  
  157. }
  158.  
  159.  
  160. int getKey() {
  161. int key;
  162.  
  163.   if ((debounceTimer < millis()) && (analogRead(A0) > 400)) {
  164.     delay(50);
  165.     if (analogRead(A0) > 400) {
  166.     key = KEY_RESET;
  167.     debounceTimer = millis() + 500;
  168.     return key;
  169.     }
  170.   }
  171.   if ((debounceTimer < millis()) && (analogRead(A1) > 400)) {
  172.     delay(50);
  173.     if (analogRead(A1) > 400) {
  174.     key = KEY_LEFT;
  175.     debounceTimer = millis() + 500;
  176.     return key;
  177.     }
  178.   }
  179.   if ((debounceTimer < millis()) && (analogRead(A2) > 400)) {
  180.  
  181.     delay(50);
  182.     if (analogRead(A2) > 400) {
  183.     key = KEY_DOWN;
  184.     debounceTimer = millis() + 500;
  185.     return key;
  186.     }
  187.   }
  188.   if ((debounceTimer < millis()) && (analogRead(A3) > 400)) {
  189.  
  190.     delay(50);
  191.     if (analogRead(A3) > 400) {
  192.     key = KEY_UP;
  193.     debounceTimer = millis() + 500;
  194.     return key;
  195.     }
  196.   }
  197.   if ((debounceTimer < millis()) && (analogRead(A4) > 400)) {
  198.  
  199.     delay(50);
  200.     if (analogRead(A4) > 400) {
  201.     key = KEY_RIGHT;
  202.     debounceTimer = millis() + 500;
  203.     return key;
  204.     }
  205.   }
  206.   if ((debounceTimer < millis()) && (analogRead(A5) > 400)) {
  207.  
  208.     delay(50);
  209.     if (analogRead(A5) > 400) {
  210.     key = KEY_ENTER;
  211.     debounceTimer = millis() + 500;
  212.     return key;
  213.     }
  214.   }
  215.  
  216.  
  217.  
  218.   if (Serial.available() > 0) {
  219.     char ch = Serial.read();
  220. //    Serial.println(ch, HEX);
  221.     if(ch == 0x72)
  222.     {
  223.       key = KEY_RESET;
  224. //      Serial.println("R");
  225.     }
  226.     if(ch == 0xD)
  227.     {
  228. //      Serial.println("Enter");
  229.       key = KEY_ENTER;
  230.       }
  231.     else if(ch == 0x1B)
  232.     {
  233.       keystate = 0x1B;
  234.       key = KEY_NONE;
  235.     }
  236.     else if(ch == 0x5B)
  237.     {
  238.       if(keystate == 0x1B) {
  239.         keystate = 0x5B;
  240.         key = KEY_NONE;
  241.       } else {
  242.         keystate = 0;
  243.         key = KEY_NONE;
  244.       }
  245.     }
  246.     else if(keystate == 0x5B)
  247.     {
  248.       if(ch == 0x43) // Right arrow key
  249.       {
  250. //        Serial.println("Right");
  251.         key = KEY_RIGHT;
  252.       }
  253.       else if(ch == 0x44) // Left arrow key
  254.       {
  255. //        Serial.println("Left");
  256.         key = KEY_LEFT;
  257.       }
  258.       else if(ch == 0x42) // Down arrow key
  259.       {
  260. //        Serial.println("Down");
  261.         key = KEY_DOWN;
  262.       }
  263.       else if(ch == 0x41) // Up arrow key
  264.       {
  265. //        Serial.println("Up");
  266.         key = KEY_UP;
  267.       }
  268.       else
  269.       {
  270.         keystate = 0;
  271.         key = KEY_NONE;
  272.       }
  273.     }
  274.   } else {
  275.     key = KEY_NONE;
  276.   }
  277.   return key;
  278. }
  279.  
  280.  
  281. void optionMenuMoveCamera() {
  282.   modeDispLine = "Move-Ent to Exit";
  283.   updateScreen();
  284.   manualMoveCamera();
  285. }
  286.  
  287. void optionMenuConfigure() {
  288.   modeDispLine = "Set End Pos 1";
  289.   updateScreen();
  290.   manualMoveCamera();
  291.   int pos1Pan = servoPan.read();
  292.   int pos1Tilt = servoTilt.read();
  293.   modeDispLine = "Set End Pos 2";
  294.   updateScreen();
  295.   manualMoveCamera();
  296.   int pos2Pan = servoPan.read();
  297.   int pos2Tilt = servoTilt.read();
  298.   if(pos1Pan < pos2Pan) {
  299.     panStart = pos2Pan;
  300.     panEnd = pos1Pan;
  301.   } else if(pos1Pan > pos2Pan) {
  302.     panStart = pos1Pan;
  303.     panEnd = pos2Pan;
  304.   }
  305.   if(pos1Tilt < pos2Tilt) {
  306.     tiltStart = pos2Tilt;
  307.     tiltEnd = pos1Tilt;
  308.   } else if(pos1Tilt > pos2Tilt) {
  309.     tiltStart = pos1Tilt;
  310.     tiltEnd = pos2Tilt;
  311.   }
  312.  
  313. }
  314.  
  315. void optionMenuConfigureStep() {
  316.   modeDispLine = "Set Step Pos 1";
  317.   updateScreen();
  318.   manualMoveCamera();
  319.   int pos1Pan = servoPan.read();
  320.   int pos1Tilt = servoTilt.read();
  321.   modeDispLine = "Set Step Pos 2";
  322.   updateScreen();
  323.   manualMoveCamera();
  324.   int pos2Pan = servoPan.read();
  325.   int pos2Tilt = servoTilt.read();
  326.   if(pos1Pan > pos2Pan) {
  327.     panStep = pos1Pan - pos2Pan;
  328.   } else if(pos2Pan > pos1Pan) {
  329.     panStep = pos2Pan - pos1Pan;
  330.   }
  331.   if(pos1Tilt > pos2Tilt) {
  332.     tiltStep = pos1Tilt - pos2Tilt;
  333.   } else if(pos2Tilt > pos1Tilt) {
  334.     tiltStep = pos2Tilt - pos1Tilt;
  335.   }
  336. }
  337.  
  338. void manualMoveCamera() {
  339.   boolean updateTilt = false;
  340.   boolean updatePan = false;
  341.  
  342.  
  343.   while (true) {
  344.  
  345.     int key = getKey();
  346.     if (key == KEY_ENTER) {
  347.       Serial.println("Exit Move");
  348.       return;    // done!
  349.     } else if (key == KEY_UP) {
  350. //    Serial.println("Up");
  351.     int tiltAngle = servoTilt.read();
  352.     if(tiltAngle < 180 - tiltMoveStep)
  353.     {
  354.       tiltAngle = tiltAngle + tiltMoveStep; // step right
  355.     }
  356.     else
  357.     {
  358.       tiltAngle = 180;
  359.     }
  360.     moveServoTo(servoTilt, tiltAngle);
  361.     updateScreen();
  362.       updateTilt = true;
  363.     } else if (key == KEY_DOWN) {
  364. //        Serial.println("Down");
  365.         int tiltAngle = servoTilt.read();
  366.         if(tiltAngle > tiltMoveStep)
  367.         {
  368.           tiltAngle = tiltAngle - tiltMoveStep; // step Down
  369.         }
  370.         else
  371.         {
  372.           tiltAngle = 0;
  373.         }
  374.         moveServoTo(servoTilt, tiltAngle);
  375.         updateScreen();
  376.       updateTilt = true;
  377.     } else if (key == KEY_LEFT) {
  378. //        Serial.println("Left");
  379.         int panAngle = servoPan.read();
  380.         if(panAngle < 180 - panMoveStep)
  381.         {
  382.           panAngle = panAngle + panMoveStep; // step right
  383.         }
  384.         else
  385.         {
  386.           panAngle = 180;
  387.         }
  388.         moveServoTo(servoPan, panAngle);
  389.         updateScreen();
  390.       updatePan = true;
  391.     } else if (key == KEY_RIGHT) {
  392. //        Serial.println("Right");
  393.         int panAngle = servoPan.read();
  394.         if(panAngle > panMoveStep)
  395.         {
  396.           panAngle = panAngle - panMoveStep; // step left
  397.         }
  398.         else
  399.         {
  400.           panAngle = 0;
  401.         }
  402.         moveServoTo(servoPan, panAngle);
  403.         updateScreen();
  404.       updatePan = true;
  405.     }
  406.  
  407.   }
  408. }
  409.  
  410.  
  411. void snap() {
  412.   servoShutter.write(SHUTTER_SHOOT_POINT);
  413.   delay(500);
  414.   servoShutter.write(SHUTTER_START_POINT);
  415.   delay(5000);
  416. }
  417.  
  418. void moveServoTo(Servo servo, int targetPos) {
  419.   int currentPos = servo.read();  //  get last position (the last write)
  420.   if (targetPos > currentPos) {
  421.     for (int pos=currentPos; pos<=targetPos; pos++) {
  422.       servo.write(pos);
  423.       delay(20);
  424.     }
  425.   }
  426.   else {
  427.  
  428.     for (int pos=currentPos; pos>=targetPos; pos--) {
  429.       servo.write(pos);
  430.       delay(20);
  431.     }
  432.   }
  433.   delay(50);
  434. }
  435.  
  436. // function clearAndHome()
  437. // clear the terminal screen and send the cursor home
  438. void terminalClear()
  439. {
  440. Serial.write(27); // ESC
  441. Serial.write("[2J"); // clear screen
  442. Serial.write(27); // ESC
  443. Serial.write("[H"); // cursor to home
  444. Serial.println("PanoBot - v0.19"); // Header
  445. Serial.println("----------------");
  446.  
  447. }
  448.  
  449. void updateScreen() {
  450.       int panAngle = servoPan.read();
  451.       int tiltAngle = servoTilt.read();
  452.       panAngle = 180 - panAngle;
  453.       char angleprint[19];
  454.       sprintf(angleprint, "Pan:%3d Tilt:%3d", panAngle, tiltAngle);
  455.       lcd.clear();
  456.       lcd.setCursor(0, 0);
  457.       lcd.print(angleprint);
  458.       lcd.setCursor(0, 1);
  459.       lcd.print(modeDispLine);
  460.       terminalClear();
  461.       Serial.println(angleprint);
  462.       Serial.println(modeDispLine);
  463.       while (Serial.available() > 0) char nothing = Serial.read();
  464. }
  465.  
  466. void beep(){
  467.   digitalWrite(PIN_BEEP, HIGH);
  468.   delay(10);
  469.   digitalWrite(PIN_BEEP, LOW);
  470. }
  471.  
  472. void gogopan() {
  473. Serial.println("PanoBot GO");
  474.   modeDispLine = "PanoBot GO";
  475.   beep();
  476.  
  477.   for (int tiltPos=tiltStart; tiltPos>=tiltEnd; tiltPos-=tiltStep) {
  478.     moveServoTo(servoTilt, tiltPos);
  479.     delay(200);
  480.     for (int panPos=panStart; panPos>=panEnd; panPos-=panStep) {
  481.       delay(300);
  482.       moveServoTo(servoPan, panPos);
  483.       updateScreen();
  484. //    delay(750);
  485.       snap();
  486.     }
  487.   }
  488.   beep();
  489.   moveServoTo(servoPan, 90);
  490.   moveServoTo(servoTilt, 90);
  491. }
RAW Paste Data
Top