diff --git a/.gitignore b/.gitignore index b8bd026..10e6534 100644 --- a/.gitignore +++ b/.gitignore @@ -26,3 +26,7 @@ *.exe *.out *.app + +# Eclipse +.*project + diff --git a/Makefile b/Makefile index d60d91d..ba97623 100644 --- a/Makefile +++ b/Makefile @@ -2,33 +2,52 @@ # # -ARDUINO := arduino --verbose --preserve-temp-files +ARDUINO := arduino --preserve-temp-files + +ifndef board +$(error Usage: make board=) +endif + +ifeq ($(board),teensy31) + # Teensy 3.1/3.2 target + # Teensy options hardware/teensy/avr/boards.txt + BOARD = teensy:avr:teensy31:usb=serial,keys=en-us,speed=72 + PORT = /dev/ttyACM0 +endif +ifeq ($(board),teensyLC) + # Teensy LC target + # Teensy options hardware/teensy/avr/boards.txt + BOARD = teensy:avr:teensyLC:usb=serial,keys=en-us,speed=48 + PORT = /dev/ttyACM0 +endif +ifeq ($(board),feather) + # Adafruit Feather M0 + # must be installed with arduino --install-boards "adafruit:samd" + # ~/.arduino15/packages/adafruit/hardware/samd/1.0.13/boards.txt + BOARD = adafruit:samd:adafruit_feather_m0 + PORT = /dev/ttyACM0 +endif +ifeq ($(board),uno) + # Arduino UNO target + # arduino options hardware/arduino/avr/boards.txt + BOARD = arduino:avr:uno + PORT = /dev/ttyUSB0 +endif + # Mac #ARDUINO := Arduino.app/Contents/MacOS/Arduino --verbose --preserve-temp-files TMPDIR := /tmp/pano-controller-build -SOURCES := pano-controller.ino *.cpp *.h - -# -# Specify which of the below targets to build (default teensyLC) -# -all: teensyLC +LIB_SOURCES = src/*.cpp src/*.h -# Arduino UNO target -# arduino options hardware/arduino/avr/boards.txt -uno: BOARD = arduino:avr:uno +panocontroller: examples/PanoController/PanoController.ino $(LIB_SOURCES) $(TMPDIR) + $(ARDUINO) --upload $< --board $(BOARD) --port $(PORT) --pref build.path=$(TMPDIR) -# Teensy LC target -# Teensy options hardware/teensy/avr/boards.txt -teensyLC: BOARD = teensy:avr:teensyLC - -# -# Common rule for building and uploading the project -# -# See https://github.com/arduino/Arduino/blob/ide-1.5.x/build/shared/manpage.adoc -uno teensyLC: $(SOURCES) $(TMPDIR) - $(ARDUINO) --upload $< --board $(BOARD) --pref build.path=$(TMPDIR) +navigator: examples/Navigator/Navigator.ino $(LIB_SOURCES) $(TMPDIR) + $(ARDUINO) --upload $< --board $(BOARD) --port $(PORT) --pref build.path=$(TMPDIR) +executor: examples/Executor/Executor.ino $(LIB_SOURCES) $(TMPDIR) + $(ARDUINO) --upload $< --board $(BOARD) --port $(PORT) --pref build.path=$(TMPDIR) $(TMPDIR): mkdir -p $(TMPDIR) diff --git a/README.md b/README.md index 63f3965..8027f5f 100644 --- a/README.md +++ b/README.md @@ -6,15 +6,19 @@ flexible and support home-brew panoramic platforms. See the Official Facebook page for demo videos and more progress photos. -Current state: fully functional prototype board, field-tested, frequently upgraded. +Current state: fully functional two-module prototype boards, field-tested, frequently upgraded. One module (Executor) is installed on the Gigapan platform. The other module +is the radio controller with OLED display (Navigator). + +Pano Controller Executor Boards installed in Gigapan EPIC 100 +Pano Controller Remote Navigator Unit +Pano Controller Pano Information Screen -Pano Controller Prototype Board installed in Gigapan EPIC 100 -Pano Controller Pano Information Screen ## Features: ### Software - **Zero-motion shutter delay!** When gyro is connected, waits for platform to stabilize before triggering. Useful to compensate for tripod stability, platform's own movement or wind gusts. +- **Remote Control Demo** - Focal length presets from 12 to 600mm. Precise sub-degree movement control. - Seamless 360 pano option - Display: @@ -24,97 +28,191 @@ Current state: fully functional prototype board, field-tested, frequently upgrad - Precision movement control - Multiple delay options: pre-shutter and post-shutter, short or long shutter pulse (for bracketing). - Bluetooth joystick control via phone +- Radio Remote Menu Navigation unit ### Hardware -- 32-bit ARM controller board +- 32-bit ARM controllers (Gigapan had 8-bit AVR) - OLED 128x64 display -- Joystick menu navigation, optional IR remote +- Radio-controlled joystick menu navigation - Can operate with battery voltage from 10V down to 6V - Lower power usage and later voltage cutoff than original Gigapan ## Wiring map -### Teensy LC / 3.x -- A0 - Battery Voltage via divider: Vin---[47K]---A0---[10K]---GND -- A1 - Joystick SW -- A2 - Joystick Vy -- A3 - Joystick Vx -- A4 - SDA - Display and MPU-6050 board -- A5 - SCL - Display and MPU-6050 board -- A6 -- A7 -- D0/RX - camera focus (active LOW) -- D1/TX - camera shutter (active LOW) +The new modular layout I'm working on needs two controller boards: an Executor unit +installed on the platform, and a radio remote Navigator unit with all the menus and +controls. Both units are needed. + +For the previous single-board setup, see release v1.4 (https://github.com/laurb9/pano-controller/tree/v1.4) + +Future plans involve replacing the remote Navigator unit with a phone or other +high-level interface device. + +### Executor Unit + +The execution unit is installed on the rotating platform. Since there are no +buttons here, the display is only used for status info and may eventually go away. + +#### Teensy LC / 3.x + +Status: Tested with NRF24L01+ radio. +BLE/BLUEFRUIT is not part of the current Executor code. + +- A0 - BATTERY - Battery Voltage via divider: Vin---[47K]---A0---[10K]---GND +- A1 +- A2 +- A3 +- A4 - SDA - MPU-6050 board and SSD-1306 Display +- A5 - SCL - MPU-6050 board and SSD-1306 Display +- A6 - NRF24_CE +- A7 - NRF24_CSN +- D0/RX - CAMERA_FOCUS active LOW +- D1/TX - CAMERA_SHUTTER active LOW - D2 - MPU-6050 INT -- D3(int) - IR Remote In (AX-1838HS) -- D4 - BLE RST -- D5 - DIR (both) -- D6 - StepperV STEP -- D7 - BLE INT -- D8 - BLE CS -- D9 - StepperH STEP -- D10 - ~ENABLE (both) -- D11 - SPI MOSI[BLE] -- D12 - SPI MISO[BLE] -- D13[LED] - SPI SCK[BLE] - -### Feather M0 / Bluefruit +- D3 +- D4 - (BLUEFRUIT_SPI_RST) +- D5 - DIR +- D6 - VERT_STEP +- D7 - (BLUEFRUIT_SPI_IRQ) +- D8 - (BLUEFRUIT_SPI_CS) +- D9 - HORIZ_STEP +- D10 - nENABLE +- D11 - SPI_MOSI - NRF24 or (BLUEFRUIT) +- D12 - SPI_MISO - NRF24 or (BLUEFRUIT) +- D13 - SPI_SCK - NRF24 or (BLUEFRUIT) + +#### Feather M0 / Bluefruit + +Status: Not tested. See release v1.4 for functional onboard Feather/BLE. +NRF24 library not working with Feather M0 yet so that's a dead end at the moment. - A0 -- A1 - Joystick SW -- A2 - Joystick Vy -- A3 - Joystick Vx +- A1 +- A2 +- A3 - A4 - Battery Voltage via divider: Vin---[47K]---A0---[10K]---GND -- A5 -- SCK[BLE] -- MOSI[BLE] -- MISO[BLE] -- RX/0 - camera focus (active LOW) -- TX/1 - camera shutter (active LOW) - +- A5 - (NRF24_CE) +- SCK - BLE(internal) and NRF24 +- MOSI - BLE(internal) and NRF24 +- MISO - BLE(internal) and NRF24 +- RX/0 - CAMERA_FOCUS active LOW +- TX/1 - CAMERA_SHUTTER active LOW - 4[BLE] CS (internally connected) - 7[BLE] IRQ (internally connected) - 8[BLE] RST (internally connected) - -- SDA/20 - Display and MPU-6050 board -- SCL/21 - Display and MPU-6050 board -- 5 - DIR (both) -- 6 - StepperV STEP -- 9[A7] - StepperH STEP -- 10 - ~ENABLE (both) -- 11 +- SDA/20 - MPU-6050 board and SSD-1306 Display +- SCL/21 - MPU-6050 board and SSD-1306 Display +- 5 - DIR +- 6 - VERT_STEP +- 9[A7] - HORIZ_STEP +- 10 - nENABLE +- 11 - (NRF24_CSN) - 12 - MPU-6050 INT - 13[LED] -### Other +#### All +- Vin is 6-10V +- 3.3V step-down adapter from Vin to Vcc for logic power - All ~SLEEP tied to Vcc - All VMOT tied to Vin - All M1 tied to Vcc (1:32 mode) - All M0 left unconnected (1:32 mode) -- 3.3V step-down adapter from Vin to Vcc +- All ~ENABLE tied together +- All DIR tied together +- 100uF capacitor at Vin +- 10uF capacitor at Vcc +- 10K pull-up resistor from Vcc to ~ENABLE +- Voltage divider Vin---[47K]---A0---[10K]---GND + +### Navigator Remote Control Unit + +This is where all the configuration happens. This can sit in your hand while the +platform is perched up on a pole and not physically reachable. + +Settings and commands are sent to the Executor unit for execution. + +#### Teensy LC / 3.x + +Status: Tested with NRF24L01+ radio. +BLE/BLUEFRUIT is not part of the current Navigator code. + +- A0 +- A1 - Joystick SW +- A2 - Joystick Vy +- A3 - Joystick Vx +- A4 - SDA - SSD-1306 Display +- A5 - SCL - SSD-1306 Display +- A6 - NRF24_CE +- A7 - NRF24_CSN +- D0/RX +- D1/TX +- D2 +- D3 +- D4 - (BLUEFRUIT_SPI_RST) +- D5 +- D6 +- D7 - (BLUEFRUIT_SPI_IRQ) +- D8 - (BLUEFRUIT_SPI_CS) +- D9 +- D10 +- D11 - SPI_MOSI - NRF24 or (BLUEFRUIT) +- D12 - SPI_MISO - NRF24 or (BLUEFRUIT) +- D13 - SPI_SCK - NRF24 or (BLUEFRUIT) + +#### Feather M0 / Bluefruit + +Status: untested (NRF24 library not working with Feather yet). + +- A0 +- A1 - Joystick SW +- A2 - Joystick Vy +- A3 - Joystick Vx +- A4 +- A5 - NRF24_CE +- SCK - BLE(internal) and NRF24 +- MOSI - BLE(internal) and NRF24 +- MISO - BLE(internal) and NRF24 +- RX/0 +- TX/1 +- 4[BLE] CS (internally connected) +- 7[BLE] IRQ (internally connected) +- 8[BLE] RST (internally connected) +- SDA/20 - SSD-1306 Display +- SCL/21 - SSD-1306 Display +- 5 +- 6 +- 9[A7] +- 10 +- 11 - NRF24_CSN +- 12 +- 13[LED] + +#### All + +- Vin power from USB or 3.3-5V external battery using Teensy converter +- 10uF capacitor at Vcc ## Notes - *Atmega328-based boards are not supported*, see issue #57 -- IR remote not supported on Adafruit Feather M0, see issue #59 +- TWO controllers are needed for the current design. The last single-controller design is release v1.4. - Settings memory on Feather M0 only works with Bluefruit (M0 has no EEPROM itself) - Future rewiring plan - if we ever want to use ESP-12, need to reduce pins. ESP-12 only has 11: (0,2,4,5,12,13,14,15,16,RXD,TXD,ADC) - - Adafruit Feather M0 BLE uses 4, 7, 8 as Bluefruit RESET, IRQ and CS. - Should use same pinout for connecting to external BLE board (SPI). - - nrf24 library hardcodes pin 13 so if we plan to use it, we'll have to rewire. -Breadboard setup with Teensy LC +Breadboard playing with Feather M0 -## Setting DRV8834 reference +## Setting stepper motor current limit for DRV8834 - Vref0 = 2V +- 0.1V - Pololu schematic shows Risense = 0.1 ohm - Itrip = Vref/(5*Risense) - So set Vref = Itrip/2 +### Gigapan EPIC 100 + Gigapan motor spec is 1A, so 0.5V. At full step the current limit is 0.7*Itrip, so we have to set Itrip to 1.4 and Vref to 0.7V as upper bound. @@ -131,12 +229,8 @@ lower current even, if we reduce the speed. - Microcontroller, one of below - PJRC Teensy LC - PJRC Teensy 3.1+ - - Adafruit Feather M0 (no IR remote) - - Adafruit Feather M0 Bluefruit (no IR remote) - - (untested but same Cortex M0 as above) Arduino Zero - 2 x DRV8834 Low-Voltage Stepper Motor Driver from Pololu -- 128x64 OLED display, SSD1306 I2C from anywhere -- Adafruit Bluefruit SPI Breakout (if not onboard) - provides Bluetooth LE 4.1 joystick control - optional +- 2 x 128x64 OLED display, SSD1306 I2C from anywhere - 2-axis + switch analog joystick - 1834HS IR receiver with some remote - optional but recommended - Remote codes are hardcoded in remote.cpp if you have a different remote @@ -150,11 +244,20 @@ lower current even, if we reduce the speed. - 2-pin power connector/DC power jack - 6AA battery holder or a 6V-10V power source. +#### Future support planned for + +- lack of support for NRF24 + - Adafruit Feather M0 + - Adafruit Feather M0 Bluefruit + - (untested but same Cortex M0 as above) Arduino Zero +- Adafruit Bluefruit SPI Breakout (if not onboard) - provides Bluetooth LE 4.1 joystick control + ### Libraries - Adafruit_SSD1306 - Adafruit_GFX - Adafruit Bluefruit nRF51 -- IRremote +- RF24 +- (Adafruit_BluefruitLE_SPI) - Wire - StepperDriver @@ -172,4 +275,3 @@ The only thing required of the platform is the two horiz/vert stepper motors. - Notes: - the DRV8834 current limit must be set according to motor spec - reduction gear settings are hardcoded in pano.h - diff --git a/examples/Executor/Executor.ino b/examples/Executor/Executor.ino new file mode 100644 index 0000000..d075d67 --- /dev/null +++ b/examples/Executor/Executor.ino @@ -0,0 +1,291 @@ +/* + * Headless Pano Controller + * + * A basic panorama controller without menus and advanced HID devices. + * + * Copyright (C)2015,2016 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include +#include +#include +#include "config.h" +#include "pano.h" +#include "camera.h" +#include "display.h" +#include "mpu.h" +#include "radio.h" +#include "protocol.h" + +// these variables are modified by the menu +PanoSettings settings; + +// This is the panorama engine state +// should technically be in the Pano object +PanoState state; + +static Display display(OLED_RESET); +static Radio radio(NRF24_CE, NRF24_CSN); +static Exec comm(radio); + +static Camera* camera; +static MPU* mpu; +static DRV8834* horiz_motor; +static DRV8834* vert_motor; +static Pano* pano; + +void setup() { + Serial.begin(38400); + delay(1000); // wait for serial + + display.begin(SSD1306_SWITCHCAPVCC, DISPLAY_I2C_ADDRESS, false); + display.setRotation(2); + display.clearDisplay(); + display.setTextCursor(0,0); + display.setTextColor(WHITE); + display.setTextSize(TEXT_SIZE); + + radio.begin(); + + Serial.println("Checking Camera Shutter Connection"); + pinMode(CAMERA_FOCUS, INPUT_PULLDOWN); + pinMode(CAMERA_SHUTTER, INPUT_PULLDOWN); + Serial.print(" Focus: "); + Serial.println((digitalRead(CAMERA_FOCUS) ? "OK" : "N/C")); + Serial.print(" Shutter: "); + Serial.println((digitalRead(CAMERA_SHUTTER) ? "OK" : "N/C")); + camera = new Camera(CAMERA_FOCUS, CAMERA_SHUTTER); + + mpu = new MPU(MPU_I2C_ADDRESS, MPU_INT); + mpu->init(); + + horiz_motor = new DRV8834(MOTOR_STEPS, DIR, HORIZ_STEP, nENABLE); + vert_motor = new DRV8834(MOTOR_STEPS, DIR, VERT_STEP); + horiz_motor->setMicrostep(32); + vert_motor->setMicrostep(32); + + pano = new Pano(*horiz_motor, *vert_motor, *camera, *mpu); + + pinMode(BATTERY, INPUT); +#if defined(__MK20DX256__) || defined(__MKL26Z64__) + analogReadRes(10); + analogReadAveraging(32); +#endif + + Serial.println("Waiting to connect..."); + display.printf("Waiting to connect..."); + display.display(); + + while (!comm.getConfig(settings)) delay(100); + Serial.println("Received settings"); +} + +void readBattery(void){ + state.battery = map(analogRead(BATTERY), 0, (1<<10)-1, 0, BATT_RANGE/100) * 100; + if (state.battery < LOW_BATTERY){ + state.battery = -state.battery; + } +} + +/* + * Display current panorama status (photo index, etc) + */ +void displayPanoStatus(bool complete){ + display.clearDisplay(); + display.setTextCursor(0,0); + int photos = pano->getHorizShots() * pano->getVertShots(); + + if (state.running){ + display.printf("# %d of %d\n", pano->position+1, photos); + display.printf("at %d x %d\n", 1+pano->getCurRow(), 1+pano->getCurCol()); + } else { + display.printf("%d photos\n\n", photos); + } + display.printf("grid %d x %d \n", pano->getVertShots(), pano->getHorizShots()); + + float horiz_fov = camera->getHorizFOV(); + float vert_fov = camera->getVertFOV(); + display.setTextCursor(3,0); + display.printf("Lens: %dmm\n", settings.focal); + display.printf(" %d.%d x %d.%d\n", + int(horiz_fov), round(10*(horiz_fov-int(horiz_fov))), + int(vert_fov), round(10*(vert_fov-int(vert_fov)))); + display.printf("Pano %d x %d deg\n", pano->horiz_fov, pano->vert_fov); + + // Print battery voltage at cursor, format is #.#V (4 chars) + display.setTextCursor(0, 16); + // poor attempt at blinking + if (state.battery < 0 && millis() & 1024){ + display.setTextColor(BLACK, WHITE); + } + display.printf("%2d.%dV", abs(state.battery)/1000, (abs(state.battery) % 1000)/100); + display.setTextColor(WHITE, BLACK); + + // motors active + if (state.running){ + display.setTextCursor(0,14); + display.print("*"); + } + + // radio connection status + display.setTextCursor(0,15); + if (radio.connected){ + display.print("^"); + } else { + display.print("X"); + } + + display.setTextCursor(6, 0); + if (state.position + 1 < photos){ + display.printf("%d minutes ", pano->getTimeLeft()/60); + } + if (pano->steady_delay_avg > 500){ + display.setTextCursor(6, 16); + display.printf("%2ds ", (pano->steady_delay_avg+500)/1000); + display.printf((pano->steady_delay_avg < 8000) ? "\x12" : "!"); + } + + // show progress bar + if (state.running){ + display.setTextCursor(7, 0); + for (int i=(pano->position+1) * DISPLAY_COLS / photos; i > 0; i--){ + display.print('\xdb'); + }; + }; + if (complete){ + display.display(); + } +} + +/* + * Update camera and pano settings received from navigator + */ +void setPanoParams(void){ + camera->setAspect(settings.aspect); + camera->setFocalLength(settings.focal); + pano->setShutter(settings.shutter, settings.pre_shutter, settings.post_wait, settings.long_pulse); + pano->setShots(settings.shots); + pano->setFOV(settings.horiz, settings.vert); + pano->computeGrid(); + // pano->position = state.position; +} + +/* + * Callbacks for commands received from remote Navigator + */ +void doConfig(void){ + setPanoParams(); + display.invertDisplay(settings.display_invert); +}; +void doStart(void){ + Serial.println("Start pano"); + pano->start(); + state.running = true; + state.paused = (settings.shutter == 0); +}; +void doCancel(void){ + Serial.println("Cancel pano"); + pano->end(); + state.running = false; + state.paused = false; +}; +void doPause(void){ + Serial.println("Pause pano"); + if (state.running){ + state.paused = true; + } +}; +void doShutter(void){ + Serial.println("Shutter"); + pano->shutter(); + if (state.running){ // manual shutter mode, advance to next + state.running = pano->next(); + if (!state.running){ + pano->end(); + } + } +}; +void doSetHome(void){ + Serial.println("Set home"); + pano->setMotorsHomePosition(); +}; +void doGoHome(void){ + Serial.println("Go home"); + pano->motorsEnable(true); + pano->moveMotorsHome(); +}; +void doFreeMove(move_t& move){ + horiz_motor->setRPM(move.horiz_rpm); + vert_motor->setRPM(move.vert_rpm); + pano->motorsEnable(true); + pano->moveMotors(move.horiz_move, move.vert_move); +}; +void doGridMove(const char direction){ + Serial.println("Inc move"); + switch (direction){ + case '<': pano->prev(); break; + case '>': pano->next(); break; + case '^': pano->moveTo(pano->getCurRow() - 1, pano->getCurCol()); break; + case 'v': pano->moveTo(pano->getCurRow() + 1, pano->getCurCol()); break; + } +}; +// unused +void doGridMoveTo(settings_t row, settings_t col){ + Serial.println("Head move"); + pano->moveTo(row, col); +} + +const comm_callbacks callbacks = { + doConfig, + doStart, + doCancel, + doPause, + doShutter, + doSetHome, + doGoHome, + doFreeMove, + doGridMove +}; + +void loop() { + /* + * Read state from menu navigator + */ + comm.getCmd(settings, callbacks); + /* + * Collect and send state to menu navigator + */ + readBattery(); + state.steady_delay_avg = pano->steady_delay_avg; + state.position = pano->position; + state.horiz_offset = pano->horiz_home_offset; + state.vert_offset = pano->vert_home_offset; + state.motors_on = settings.motors_on; + /* + * Render state. + * TODO: We should do this only if anything has changed though. + */ + //displayPanoStatus(true); + /* + * Execute pano + */ + if (!settings.motors_enable || state.running){ + comm.sendState(state); + displayPanoStatus(true); + } + if (state.running){ + if (!state.paused){ + pano->shutter(); + state.running = pano->next(); + if (!state.running){ + pano->end(); + }; + }; + } else { + pano->motorsEnable(state.motors_on); + }; + + yield(); +} diff --git a/config.h b/examples/Executor/config.h similarity index 82% rename from config.h rename to examples/Executor/config.h index fb6c536..97478f4 100644 --- a/config.h +++ b/examples/Executor/config.h @@ -2,16 +2,6 @@ * Pano Controller Master Configuration File */ -#if defined(__AVR__) -#error "AVR is not supported" - -#elif !defined(ARDUINO_SAMD_FEATHER_M0) -#include "config_teensy.h" - -#else -#include "config_feather_m0.h" -#endif - // Address of I2C OLED display. If screen looks scaled edit Adafruit_SSD1306.h // and pick SSD1306_128_64 or SSD1306_128_32 that matches display type. #define DISPLAY_I2C_ADDRESS 0x3C @@ -34,3 +24,17 @@ // Stepper motors steps per revolution #define MOTOR_STEPS 200 + +// board-specific pin settings +#if defined(ARDUINO_ARCH_SAMD) && defined(ARDUINO_SAMD_FEATHER_M0) +#include "config_feather_m0.h" + +#elif defined(__arm__) && defined(CORE_TEENSY) +#include "config_teensy.h" + +#elif defined(__AVR__) +#error "AVR is not supported" + +#else +#error "Unsupported board" +#endif diff --git a/examples/Executor/config_feather_m0.h b/examples/Executor/config_feather_m0.h new file mode 100644 index 0000000..b0c3a38 --- /dev/null +++ b/examples/Executor/config_feather_m0.h @@ -0,0 +1,36 @@ +/* + * Pano Controller Configuration File for Adafruit Feather M0 + * Supported: + * - Adafruit Feather M0 + * - Adafruit Feather M0 Bluefruit + */ + +// Bluetooth module. These internally connected on the Feather Bluefruit. +#define BLUEFRUIT_SPI_CS 8 +#define BLUEFRUIT_SPI_IRQ 7 +#define BLUEFRUIT_SPI_RST 4 + +// NRF24 radio +#define NRF24_CE A5 +#define NRF24_CSN 11 + +// Camera shutter controls +#define CAMERA_FOCUS 0 +#define CAMERA_SHUTTER 1 + +// Battery measurement pin R1/R2 +#define BATTERY A4 + +// MPU (accel/gyro) +#define MPU_INT 12 + +// Future devices +//#define COMPASS_DRDY 11 + +// Stepper drivers control +#define DIR 5 +#define VERT_STEP 6 +#define HORIZ_STEP 9 + +// this should be hooked up to nENABLE on both drivers +#define nENABLE 10 diff --git a/examples/Executor/config_teensy.h b/examples/Executor/config_teensy.h new file mode 100644 index 0000000..ffe3d8e --- /dev/null +++ b/examples/Executor/config_teensy.h @@ -0,0 +1,41 @@ +/* + * Pano Controller Configuration File for Teensy boards from PJRC + * Supported: + * - Teensy LC + * - Teensy 3.1 + * - Teensy 3.2 + */ + +// Bluetooth module SPI +#define BLUEFRUIT_SPI_CS 8 +#define BLUEFRUIT_SPI_IRQ 7 +#define BLUEFRUIT_SPI_RST 4 // Optional but recommended, set to -1 if unused + +#define SPI_MOSI 11 +#define SPI_MISO 12 +#define SPI_SCK 13 + +// NRF24 radio +#define NRF24_CE A6 +#define NRF24_CSN A7 + +// Camera shutter controls +#define CAMERA_FOCUS 0 +#define CAMERA_SHUTTER 1 + +// Battery measurement pin R1/R2 +#define BATTERY A0 + +// MPU (accel/gyro) +#define MPU_INT 2 + +// Future devices +//#define COMPASS_DRDY 4 + +// Stepper drivers control +#define DIR 5 +#define VERT_STEP 6 +#define HORIZ_STEP 9 + +// this should be hooked up to nENABLE on both drivers +#define nENABLE 10 diff --git a/examples/Navigator/Navigator.ino b/examples/Navigator/Navigator.ino new file mode 100644 index 0000000..f0badd9 --- /dev/null +++ b/examples/Navigator/Navigator.ino @@ -0,0 +1,419 @@ +/* + * Pano Controller for Arduino project + * + * Copyright (C)2015,2016 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#include +#include +#include +#include "config.h" +#include "pano.h" +#include "camera.h" +#include "hid.h" +#include "joystick.h" +#include "remote.h" +#include "menu.h" +#include "display.h" +#include "radio.h" +#include "protocol.h" + +// these variables are modified by the menu +PanoSettings settings; + +// This is the panorama engine state +// should technically be in the Pano object +PanoState state; + +static Display display(OLED_RESET); +static Radio radio(NRF24_CE, NRF24_CSN); +static Command comm(radio); + +static Camera* camera; +static Joystick* joystick; +static Remote* remote; +// HID (Human Interface Device) Combined joystick+remote +static AllHID* hid; +static PanoSetup* pano; +static Menu* menu; + +void setup() { + Serial.begin(38400); + delay(1000); // wait for serial + + display.begin(SSD1306_SWITCHCAPVCC, DISPLAY_I2C_ADDRESS, false); + display.setRotation(2); + display.clearDisplay(); + display.setTextCursor(0,0); + display.setTextColor(WHITE); + display.setTextSize(TEXT_SIZE); + + radio.begin(); + + camera = new Camera(PIN_UNCONNECTED, PIN_UNCONNECTED); + joystick = new Joystick(JOYSTICK_X, JOYSTICK_Y, JOYSTICK_SW, CANCEL_PIN); + remote = new Remote(REMOTE_IN); + // HID (Human Interface Device) Combined joystick+remote + hid = new AllHID(2, new HID* const[2] {joystick, remote}); + + pano = new PanoSetup(*camera); + + menu = getMainMenu(settings); +} + +/* + * Add a status overlay (currently battery and radio status) + */ +void displayStatusOverlay(void){ + + // motors active + if (state.motors_on || state.running){ + display.setTextCursor(0,14); + display.print("*"); + } + + // show a character indicating radio is connected + display.setTextCursor(0,15); + if (radio.connected){ + display.print("^"); + } else { + display.print("X"); + } + + if (!radio.connected){ + return; + } + + // Print battery voltage at cursor, format is #.#V (4 chars) + display.setTextCursor(0, 16); + if (state.battery){ + // poor attempt at blinking + if (state.battery < 0 && millis() & 1024){ + display.setTextColor(BLACK, WHITE); + } + display.printf("%2d.%dV", abs(state.battery)/1000, (abs(state.battery) % 1000)/100); + display.setTextColor(WHITE, BLACK); + } +} + +/* + * Display current panorama status (photo index, etc) + */ +void displayPanoStatus(bool complete){ + display.clearDisplay(); + display.setTextCursor(0,0); + int photos = pano->getHorizShots() * pano->getVertShots(); + + if (state.running){ + display.printf("# %d of %d\n", pano->position+1, photos); + display.printf("at %d x %d\n", 1+pano->getCurRow(), 1+pano->getCurCol()); + } else { + display.printf("%d photos\n\n", photos); + } + display.printf("grid %d x %d \n", pano->getVertShots(), pano->getHorizShots()); + + float horiz_fov = camera->getHorizFOV(); + float vert_fov = camera->getVertFOV(); + display.setTextCursor(3,0); + display.printf("Lens: %dmm\n", settings.focal); + display.printf(" %d.%d x %d.%d\n", + int(horiz_fov), round(10*(horiz_fov-int(horiz_fov))), + int(vert_fov), round(10*(vert_fov-int(vert_fov)))); + display.printf("Pano %d x %d deg\n", pano->horiz_fov, pano->vert_fov); + + displayStatusOverlay(); + + display.setTextCursor(6, 0); + if (state.position + 1 < photos){ + display.printf("%d minutes ", pano->getTimeLeft()/60); + } + + // steady delay / shake indicator + if (pano->steady_delay_avg > 500){ + display.setTextCursor(6, 16); + display.printf("%2ds ", (pano->steady_delay_avg+500)/1000); + display.printf((pano->steady_delay_avg < 8000) ? "\x12" : "!"); + } + + // progress bar + if (state.running){ + display.setTextCursor(7, 0); + for (int i=(pano->position+1) * DISPLAY_COLS / photos; i > 0; i--){ + display.print('\xdb'); + } + } + if (complete){ + display.display(); + } +} + +/* + * Display the four navigation arrows, cancel and OK buttons. + */ +void displayArrows(){ + display.setTextCursor(3, 0); + display.println(" \x1e \n" + " [X] \x11+\x10 [OK]\n" + " \x1f "); + display.display(); +} + +/* + * Let user move the camera while optionally recording position + * @param msg: title string + * @param horiz: pointer to store horizontal movement + * @param vert: pointer to store vertical movement + * if both horiz and vert are NULL then the head moves freely around + */ +bool positionCamera(const char *msg, settings_t *horiz, settings_t *vert){ + move_t move; + int step = 1; + + if (horiz || vert){ + comm.setHome(); + } + + display.clearDisplay(); + display.setTextCursor(0,0); + display.println(msg); + displayArrows(); + + while (true){ + comm.getState(state, setPanoParams); + move.horiz_offset = state.horiz_offset; + move.vert_offset = state.vert_offset; + + hid->read(); + if (hid->isLastEventOk() || hid->isLastEventCancel()) break; + + move.horiz_rpm = DYNAMIC_HORIZ_RPM(camera->getHorizFOV()); + move.horiz_move = joystick->getPositionX() * step; + if (move.horiz_move == 0){ + if (hid->isLastEventRight()) move.horiz_move = step; + if (hid->isLastEventLeft()) move.horiz_move = -step; + } + if (move.horiz_move && horiz){ + if (move.horiz_move < -move.horiz_offset){ + move.horiz_move = -move.horiz_offset; + } else if (abs(move.horiz_move + move.horiz_offset) + camera->getHorizFOV() > 360){ + move.horiz_move = 360 - camera->getHorizFOV() - move.horiz_offset; + } + } + + move.vert_rpm = DYNAMIC_VERT_RPM(camera->getVertFOV()); + move.vert_move = joystick->getPositionY() * step; + if (move.vert_move == 0){ + if (hid->isLastEventUp()) move.vert_move = step; + if (hid->isLastEventDown()) move.vert_move = -step; + move.vert_rpm /= 3; + } + if (move.vert_move && vert){ + if (move.vert_move > -move.vert_offset){ + move.vert_move = -move.vert_offset; + } else if (-(move.vert_move + move.vert_offset) + camera->getVertFOV() > 180){ + move.vert_move = -(180 - camera->getVertFOV() + move.vert_offset); + } + } + + if (vert){ + pano->setFOV((horiz) ? abs(move.horiz_offset) + camera->getHorizFOV() : 360, + abs(move.vert_offset) + camera->getVertFOV()); + pano->computeGrid(); + displayPanoStatus(true); + } + + if (move.horiz_move || move.vert_move){ + comm.freeMove(move); + move.horiz_offset = move.horiz_offset + move.horiz_move; + move.vert_offset = move.vert_offset + move.vert_move; + } + } + + if (horiz || vert){ + if (horiz){ + *horiz = abs(move.horiz_offset) + camera->getHorizFOV(); + } + if (vert){ + *vert = abs(move.vert_offset) + camera->getVertFOV(); + } + + comm.goHome(); + } + + return (hid->isLastEventOk()); +} + +/* + * Follow panorama execution from start to finish. + * Button click interrupts. + */ +void followPano(void){ + bool manualMode = (state.paused); + + while (state.running){ + displayPanoStatus(false); + + if (manualMode){ + displayArrows(); + } else { + display.display(); + } + + comm.getState(state, setPanoParams); + + if (!hid->read()){ + continue; + } + + // button was clicked mid-pano + if (!manualMode){ + comm.pausePano(state); + manualMode = true; + hid->clear(1000); + continue; + } + + if (hid->isLastEventOk()) comm.shutter(state); + else if (hid->isLastEventCancel()) comm.cancelPano(state); + else if (hid->isLastEventLeft()) comm.gridMoveLeft(state); + else if (hid->isLastEventRight()) comm.gridMoveRight(state); + else if (hid->isLastEventUp()) comm.gridMoveUp(state); + else if (hid->isLastEventDown()) comm.gridMoveDown(state); + }; + + displayPanoStatus(true); +} + +/* + * Update common camera and pano settings from external vars + */ +void setPanoParams(void){ + comm.sendConfig(settings); + camera->setAspect(settings.aspect); + camera->setFocalLength(settings.focal); + pano->setShutter(settings.shutter, settings.pre_shutter, settings.post_wait, settings.long_pulse); + pano->setShots(settings.shots); + pano->setFOV(settings.horiz, settings.vert); + pano->computeGrid(); + pano->position = state.position; + pano->steady_delay_avg = state.steady_delay_avg; +} + +/* + * Menu callback invoked by selecting "Repeat" + */ +int onRepeat(int __){ + settings.motors_on = true; + setPanoParams(); + if (!positionCamera("Adjust start pos\nSet exposure & focus", NULL, NULL)){ + return false; + } + menu->sync(); + + hid->clear(4000); + + comm.startPano(state); + return __; +} + +/* + * Menu callback invoked by selecting "Start" + */ +int onStart(int __){ + settings_t horiz, vert; + settings.motors_on = true; + + // set panorama FOV + if (!positionCamera("Set Top Left", NULL, NULL) || + !positionCamera("Set Bottom Right", &horiz, &vert)){ + return false; + } + + settings.horiz = horiz; + settings.vert = vert; + pano->setFOV(horiz, vert); + + return onRepeat(__); +} + +/* + * Menu callback invoked by selecting "360 Pano" + */ +int on360(int __){ + settings_t horiz = 360, vert; + settings.motors_on = true; + + if (!positionCamera("Set Top", NULL, NULL) || + !positionCamera("Set Bottom", NULL, &vert)){ + return false; + } + + settings.horiz = horiz; + settings.vert = vert; + pano->setFOV(horiz, vert); + + return onRepeat(__); +} + +/* + * Menu callback for displaying last pano info + */ +int onPanoInfo(int __){ + setPanoParams(); + displayPanoStatus(true); + hid->waitAnyKey(); + return __; +} + +/* + * Menu callback for testing camera shutter + */ +int onTestShutter(int __){ + comm.shutter(state); + return __; +} + +/* + * Menu callback for displaying About... info + */ +int onAboutPanoController(int __){ + display.clearDisplay(); + display.setTextCursor(2, 0); + display.print("Pano Controller\n\n" + "Built " __DATE__ "\n" + __TIME__ "\n"); + display.display(); + hid->waitAnyKey(); + return __; +} + +void onMenuLoop(bool updated){ + static unsigned next_update_time = 0; + if (updated || millis() >= next_update_time){ + settings.motors_on = settings.motors_enable; + comm.sendConfig(settings); + next_update_time = millis() + 4000; + } + displayStatusOverlay(); + display.invertDisplay(settings.display_invert); +} + +void loop() { + bool stateChanged; + + stateChanged = comm.getState(state, setPanoParams); + + /* + * Render menu or state + */ + if (state.running){ + followPano(); + } else if (stateChanged){ + setPanoParams(); + } + displayMenu(*menu, display, DISPLAY_ROWS, *hid, onMenuLoop); + + yield(); +} diff --git a/examples/Navigator/config.h b/examples/Navigator/config.h new file mode 100644 index 0000000..4561934 --- /dev/null +++ b/examples/Navigator/config.h @@ -0,0 +1,25 @@ +/* + * Pano Controller Navigator Master Configuration File + */ + +// Address of I2C OLED display. If screen looks scaled edit Adafruit_SSD1306.h +// and pick SSD1306_128_64 or SSD1306_128_32 that matches display type. +#define DISPLAY_I2C_ADDRESS 0x3C +#define OLED_RESET 12 +#define TEXT_SIZE 1 +#define DISPLAY_ROWS SSD1306_LCDHEIGHT/8/TEXT_SIZE +#define DISPLAY_COLS SSD1306_LCDWIDTH/6/TEXT_SIZE + +// board-specific pin settings +#if defined(ARDUINO_ARCH_SAMD) && defined(ARDUINO_SAMD_FEATHER_M0) +#include "config_feather_m0.h" + +#elif defined(__arm__) && defined(CORE_TEENSY) +#include "config_teensy.h" + +#elif defined(__AVR__) +#error "AVR is not supported" + +#else +#error "Unsupported board" +#endif diff --git a/examples/Navigator/config_feather_m0.h b/examples/Navigator/config_feather_m0.h new file mode 100644 index 0000000..98c0301 --- /dev/null +++ b/examples/Navigator/config_feather_m0.h @@ -0,0 +1,24 @@ +/* + * Pano Controller Navigator Configuration File for Adafruit Feather M0 + * Supported: + * - Adafruit Feather M0 + * - Adafruit Feather M0 Bluefruit + */ + +// Bluetooth module. These internally connected on the Feather Bluefruit. +#define BLUEFRUIT_SPI_CS 8 +#define BLUEFRUIT_SPI_IRQ 7 +#define BLUEFRUIT_SPI_RST 4 + +// Joystick inputs +#define JOYSTICK_X A3 +#define JOYSTICK_Y A2 +#define JOYSTICK_SW A1 + +// IR remote is not supported on Feather M0 +// https://github.com/z3t0/Arduino-IRremote/issues/274 +#define REMOTE_IN -1 + +// NRF24 radio +#define NRF24_CE A5 +#define NRF24_CSN 11 diff --git a/examples/Navigator/config_teensy.h b/examples/Navigator/config_teensy.h new file mode 100644 index 0000000..db7a27c --- /dev/null +++ b/examples/Navigator/config_teensy.h @@ -0,0 +1,29 @@ +/* + * Pano Controller Navigator Configuration File for Teensy boards from PJRC + * Supported: + * - Teensy LC + * - Teensy 3.1 + * - Teensy 3.2 + */ + +// Bluetooth module SPI +#define BLUEFRUIT_SPI_CS 8 +#define BLUEFRUIT_SPI_IRQ 7 +#define BLUEFRUIT_SPI_RST 4 // Optional but recommended, set to -1 if unused + +#define SPI_MOSI 11 +#define SPI_MISO 12 +#define SPI_SCK 13 + +// Joystick inputs +#define JOYSTICK_X A3 +#define JOYSTICK_Y A2 +#define JOYSTICK_SW A1 +#define CANCEL_PIN A8 + +// IR remote +#define REMOTE_IN 3 + +// NRF24 radio +#define NRF24_CE A6 +#define NRF24_CSN A7 diff --git a/pano-controller.ino b/examples/PanoController/PanoController.ino similarity index 86% rename from pano-controller.ino rename to examples/PanoController/PanoController.ino index f168d9a..277e386 100644 --- a/pano-controller.ino +++ b/examples/PanoController/PanoController.ino @@ -23,9 +23,11 @@ #include "mpu.h" // these variables are modified by the menu -volatile int focal, shutter, pre_shutter, post_wait, long_pulse, - orientation, aspect, shots, motors_enable, display_invert, - horiz, vert, running; +PanoSettings settings; + +// This is the panorama engine state +// should technically be in the Pano object +PanoState state; static Display display(OLED_RESET); Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST); @@ -64,7 +66,7 @@ void setup() { display.setTextSize(TEXT_SIZE); camera = new Camera(CAMERA_FOCUS, CAMERA_SHUTTER); - joystick = new Joystick(JOYSTICK_SW, JOYSTICK_X, JOYSTICK_Y); + joystick = new Joystick(JOYSTICK_X, JOYSTICK_Y, JOYSTICK_SW, CANCEL_PIN); remote = new Remote(REMOTE_IN); ble_remote = new BLERemote(ble); // HID (Human Interface Device) Combined joystick+remote @@ -88,11 +90,12 @@ void setup() { analogReadAveraging(32); #endif - menu = getMainMenu(); + menu = getMainMenu(settings); } -int readBattery(void){ - return map(analogRead(BATTERY), 0, (1<<10)-1, 0, BATT_RANGE); + +void readBattery(void){ + state.battery = map(analogRead(BATTERY), 0, (1<<10)-1, 0, BATT_RANGE); } /* @@ -101,13 +104,12 @@ int readBattery(void){ void displayStatusOverlay(void){ // Print battery voltage at cursor, format is #.#V (4 chars) - int battmV = readBattery(); display.setTextCursor(0, 16); // poor attempt at blinking - if (battmV < LOW_BATTERY && millis() & 1024){ + if (state.battery < LOW_BATTERY && millis() & 1024){ display.setTextColor(BLACK, WHITE); } - display.printf("%2d.%dV", battmV/1000, (battmV % 1000)/100); + display.printf("%2d.%dV", state.battery/1000, (state.battery % 1000)/100); display.setTextColor(WHITE, BLACK); // show a character indicating bluetooth is connected @@ -121,6 +123,7 @@ void displayStatusOverlay(void){ * Display current panorama status (photo index, etc) */ void displayPanoStatus(void){ + readBattery(); display.clearDisplay(); display.setTextCursor(0,0); @@ -155,9 +158,10 @@ void displayProgress(void){ void displayPanoInfo(void){ float horiz_fov = camera->getHorizFOV(); float vert_fov = camera->getVertFOV(); + readBattery(); display.clearDisplay(); display.setTextCursor(0,0); - display.printf("Lens: %dmm\n", focal); + display.printf("Lens: %dmm\n", settings.focal); display.printf(" %d.%d x %d.%d\n", int(horiz_fov), round(10*(horiz_fov-int(horiz_fov))), int(vert_fov), round(10*(vert_fov-int(vert_fov)))); @@ -193,7 +197,7 @@ void displayArrows(){ * @param horiz: pointer to store horizontal movement * @param vert: pointer to store vertical movement */ -bool positionCamera(const char *msg, volatile int *horiz, volatile int *vert){ +bool positionCamera(const char *msg, settings_t *horiz, settings_t *vert){ int pos_x, pos_y; int horiz_rpm, vert_rpm; @@ -258,6 +262,7 @@ bool positionCamera(const char *msg, volatile int *horiz, volatile int *vert){ display.setTextCursor(6, 0); displayPanoSize(); display.printf("FOV %d x %d ", pano->horiz_fov, pano->vert_fov); + readBattery(); displayStatusOverlay(); display.display(); } @@ -290,21 +295,21 @@ void executePano(void){ hid->clear(4000); pano->start(); - while (running){ + while (state.running){ displayPanoStatus(); if (!pano->position){ delay(2000); } - if (shutter > 0){ + if (settings.shutter > 0){ pano->shutter(); } - if (shutter == 0 || hid->read()){ + if (settings.shutter == 0 || hid->read()){ hid->clear(1000); // button was clicked mid-pano or we are in manual shutter mode displayPanoStatus(); displayArrows(); - while (running){ + while (state.running){ if (!hid->read()) continue; else if (hid->isLastEventLeft()) pano->prev(); else if (hid->isLastEventRight()) pano->next(); @@ -312,17 +317,17 @@ void executePano(void){ else if (hid->isLastEventDown()) pano->moveTo(pano->getCurRow() + 1, pano->getCurCol()); else if (hid->isLastEventOk()) break; else if (hid->isLastEventCancel()){ - running = false; + state.running = false; break; } displayPanoStatus(); displayArrows(); } } - running = running && pano->next(); + state.running = state.running && pano->next(); }; - running = false; + state.running = false; displayPanoStatus(); pano->end(); @@ -335,10 +340,10 @@ void executePano(void){ */ void setPanoParams(void){ menu->cancel(); - camera->setAspect(aspect); - camera->setFocalLength(focal); - pano->setShutter(shutter, pre_shutter, post_wait, long_pulse); - pano->setShots(shots); + camera->setAspect(settings.aspect); + camera->setFocalLength(settings.focal); + pano->setShutter(settings.shutter, settings.pre_shutter, settings.post_wait, settings.long_pulse); + pano->setShots(settings.shots); } /* @@ -349,15 +354,15 @@ int onStart(int __){ // set panorama FOV if (!positionCamera("Set Top Left", NULL, NULL) || - !positionCamera("Set Bottom Right", &horiz, &vert)){ + !positionCamera("Set Bottom Right", &settings.horiz, &settings.vert)){ return false; } - pano->setFOV(horiz, vert); + pano->setFOV(settings.horiz, settings.vert); if (!positionCamera("Adjust start pos\nSet exposure & focus", NULL, NULL)){ return false; } - running = true; + state.running = true; menu->sync(); executePano(); return __; @@ -369,11 +374,11 @@ int onStart(int __){ int onRepeat(int __){ setPanoParams(); - pano->setFOV(horiz, vert); + pano->setFOV(settings.horiz, settings.vert); if (!positionCamera("Adjust start pos\nSet exposure & focus", NULL, NULL)){ return false; } - running = true; + state.running = true; menu->sync(); executePano(); return __; @@ -386,17 +391,17 @@ int on360(int __){ setPanoParams(); // set panorama FOV - horiz = 360; + settings.horiz = 360; if (!positionCamera("Set Top", NULL, NULL) || - !positionCamera("Set Bottom", NULL, &vert)){ + !positionCamera("Set Bottom", NULL, &settings.vert)){ return false; } - pano->setFOV(horiz, vert); + pano->setFOV(settings.horiz, settings.vert); if (!positionCamera("Adjust start pos\nSet exposure & focus", NULL, NULL)){ return false; } - running = true; + state.running = true; menu->sync(); executePano(); return __; @@ -407,7 +412,7 @@ int on360(int __){ */ int onPanoInfo(int __){ setPanoParams(); - pano->setFOV(horiz, vert); + pano->setFOV(settings.horiz, settings.vert); pano->computeGrid(); displayPanoInfo(); hid->waitAnyKey(); @@ -437,10 +442,10 @@ int onAboutPanoController(int __){ return __; } -void onMenuLoop(void){ +void onMenuLoop(bool updated){ displayStatusOverlay(); - display.invertDisplay(display_invert); - pano->motorsEnable(motors_enable); + display.invertDisplay(settings.display_invert); + pano->motorsEnable(settings.motors_enable); } void loop() { diff --git a/examples/PanoController/config.h b/examples/PanoController/config.h new file mode 100644 index 0000000..97478f4 --- /dev/null +++ b/examples/PanoController/config.h @@ -0,0 +1,40 @@ +/* + * Pano Controller Master Configuration File + */ + +// Address of I2C OLED display. If screen looks scaled edit Adafruit_SSD1306.h +// and pick SSD1306_128_64 or SSD1306_128_32 that matches display type. +#define DISPLAY_I2C_ADDRESS 0x3C +#define OLED_RESET 12 +#define TEXT_SIZE 1 +#define DISPLAY_ROWS SSD1306_LCDHEIGHT/8/TEXT_SIZE +#define DISPLAY_COLS SSD1306_LCDWIDTH/6/TEXT_SIZE + +// Battery monitoring settings +#define VCC 3300 +#define LOW_BATTERY 7000 +// R1/R2 is the voltage divisor in Ω (GND-R1-A0-R2-Vin) +// measure resistors and enter actual values for a more accurate voltage +#define BATT_R1 9980 +#define BATT_R2 46500 +#define BATT_RANGE (VCC * (BATT_R1 + BATT_R2) / BATT_R1) + +// MPU (accel/gyro) +#define MPU_I2C_ADDRESS 0x68 + +// Stepper motors steps per revolution +#define MOTOR_STEPS 200 + +// board-specific pin settings +#if defined(ARDUINO_ARCH_SAMD) && defined(ARDUINO_SAMD_FEATHER_M0) +#include "config_feather_m0.h" + +#elif defined(__arm__) && defined(CORE_TEENSY) +#include "config_teensy.h" + +#elif defined(__AVR__) +#error "AVR is not supported" + +#else +#error "Unsupported board" +#endif diff --git a/config_feather_m0.h b/examples/PanoController/config_feather_m0.h similarity index 97% rename from config_feather_m0.h rename to examples/PanoController/config_feather_m0.h index 1d23e70..de221e7 100644 --- a/config_feather_m0.h +++ b/examples/PanoController/config_feather_m0.h @@ -21,6 +21,7 @@ #define JOYSTICK_X A3 #define JOYSTICK_Y A2 #define JOYSTICK_SW A1 +#define CANCEL_PIN A0 // IR remote is not supported on Feather M0 // https://github.com/z3t0/Arduino-IRremote/issues/274 diff --git a/config_teensy.h b/examples/PanoController/config_teensy.h similarity index 97% rename from config_teensy.h rename to examples/PanoController/config_teensy.h index 3b60f59..eb01a7c 100644 --- a/config_teensy.h +++ b/examples/PanoController/config_teensy.h @@ -26,6 +26,7 @@ #define JOYSTICK_X A3 #define JOYSTICK_Y A2 #define JOYSTICK_SW A1 +#define CANCEL_PIN A8 // IR remote #define REMOTE_IN 3 diff --git a/images/executor.jpg b/images/executor.jpg new file mode 100644 index 0000000..9c4f5f2 Binary files /dev/null and b/images/executor.jpg differ diff --git a/images/navigator.jpg b/images/navigator.jpg new file mode 100644 index 0000000..38a75de Binary files /dev/null and b/images/navigator.jpg differ diff --git a/images/pano-info.jpg b/images/pano-info.jpg index 33969b9..bf0a6da 100644 Binary files a/images/pano-info.jpg and b/images/pano-info.jpg differ diff --git a/images/prototype.jpg b/images/prototype.jpg index e4cc0a8..e88f752 100644 Binary files a/images/prototype.jpg and b/images/prototype.jpg differ diff --git a/keywords.txt b/keywords.txt new file mode 100644 index 0000000..f92faee --- /dev/null +++ b/keywords.txt @@ -0,0 +1,8 @@ +Pano KEYWORD1 +Menu KEYWORD1 +Display KEYWORD1 +Camera KEYWORD1 +Joystick KEYWORD1 +BLERemote KEYWORD1 +AllHID KEYWORD1 +MPU KEYWORD1 diff --git a/library.properties b/library.properties new file mode 100644 index 0000000..20c6f6b --- /dev/null +++ b/library.properties @@ -0,0 +1,11 @@ +name=PanoController +version=1.4.0 +author=Laurentiu Badea +maintainer=Laurentiu Badea +sentence=Controller for high-resolution panoramic photography. +paragraph=Move a camera to take a mosaic of images at high zoom, or an almost complete 360 photo sphere. A physical camera platform with two stepper motors is required. +category=Other +url=https://github.com/laurb9/StepperDriver +architectures=* +dot_a_linkage=false +includes=camera.h,display.h,menu.h,mpu.h,pano.h,hid.h,joystick.h,remote.h,ble_remote.h diff --git a/ble_remote.cpp b/src/ble_remote.cpp similarity index 100% rename from ble_remote.cpp rename to src/ble_remote.cpp diff --git a/ble_remote.h b/src/ble_remote.h similarity index 100% rename from ble_remote.h rename to src/ble_remote.h diff --git a/camera.cpp b/src/camera.cpp similarity index 93% rename from camera.cpp rename to src/camera.cpp index 2631e68..6b63e6a 100644 --- a/camera.cpp +++ b/src/camera.cpp @@ -19,20 +19,18 @@ Camera::Camera(int focus_pin, int shutter_pin) :focus_pin(focus_pin), shutter_pin(shutter_pin) { - pinMode(focus_pin, OUTPUT); - pinMode(shutter_pin, OUTPUT); - digitalWrite(focus_pin, HIGH); - digitalWrite(shutter_pin, HIGH); setFocalLength(24); } void Camera::shutter(int delay_ms, bool long_pulse){ int shutter_pulse = (long_pulse) ? delay_ms : SHUTTER_PULSE; + pinMode(focus_pin, OUTPUT); + pinMode(shutter_pin, OUTPUT); digitalWrite(focus_pin, LOW); digitalWrite(shutter_pin, LOW); delay(min(shutter_pulse, delay_ms)); - digitalWrite(focus_pin, HIGH); - digitalWrite(shutter_pin, HIGH); + pinMode(focus_pin, INPUT); + pinMode(shutter_pin, INPUT); if (delay_ms > shutter_pulse){ delay(delay_ms - shutter_pulse); } diff --git a/camera.h b/src/camera.h similarity index 100% rename from camera.h rename to src/camera.h diff --git a/display.cpp b/src/display.cpp similarity index 100% rename from display.cpp rename to src/display.cpp diff --git a/display.h b/src/display.h similarity index 100% rename from display.h rename to src/display.h diff --git a/eeprom.h b/src/eeprom.h similarity index 100% rename from eeprom.h rename to src/eeprom.h diff --git a/hid.h b/src/hid.h similarity index 100% rename from hid.h rename to src/hid.h diff --git a/joystick.cpp b/src/joystick.cpp similarity index 67% rename from joystick.cpp rename to src/joystick.cpp index d08f76c..86366fa 100644 --- a/joystick.cpp +++ b/src/joystick.cpp @@ -13,29 +13,38 @@ /* * Interrupt handler triggered by joystick button click */ -volatile static int button_clicked = false; +volatile static uint8_t button_clicked = false; void button_click(){ button_clicked = true; } +volatile static uint8_t cancel_clicked = false; +void cancel_click(){ + cancel_clicked = true; +} -Joystick::Joystick(int sw_pin, int x_pin, int y_pin) -:sw_pin(sw_pin), - x_pin(x_pin), - y_pin(y_pin) +Joystick::Joystick(int x_pin, int y_pin, int sw_pin, int cancel_pin) +:x_pin(x_pin), + y_pin(y_pin), + sw_pin(sw_pin), + cancel_pin(cancel_pin) { - pinMode(sw_pin, INPUT_PULLUP); pinMode(x_pin, INPUT); pinMode(y_pin, INPUT); + pinMode(sw_pin, INPUT_PULLUP); + pinMode(cancel_pin, INPUT_PULLUP); sw_state = HIGH; + cancel_state = HIGH; x_state = 0; y_state = 0; read(); attachInterrupt(digitalPinToInterrupt(sw_pin), button_click, FALLING); + attachInterrupt(digitalPinToInterrupt(cancel_pin), cancel_click, FALLING); } Joystick::~Joystick(void){ // clean up detachInterrupt(digitalPinToInterrupt(sw_pin)); + detachInterrupt(digitalPinToInterrupt(cancel_pin)); } bool Joystick::isConnected(void){ @@ -49,6 +58,26 @@ bool Joystick::isConnected(void){ return connected; } +/* + * + */ +static bool read_button_state(int& state, volatile uint8_t& clicked, int pin){ + int current_state; + bool active = false; + current_state = (clicked) ? LOW : digitalRead(pin); + if (current_state != state){ + state = current_state; + if (state == LOW){ + active = true; + for (int i=50; i && digitalRead(pin) == LOW; i--){ + delay(10); + } + } + clicked = false; + } + return active; +} + unsigned Joystick::read(void){ unsigned event = EVENT_NONE; int current_state; @@ -61,17 +90,12 @@ unsigned Joystick::read(void){ return event; } - // read click switch - current_state = (button_clicked) ? LOW : digitalRead(sw_pin); - if (current_state != sw_state){ - sw_state = current_state; - if (sw_state == LOW){ - event |= EVENT_OK; - for (int i=50; i && digitalRead(sw_pin) == LOW; i--){ - delay(10); - }; - } - button_clicked = false; + // read click switches + if (read_button_state(sw_state, button_clicked, sw_pin)){ + event |= EVENT_OK; + } + if (read_button_state(cancel_state, cancel_clicked, cancel_pin)){ + event |= EVENT_CANCEL; } // read X position diff --git a/joystick.h b/src/joystick.h similarity index 82% rename from joystick.h rename to src/joystick.h index 79c5404..c3cad3b 100644 --- a/joystick.h +++ b/src/joystick.h @@ -15,8 +15,8 @@ class Joystick : public HID { private: static const int sensitivity = 7; // lower is higher :) protected: - int sw_pin, x_pin, y_pin; - int sw_state = 0, x_state = 0, y_state = 0; + int x_pin, y_pin, sw_pin, cancel_pin; + int x_state = 0, y_state = 0, sw_state = 0, cancel_state = 0; int last_read; bool connected = false; public: @@ -24,7 +24,7 @@ class Joystick : public HID { int autorepeat_delay; int autorepeat_repeat; unsigned read(void) override; - Joystick(int sw_pin, int x_pin, int y_pin); + Joystick(int x_pin, int y_pin, int sw_pin, int cancel_pin); ~Joystick(void); int getPositionX(bool if_connected=true); int getPositionY(bool if_connected=true); diff --git a/menu.cpp b/src/menu.cpp similarity index 91% rename from menu.cpp rename to src/menu.cpp index 210a3d1..5b6b7c1 100644 --- a/menu.cpp +++ b/src/menu.cpp @@ -59,7 +59,7 @@ ActionItem::ActionItem(const char *description, int(*onselect)(int)) /* * MultiSelect: share functionality among the other menu classes */ -MultiSelect::MultiSelect(const char *description, volatile int *value, int default_val, int eeprom, int(*onselect)(int)) +MultiSelect::MultiSelect(const char *description, settings_t *value, int default_val, int eeprom, int(*onselect)(int)) :BaseMenu(description, onselect), value(value), default_val(default_val), @@ -126,12 +126,16 @@ int MultiSelect::render(DISPLAY_DEVICE display, int rows){ /* * RangeSelector: a number with up/down controls */ -RangeSelector::RangeSelector(const char *description, volatile int *value, int default_val, int eeprom, int(*onselect)(int), +RangeSelector::RangeSelector(const char *description, settings_t *value, int default_val, int eeprom, int(*onselect)(int), int min_val, int max_val, int step) :MultiSelect(description, value, default_val, eeprom, onselect), min_val(min_val), max_val(max_val), step(step) { pointer = this->default_val; + if (step > 1){ + // pointer should be an integer number of steps from min_val + pointer = pointer - (pointer - min_val) % step; + } pos = pointer; }; @@ -151,7 +155,8 @@ void RangeSelector::select(void){ cancel(); } void RangeSelector::sync(void){ - pointer = *value % step; + // pointer should be an integer number of steps from min_val + pointer = *value - (*value - min_val) % step; if (pointer > max_val) pointer = max_val; if (pointer < min_val) pointer = min_val; pos = pointer; @@ -180,7 +185,7 @@ int RangeSelector::render(DISPLAY_DEVICE display, int rows){ /* * ListSelector: list of numeric options */ -ListSelector::ListSelector(const char *description, volatile int *value, int default_val, int eeprom, int(*onselect)(int), +ListSelector::ListSelector(const char *description, settings_t *value, int default_val, int eeprom, int(*onselect)(int), int count, const int values[]) :MultiSelect(description, value, default_val, eeprom, onselect), values(values) @@ -237,7 +242,7 @@ int ListSelector::render(DISPLAY_DEVICE display, int rows){ /* * NamedListSelector: list of named numeric options */ -NamedListSelector::NamedListSelector(const char *description, volatile int *value, int default_val, int eeprom, int(*onselect)(int), +NamedListSelector::NamedListSelector(const char *description, settings_t *value, int default_val, int eeprom, int(*onselect)(int), int count, const char * const names[], const int values[]) :ListSelector(description, value, default_val, eeprom, onselect, count, values), names(names) @@ -379,7 +384,7 @@ int Menu::render(DISPLAY_DEVICE display, int rows){ * This must be called in a loop. */ void displayMenu(Menu& menu, DISPLAY_DEVICE display, const int rows, - AllHID& hid, void(*onMenuLoop)(void)){ + AllHID& hid, void(*onMenuLoop)(bool)){ static int last_event_timestamp = 0; if (!hid.read() && last_event_timestamp){ @@ -387,7 +392,7 @@ void displayMenu(Menu& menu, DISPLAY_DEVICE display, const int rows, display.clearDisplay(); display.display(); } else { - if (onMenuLoop) onMenuLoop(); + if (onMenuLoop) onMenuLoop(false); display.display(); } return; @@ -403,7 +408,7 @@ void displayMenu(Menu& menu, DISPLAY_DEVICE display, const int rows, display.clearDisplay(); display.setTextCursor(0,0); menu.render(display, rows); - if (onMenuLoop) onMenuLoop(); + if (onMenuLoop) onMenuLoop(true); display.display(); delay(100); } diff --git a/menu.h b/src/menu.h similarity index 86% rename from menu.h rename to src/menu.h index adbaf1e..2ae360d 100644 --- a/menu.h +++ b/src/menu.h @@ -10,6 +10,7 @@ #define MENU_H_ #include "display.h" #include "hid.h" +#include "pano_settings.h" #define DISPLAY_DEVICE Display& @@ -48,7 +49,7 @@ class BaseMenu { class MultiSelect : public BaseMenu { protected: - volatile int *value = NULL; // store current option value here + settings_t *value = NULL; // store current option value here int default_val = 0; // default option value int eeprom = 0; // EEPROM save location (0 = no save) int pointer = 0; // currently selected entry @@ -56,7 +57,7 @@ class MultiSelect : public BaseMenu { int count = 0; // number of entries int calc_start(int rows); // helper method - MultiSelect(const char *description, volatile int *value, int default_val, int eeprom, int(*onselect)(int)); + MultiSelect(const char *description, settings_t *value, int default_val, int eeprom, int(*onselect)(int)); public: bool active = false; // flag indicating the menu is active @@ -86,7 +87,7 @@ class RangeSelector : public MultiSelect { static const ClassID class_id = CLASS_RANGE; virtual const ClassID getClassID(void){ return class_id; }; int min_val, max_val, step; - RangeSelector(const char *description, volatile int *value, int default_val, int eeprom, int(*onselect)(int), + RangeSelector(const char *description, settings_t *value, int default_val, int eeprom, int(*onselect)(int), int min_val, int max_val, int step); void next(void) override; void prev(void) override; @@ -103,7 +104,7 @@ class ListSelector : public MultiSelect { static const ClassID class_id = CLASS_LIST; virtual const ClassID getClassID(void){ return class_id; }; const int *values; - ListSelector(const char *description, volatile int *value, int default_val, int eeprom, int(*onselect)(int), + ListSelector(const char *description, settings_t *value, int default_val, int eeprom, int(*onselect)(int), int count, const int values[]); void select(void) override; void sync(void) override; @@ -118,7 +119,7 @@ class NamedListSelector : public ListSelector { static const ClassID class_id = CLASS_NAMES; virtual const ClassID getClassID(void){ return class_id; }; const char* const *names; - NamedListSelector(const char *description, volatile int *value, int default_val, int eeprom, int(*onselect)(int), + NamedListSelector(const char *description, settings_t *value, int default_val, int eeprom, int(*onselect)(int), int count, const char * const names[], const int values[]); int render(DISPLAY_DEVICE display, int rows) override; }; @@ -142,9 +143,9 @@ class Menu : public MultiSelect { int render(DISPLAY_DEVICE display, int rows) override; }; -Menu* getMainMenu(void); +Menu* getMainMenu(PanoSettings& settings); void displayMenu(Menu& menu, DISPLAY_DEVICE display, const int rows, - AllHID& hid, void(*onMenuLoop)(void)=NULL); + AllHID& hid, void(*onMenuLoop)(bool)=NULL); #endif /* MENU_H_ */ diff --git a/menus_definition.cpp b/src/menus_definition.cpp similarity index 63% rename from menus_definition.cpp rename to src/menus_definition.cpp index dec98d5..3e63b77 100644 --- a/menus_definition.cpp +++ b/src/menus_definition.cpp @@ -16,19 +16,6 @@ int onPanoInfo(int); int onTestShutter(int); int onAboutPanoController(int); -// variables updated directly by menus -extern volatile int horiz; -extern volatile int vert; -extern volatile int focal; -extern volatile int shutter; -extern volatile int pre_shutter; -extern volatile int post_wait; -extern volatile int long_pulse; -extern volatile int shots; -extern volatile int aspect; -extern volatile int motors_enable; -extern volatile int display_invert; - #define NO_EEPROM 0 #define USE_EEPROM assignEEPROM() @@ -43,7 +30,7 @@ int assignEEPROM(){ /* * Return the main menu object */ -Menu* getMainMenu(void){ +Menu* getMainMenu(PanoSettings& settings){ static Menu* menu = new Menu("Main Menu", 5, new BaseMenu* const[5] { @@ -52,37 +39,37 @@ Menu* getMainMenu(void){ new ActionItem("Repeat Last", onRepeat), new ActionItem("360 Pano", on360), new ActionItem("Last Pano Info", onPanoInfo), - new RangeSelector("Manual Horiz FOV", &horiz, 120, USE_EEPROM, NULL, 10, 360, 10), - new RangeSelector("Manual Vert FOV", &vert, 90, USE_EEPROM, NULL, 10, 180, 10) + new RangeSelector("Manual Horiz FOV", &settings.horiz, 120, USE_EEPROM, NULL, 10, 360, 10), + new RangeSelector("Manual Vert FOV", &settings.vert, 90, USE_EEPROM, NULL, 10, 180, 10) }), new Menu("Camera", 8, new BaseMenu* const[8] { - new ListSelector("Focal Len", &focal, 35, USE_EEPROM, NULL, 16, + new ListSelector("Focal Len", &settings.focal, 35, USE_EEPROM, NULL, 16, (const int[]){12, 14, 16, 20, 24, 28, 35, 50, 75, 105, 200, 300, 400, 450, 500, 600}), - new NamedListSelector("Shutter", &shutter, 10, USE_EEPROM, NULL, 11, + new NamedListSelector("Shutter", &settings.shutter, 10, USE_EEPROM, NULL, 11, (const char * const[]){"1/100", "1/50", "1/25", "1/10", "1/4", "0.5s", "1s", "2s", "4s", "8s", "MANUAL"}, (const int[]){10, 20, 40, 100, 250, 500, 1000, 2000, 4000, 8000, 0}), - new NamedListSelector("Delay", &pre_shutter, 100, USE_EEPROM, NULL, 6, + new NamedListSelector("Delay", &settings.pre_shutter, 100, USE_EEPROM, NULL, 6, (const char * const[]){"0.1s", "0.5s", "1s", "2s", "4s", "8s"}, (const int[]){100, 500, 1000, 2000, 4000, 8000}), - new NamedListSelector("Processing Wait", &post_wait, 100, USE_EEPROM, NULL, 7, + new NamedListSelector("Processing Wait", &settings.post_wait, 100, USE_EEPROM, NULL, 7, (const char * const[]){"0.1s", "0.25s", "0.5s", "1s", "2s", "4s", "8s"}, (const int[]){100, 250, 500, 1000, 2000, 4000, 8000}), - new NamedListSelector("Shutter Mode", &long_pulse, 0, USE_EEPROM, NULL, 2, + new NamedListSelector("Shutter Mode", &settings.long_pulse, 0, USE_EEPROM, NULL, 2, (const char * const[]){"Normal", "Cont Bkt"}, (const int[]){0, 1}), - new RangeSelector("Shots #", &shots, 1, USE_EEPROM, NULL, 1, 5, 1), - new NamedListSelector("Aspect", &aspect, 23, USE_EEPROM, NULL, 2, + new RangeSelector("Shots #", &settings.shots, 1, USE_EEPROM, NULL, 1, 5, 1), + new NamedListSelector("Aspect", &settings.aspect, 23, USE_EEPROM, NULL, 2, (const char * const[]){"P 2:3", "L 3:2"}, (const int[]){23, 32}), new ActionItem("Test Shutter", onTestShutter) }), - new NamedListSelector("Motors", &motors_enable, 0, NO_EEPROM, NULL, 2, + new NamedListSelector("Motors", &settings.motors_enable, 0, NO_EEPROM, NULL, 2, (const char * const[]){"On", "Off"}, (const int[]){1, 0}), - new NamedListSelector("Display", &display_invert, 0, NO_EEPROM, NULL, 2, + new NamedListSelector("Display", &settings.display_invert, 0, NO_EEPROM, NULL, 2, (const char * const[]){"Dark", "Bright"}, (const int[]){0, 1}), diff --git a/mpu.cpp b/src/mpu.cpp similarity index 100% rename from mpu.cpp rename to src/mpu.cpp diff --git a/mpu.h b/src/mpu.h similarity index 100% rename from mpu.h rename to src/mpu.h diff --git a/pano.cpp b/src/pano.cpp similarity index 84% rename from pano.cpp rename to src/pano.cpp index 9d4e7c3..b00225a 100644 --- a/pano.cpp +++ b/src/pano.cpp @@ -9,61 +9,65 @@ #include "pano.h" -Pano::Pano(Motor& horiz_motor, Motor& vert_motor, Camera& camera, MPU& mpu) -:horiz_motor(horiz_motor), - vert_motor(vert_motor), - camera(camera), - mpu(mpu) +PanoSetup::PanoSetup(Camera& camera) +:camera(camera) { - motorsEnable(false); - - setFOV(360,180); } -void Pano::setFOV(int horiz_angle, int vert_angle){ + +void PanoSetup::setFOV(int horiz_angle, int vert_angle){ if (horiz_angle && vert_angle && horiz_angle <= 360 && vert_angle <= 180){ horiz_fov = horiz_angle; vert_fov = vert_angle; } } -void Pano::setShutter(unsigned speed, unsigned pre_delay, unsigned post_wait, bool long_pulse){ + +void PanoSetup::setShutter(unsigned speed, unsigned pre_delay, unsigned post_wait, bool long_pulse){ shutter_delay = speed; pre_shutter_delay = pre_delay; post_shutter_delay = post_wait; shutter_long_pulse = long_pulse; } -void Pano::setShots(unsigned shots){ + +void PanoSetup::setShots(unsigned shots){ shots_per_position = shots; } -void Pano::setMode(unsigned mode){ + +void PanoSetup::setMode(unsigned mode){ } -unsigned Pano::getHorizShots(void){ + +unsigned PanoSetup::getHorizShots(void){ return horiz_count; } -unsigned Pano::getVertShots(void){ + +unsigned PanoSetup::getVertShots(void){ return vert_count; } -int Pano::getCurRow(void){ + +int PanoSetup::getCurRow(void){ return position / horiz_count; } -int Pano::getCurCol(void){ + +int PanoSetup::getCurCol(void){ return position % horiz_count; } + /* * Calculate time left to complete pano. */ -unsigned Pano::getTimeLeft(void){ +unsigned PanoSetup::getTimeLeft(void){ int photos = getHorizShots() * getVertShots() - position + 1; int seconds = photos * shots_per_position * (pre_shutter_delay + steady_delay_avg + shots_per_position * (shutter_delay + post_shutter_delay)) / 1000 + // time needed to move the platform // each photo requires a horizontal move (except last one in each row) - (photos - photos/horiz_count) * camera.getHorizFOV() * horiz_gear_ratio * 60 / DYNAMIC_RPM(HORIZ_MOTOR_RPM, camera.getHorizFOV()) / 360 + + (photos - photos/horiz_count) * camera.getHorizFOV() * HORIZ_GEAR_RATIO * 60 / DYNAMIC_RPM(HORIZ_MOTOR_RPM, camera.getHorizFOV()) / 360 + // row-to-row movement - photos / horiz_count * camera.getVertFOV() * vert_gear_ratio * 60 / DYNAMIC_RPM(VERT_MOTOR_RPM, camera.getVertFOV()) / 360 + + photos / horiz_count * camera.getVertFOV() * VERT_GEAR_RATIO * 60 / DYNAMIC_RPM(VERT_MOTOR_RPM, camera.getVertFOV()) / 360 + // row return horizontal movement photos / horiz_count * horiz_fov * 60 / HORIZ_MOTOR_RPM / 360; return seconds; } + /* * Helper to calculate grid fit with overlap * @param total_size: entire grid size (1-360 degrees) @@ -71,7 +75,7 @@ unsigned Pano::getTimeLeft(void){ * @param block_size: ref to initial (max) block size (will be updated) * @param count: ref to image count (will be updated) */ -void Pano::gridFit(int total_size, int overlap, float& block_size, int& count){ +void PanoSetup::gridFit(int total_size, int overlap, float& block_size, int& count){ if (block_size <= total_size){ /* * For 360 pano, we need to cover entire circle plus overlap. @@ -87,17 +91,33 @@ void Pano::gridFit(int total_size, int overlap, float& block_size, int& count){ count = 1; } } + /* * Calculate shot-to-shot horizontal/vertical head movement, * taking overlap into account * Must be called every time focal distance or panorama dimensions change. */ -void Pano::computeGrid(void){ +void PanoSetup::computeGrid(void){ horiz_move = camera.getHorizFOV(); gridFit(horiz_fov, MIN_OVERLAP, horiz_move, horiz_count); vert_move = camera.getVertFOV(); gridFit(vert_fov, MIN_OVERLAP, vert_move, vert_count); } + +/* + * Actual platform driver part + */ +Pano::Pano(Motor& horiz_motor, Motor& vert_motor, Camera& camera, MPU& mpu) +:PanoSetup(camera), + horiz_motor(horiz_motor), + vert_motor(vert_motor), + mpu(mpu) +{ + motorsEnable(false); + + setFOV(360,180); +} + void Pano::start(void){ computeGrid(); motorsEnable(true); @@ -213,11 +233,11 @@ void Pano::moveMotorsAdaptive(float h, float v){ */ void Pano::moveMotors(float h, float v){ if (h){ - horiz_motor.rotate(h * horiz_gear_ratio); + horiz_motor.rotate(h * HORIZ_GEAR_RATIO); horiz_home_offset += h; } if (v){ - vert_motor.rotate(v * vert_gear_ratio); + vert_motor.rotate(v * VERT_GEAR_RATIO); vert_home_offset += v; } } diff --git a/pano.h b/src/pano.h similarity index 81% rename from pano.h rename to src/pano.h index abac0e8..59d0bc2 100644 --- a/pano.h +++ b/src/pano.h @@ -11,6 +11,7 @@ #include #include "camera.h" #include "mpu.h" +#include "pano_settings.h" // Calculate maximum allowed movement at given focal length and shutter // =angular velocity that would cause a pixel to overlap next one within shot time @@ -24,6 +25,8 @@ #define HORIZ_MOTOR_RPM 20 // can do 180 @ 0.8A. 60 @ 0.3A & 1:16 #define VERT_MOTOR_RPM 60 +#define HORIZ_GEAR_RATIO 5 /* 1:5 */ +#define VERT_GEAR_RATIO 15 /* 1:15 */ #define MIN_OVERLAP 20 @@ -32,37 +35,34 @@ #define DYNAMIC_HORIZ_RPM(angle) DYNAMIC_RPM(HORIZ_MOTOR_RPM, angle) #define DYNAMIC_VERT_RPM(angle) DYNAMIC_RPM(VERT_MOTOR_RPM, angle) -class Pano { +class PanoSetup { protected: - Motor& horiz_motor; - Motor& vert_motor; Camera& camera; - MPU& mpu; - float horiz_move; - float vert_move; - int horiz_count; - int vert_count; unsigned shots_per_position = 1; unsigned shutter_delay = 1000/250; unsigned pre_shutter_delay = 0; unsigned post_shutter_delay = 100; bool shutter_long_pulse = false; + void gridFit(int total_size, int overlap, float& block_size, int& count); - // state information + + // Number of horizontal shots to cover horizontal FOV + int horiz_count; + // Number of vertical shots to cover vertical FOV + int vert_count; public: - const int horiz_gear_ratio = 5; // 1:5 - const int vert_gear_ratio = 15; // 1:15 + // Current photo position + unsigned position = 0; + // How many degrees to move horizontally to advance to next column + float horiz_move; + // How many degrees to move vertically to advance to next row + float vert_move; + // Preset panorama horizontal field of view int horiz_fov; + // Preset panorama vertical field of view int vert_fov; - unsigned position; - - float horiz_home_offset = 0; - float vert_home_offset = 0; - - unsigned steady_delay_avg = 100; - - // configuration - Pano(Motor& horiz_motor, Motor& vert_motor, Camera& camera, MPU& mpu); + unsigned steady_delay_avg = 0; + PanoSetup(Camera& camera); void setFOV(int horiz_angle, int vert_angle); void setShutter(unsigned shutter_delay, unsigned pre_delay, unsigned post_wait, bool long_pulse); void setShots(unsigned shots); @@ -75,6 +75,22 @@ class Pano { int getCurRow(void); int getCurCol(void); unsigned getTimeLeft(void); +}; + +class Pano : public PanoSetup { +protected: + Motor& horiz_motor; + Motor& vert_motor; + MPU& mpu; + // state information +public: + + float horiz_home_offset = 0; + float vert_home_offset = 0; + + unsigned steady_delay_avg = 100; + + Pano(Motor& horiz_motor, Motor& vert_motor, Camera& camera, MPU& mpu); // pano execution void start(void); diff --git a/src/pano_settings.h b/src/pano_settings.h new file mode 100644 index 0000000..b528ac6 --- /dev/null +++ b/src/pano_settings.h @@ -0,0 +1,43 @@ +/* + * Pano engine + * + * Copyright (C)2016 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ +#ifndef PANO_SETTINGS_H_ +#define PANO_SETTINGS_H_ + +typedef volatile int16_t settings_t; + +typedef struct { + settings_t + focal = 35, + shutter = 100, + pre_shutter = 100, + post_wait = 500, + long_pulse = 0, + aspect = 1, + shots = 1, + motors_enable = 0, + motors_on = 0, + display_invert = 0, + horiz = 360, + vert = 160; +} PanoSettings; + +typedef struct { + settings_t + battery = 0, + motors_on = 0, + display_invert = 0, + running = 0, + paused = 0, + position = 0, + steady_delay_avg = 0; + float horiz_offset = 0; + float vert_offset = 0; +} PanoState; + +#endif /* PANO_SETTINGS_H_ */ diff --git a/src/protocol.cpp b/src/protocol.cpp new file mode 100644 index 0000000..944ab58 --- /dev/null +++ b/src/protocol.cpp @@ -0,0 +1,172 @@ +/* + * Pano Controller for Arduino project + * Radio communication + * + * Copyright (C)2015,2016 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ + +#include "protocol.h" + +// Packet types +static const char CMD_CONFIG = 'C', + CMD_START = 'P', + CMD_CANCEL = 'X', + CMD_PAUSE = 'O', + CMD_SHUTTER = 'S', + CMD_CONTINUE = 'N', + CMD_HOME_SET = 'R', + CMD_HOME_MOVE = 'B', + CMD_FREE_MOVE = 'F', + CMD_GRID_INC = 'I', + CMD_STATE = 'T'; + +/* + * Send current state, does not wait for confirmation + */ +void Exec::sendState(PanoState& state){ + radio.write_type_data(CMD_STATE, &state, sizeof(state)); +} +/* + * Wait for a config command, returns false if none received + */ +bool Exec::getConfig(PanoSettings& settings){ + uint8_t type = 0; + void *buffer = NULL; + bool config_received = false; + while (radio.available()){ + radio.read_type_data(type, buffer); + if (type == CMD_CONFIG){ + memcpy(&settings, buffer, sizeof(PanoSettings)); + config_received = true; + } + } + return config_received; +} +/* + * Receive and execute a command + * Returns true if a command was received / executed, + * false otherwise + */ +bool Exec::getCmd(PanoSettings& settings, const comm_callbacks& callbacks){ + uint8_t type = 0; + uint8_t len = 0; + void *buffer = NULL; + move_t move; + while (radio.available()){ + len = radio.read_type_data(type, buffer); + switch((char)type){ + + case CMD_CONFIG: // Configuration settings + memcpy(&settings, buffer, sizeof(PanoSettings)); + callbacks.config(); + break; + + case CMD_START: + callbacks.start(); + break; + + case CMD_CANCEL: + callbacks.cancel(); + break; + + case CMD_PAUSE: + callbacks.pause(); + break; + + case CMD_SHUTTER: + callbacks.shutter(); + break; + + case CMD_HOME_SET: + callbacks.setHome(); + break; + + case CMD_HOME_MOVE: + callbacks.goHome(); + break; + + case CMD_FREE_MOVE: + memcpy(&move, buffer, sizeof(move)); + callbacks.freeMove(move); + break; + + case CMD_GRID_INC: + callbacks.gridMove(*(char*)buffer); + break; + } + } + return (buffer != NULL); +} + +/* + * Receive state + * Returns true if state was modified, + * false if it was unmodified or nothing was received + */ +bool Command::getState(PanoState& state, void(*callback)(void)){ + uint8_t type = 0; + uint8_t len = 0; + void *buffer; + while (radio.available()){ + len = radio.read_type_data(type, buffer); + if (type == CMD_STATE && len == sizeof(state)){ + if (memcmp(&state, buffer, sizeof(state)) == 0){ + // state received, but no change + return false; + } else { + memcpy(&state, buffer, sizeof(state)); + if (callback){ + callback(); + } + return true; + } + } + } + return false; +} + +void Command::sendConfig(PanoSettings& settings){ + radio.write_type_data(CMD_CONFIG, &settings, sizeof(settings)); +} + +// Set home to current head position +void Command::setHome(void){ + radio.write_type_data(CMD_HOME_SET); +} + +// Move head to home position +void Command::goHome(void){ + radio.write_type_data(CMD_HOME_MOVE); +} + +// Move head in degrees +void Command::freeMove(move_t& move){ + radio.write_type_data(CMD_FREE_MOVE, &move, sizeof(move)); +} + +void Command::gridMove(PanoState& state, const char dir){ + radio.write_type_data(CMD_GRID_INC, &dir, 1); +} + +void Command::startPano(PanoState& state){ + radio.write_type_data(CMD_START); +} + +void Command::pausePano(PanoState& state){ + radio.write_type_data(CMD_PAUSE); +} + +void Command::cancelPano(PanoState& state){ + radio.write_type_data(CMD_CANCEL); +} + +void Command::continuePano(PanoState& state){ + radio.write_type_data(CMD_CONTINUE); +} + +void Command::shutter(PanoState& state){ + radio.write_type_data(CMD_SHUTTER); +} diff --git a/src/protocol.h b/src/protocol.h new file mode 100644 index 0000000..7b07e63 --- /dev/null +++ b/src/protocol.h @@ -0,0 +1,75 @@ +/* + * Pano Controller for Arduino project + * Radio communication + * + * Copyright (C)2015,2016 Laurentiu Badea + * + * This file may be redistributed under the terms of the MIT license. + * A copy of this license has been included with this distribution in the file LICENSE. + */ + +#ifndef PROTOCOL_H_ +#define PROTOCOL_H_ + +#include +#include "radio.h" +#include "pano_settings.h" + +typedef struct { + settings_t horiz_rpm, vert_rpm; + settings_t horiz_move, vert_move; + float horiz_offset = 0, vert_offset = 0; +} move_t; + +typedef struct { + void(*config)(void); + void(*start)(void); + void(*cancel)(void); + void(*pause)(void); + void(*shutter)(void); + void(*setHome)(void); + void(*goHome)(void); + void(*freeMove)(move_t& move); + void(*gridMove)(char direction); +} comm_callbacks; + +/* + * Commands (Navigator side) + */ +class Command { +protected: + Radio& radio; +public: + Command(Radio& radio):radio(radio){}; + void sendConfig(PanoSettings& settings); + bool getState(PanoState& state, void(*callback)(void)=NULL); + void setHome(void); + void goHome(void); + void freeMove(move_t& move); + void gridMove(PanoState& state, const char dir); + void gridMoveUp(PanoState& state){ gridMove(state, '^'); }; + void gridMoveDown(PanoState& state){ gridMove(state, 'v'); }; + void gridMoveLeft(PanoState& state){ gridMove(state, '<'); }; + void gridMoveRight(PanoState& state){ gridMove(state, '>'); }; + void startPano(PanoState& state); + void pausePano(PanoState& state); + void continuePano(PanoState& state); + void cancelPano(PanoState& state); + void shutter(PanoState& state); +}; + + +/* + * Execution side + */ +class Exec { +protected: + Radio& radio; +public: + Exec(Radio& radio):radio(radio){}; + void sendState(PanoState& state); + bool getConfig(PanoSettings& settings); + bool getCmd(PanoSettings& settings, const comm_callbacks& callbacks); +}; + +#endif /* PROTOCOL_H_ */ diff --git a/src/radio.cpp b/src/radio.cpp new file mode 100644 index 0000000..59f4810 --- /dev/null +++ b/src/radio.cpp @@ -0,0 +1,111 @@ +/* + * Pano Controller NRF24 Radio Remote Control + */ + +#include "radio.h" + +Radio::Radio(uint8_t ce_pin, uint8_t csn_pin){ + radio = new RF24(ce_pin, csn_pin); + connected = false; + last_received_ts = millis(); +} + +void Radio::begin(void){ + radio->begin(); + radio->setChannel(76); // default is 76 + radio->setPALevel(RF24_PA_LOW); // RF24_PA_MIN, RF24_PA_LOW, RF24_PA_HIGH, RF24_PA_MAX + radio->setCRCLength(RF24_CRC_16); + radio->setDataRate(RF24_1MBPS); // RF24_250KBPS, RF24_1MBPS, RF24_2MBPS + // retry timeout window needs to allow for ack payload, see spec pdf + radio->setRetries(4, 10); // 4*250us, 10 retries + radio->setAutoAck(true); // default true + //radio->maskIRQ(1, 1, 1); // tx_ok=1, tx_fail=1, rx_ready=1 + radio->enableDynamicPayloads(); + radio->openWritingPipe(address[0]); + radio->openReadingPipe(1, address[0]); + radio->powerUp(); + radio->startListening(); + radio->enableAckPayload(); + radio->printDetails(); +} + +static void debug(const char *msg, const void *buf, int len){ + return; + Serial.print(msg); + Serial.print(len); + Serial.print(" -> "); + while (len--){ + Serial.print(*(const char*)buf, HEX); + buf += 1; + Serial.print(" "); + } + Serial.println(); +} + +int Radio::available(void){ + int count = radio->available(); + if (count > 0){ + last_received_ts = millis(); + if (!connected){ + Serial.println("NRF24 Connected"); + connected = true; + } + } else if (connected && millis() - last_received_ts > 10000){ + Serial.println("NRF24 Connection Lost"); + connected = false; + } + return count; +} + +uint8_t Radio::read(void){ + uint8_t len = 0; + if (available()){ + len = radio->getDynamicPayloadSize(); + radio->read(buffer, len); + debug("Radio Received ", buffer, len); + } + return len; +} + +void Radio::write(const void *data, uint8_t len){ + debug("Radio Send ", data, len); + radio->stopListening(); + if (!radio->write(data, len)){ + connected = false; + } + radio->startListening(); +} + +/* + * Helper method to receive a packet type with the data buffer + */ +uint8_t Radio::read_type_data(uint8_t& type, void*& data){ + uint8_t len = read(); + if (len <= 0){ + return 0; + } + type = *(uint8_t*)buffer; + len--; + data = (len) ? buffer + 1 : NULL; + return len; +} + +/* + * Helper method to send out a packet type with the data buffer + */ +void Radio::write_type_data(const uint8_t type, const void* data, uint8_t len, + const bool delayed){ + if (len == 0 && data){ + len = strlen((const char*)data); + } + len = min(len, 32); + buffer[0] = type; + for (int i=0; i < len && i < 31; i++){ + buffer[i+1] = ((const char*)data)[i]; + } + if (!delayed){ + write(buffer, len+1); + } else { + radio->writeAckPayload(1, buffer, len+1); + } +} diff --git a/src/radio.h b/src/radio.h new file mode 100644 index 0000000..9eb0a34 --- /dev/null +++ b/src/radio.h @@ -0,0 +1,34 @@ +/* + * Pano Controller NRF24 Radio Remote Control + */ +#ifndef RADIO_H_ +#define RADIO_H_ + +#include +#include +#include + +#undef printf + +// http://tmrh20.github.io/RF24/classRF24.html + +class Radio { +private: + RF24* radio; + byte address[1][6] = {"2PANO"}; + unsigned last_received_ts = 0; +public: + bool connected = false; + char buffer[32]; + char ack_payload[32]; + uint8_t ack_payload_len; + Radio(uint8_t ce_pin, uint8_t csn_pin); + void begin(void); + int available(void); + uint8_t read(void); + uint8_t read_type_data(uint8_t& type, void*& data); + void write(const void *data, uint8_t len); + void write_type_data(const uint8_t type, const void* data=NULL, uint8_t len=0, const bool delayed=false); +}; + +#endif /* RADIO_H_ */ diff --git a/remote.cpp b/src/remote.cpp similarity index 100% rename from remote.cpp rename to src/remote.cpp diff --git a/remote.h b/src/remote.h similarity index 100% rename from remote.h rename to src/remote.h