API tools faq
paste
Guest User

Untitled

a guest
Nov 14th, 2017
83
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 10.71 KB | None | 0 0
raw download clone embed print report
  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. }
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!