From a06fff3cbfdd473c3934ece8347fe3c887138f29 Mon Sep 17 00:00:00 2001 From: ZodiusInfuser Date: Tue, 15 Jun 2021 14:45:05 +0100 Subject: [PATCH] Added frame capture support and demo --- drivers/pmw3901/pmw3901.cpp | 97 +++++++++++++++---- drivers/pmw3901/pmw3901.hpp | 7 +- examples/breakout_pmw3901/CMakeLists.txt | 18 +--- .../frame_capture/CMakeLists.txt | 16 +++ .../frame_capture/frame_capture.cpp | 71 ++++++++++++++ .../breakout_pmw3901/motion/CMakeLists.txt | 16 +++ .../{demo.cpp => motion/motion.cpp} | 18 ++-- 7 files changed, 197 insertions(+), 46 deletions(-) create mode 100644 examples/breakout_pmw3901/frame_capture/CMakeLists.txt create mode 100644 examples/breakout_pmw3901/frame_capture/frame_capture.cpp create mode 100644 examples/breakout_pmw3901/motion/CMakeLists.txt rename examples/breakout_pmw3901/{demo.cpp => motion/motion.cpp} (53%) diff --git a/drivers/pmw3901/pmw3901.cpp b/drivers/pmw3901/pmw3901.cpp index cc620489..9e8eeb57 100644 --- a/drivers/pmw3901/pmw3901.cpp +++ b/drivers/pmw3901/pmw3901.cpp @@ -4,17 +4,20 @@ #include #include #include +#include namespace pimoroni { enum reg : uint8_t { - ID = 0x00, - REVISION = 0x01, - DATA_READY = 0x02, - MOTION_BURST = 0x16, - POWER_UP_RESET = 0x3a, - ORIENTATION = 0x5b, - RESOLUTION = 0x4e, // PAA5100 only + ID = 0x00, + REVISION = 0x01, + DATA_READY = 0x02, + MOTION_BURST = 0x16, + POWER_UP_RESET = 0x3a, + ORIENTATION = 0x5b, + RESOLUTION = 0x4e, // PAA5100 only + RAWDATA_GRAB = 0x58, + RAWDATA_GRAB_STATUS = 0x59, }; bool PMW3901::init() { @@ -134,16 +137,15 @@ namespace pimoroni { read_registers(reg::MOTION_BURST, buf, 12); uint8_t dr = buf[0]; //uint8_t obs = buf[1]; - x_out = (int16_t)((int32_t)buf[2] << 8 | buf[3]); - y_out = (int16_t)((int32_t)buf[4] << 8 | buf[5]); + x_out = (int16_t)((int32_t)buf[3] << 8 | buf[2]); + y_out = (int16_t)((int32_t)buf[5] << 8 | buf[4]); uint8_t quality = buf[6]; //uint8_t raw_sum = buf[7]; //uint8_t raw_max = buf[8]; //uint8_t raw_min = buf[9]; uint8_t shutter_upper = buf[10]; //uint8_t shutter_lower = buf[11]; - printf("dr = %d, x = %d, y = %d\n",dr, x_out, y_out); - if((dr & 0b10000000) && !((quality < 0x19) && (shutter_upper = 0x1f))) + if((dr & 0b10000000) && !((quality < 0x19) && (shutter_upper == 0x1f))) return true; sleep_ms(1); @@ -158,9 +160,8 @@ namespace pimoroni { uint8_t buf[5]; read_registers(reg::DATA_READY, buf, 5); uint8_t dr = buf[0]; - x_out = (int16_t)((int32_t)buf[1] << 8 | buf[2]); - y_out = (int16_t)((int32_t)buf[3] << 8 | buf[4]); - printf("dr = %d, x = %d, y = %d\n",dr, x_out, y_out); + x_out = (int16_t)((int32_t)buf[2] << 8 | buf[1]); + y_out = (int16_t)((int32_t)buf[4] << 8 | buf[3]); if(dr & 0b10000000) return true; @@ -170,8 +171,68 @@ namespace pimoroni { return false; } - void PMW3901::frame_capture(uint16_t timeout_ms) { + bool PMW3901::frame_capture(uint8_t (&raw_data_out)[RAW_DATA_LEN], uint16_t& data_size_out, uint16_t timeout_ms) { + bool success = false; + data_size_out = 0; + + uint8_t buf[] = { + 0x7f, 0x07, + 0x4c, 0x00, + 0x7f, 0x08, + 0x6a, 0x38, + 0x7f, 0x00, + 0x55, 0x04, + 0x40, 0x80, + 0x4d, 0x11, + + WAIT, 0x0a, + + 0x7f, 0x00, + 0x58, 0xff + }; + write_buffer(buf, sizeof(buf)); + + uint8_t status = 0; + uint32_t start_time = millis(); + while(millis() - start_time < timeout_ms) { + status = read_register(reg::RAWDATA_GRAB_STATUS); + if(status & 0b11000000) { + success = true; + break; + } + } + + if(success) { + write_register(reg::RAWDATA_GRAB, 0x00); + memset(raw_data_out, 0, RAW_DATA_LEN * sizeof(uint8_t)); + + uint8_t data = 0; + uint16_t x = 0; + + success = false; + start_time = millis(); + while(millis() - start_time < timeout_ms) { + data = read_register(reg::RAWDATA_GRAB); + if((data & 0b11000000) == 0b01000000) { // Upper 6-bits + raw_data_out[x] &= ~0b11111100; + raw_data_out[x] |= (data & 0b00111111) << 2; // Held in 5:0 + } + if((data & 0b11000000) == 0b10000000) { // Lower 2-bits + raw_data_out[x] &= ~0b00000011; + raw_data_out[x] |= (data & 0b00001100) >> 2; // Held in 3:2 + x++; + } + if(x == RAW_DATA_LEN) { + success = true; + break; + } + } + + data_size_out = x; + } + + return success; } void PMW3901::cs_select() { @@ -179,7 +240,9 @@ namespace pimoroni { } void PMW3901::cs_deselect() { + sleep_us(1); gpio_put(cs, true); + sleep_us(1); } void PMW3901::write_register(uint8_t reg, uint8_t data) { @@ -192,14 +255,12 @@ namespace pimoroni { } void PMW3901::write_buffer(uint8_t *buf, uint16_t len) { - cs_select(); for(uint8_t i = 0; i < len; i += 2) { if(buf[i] == WAIT) sleep_ms(buf[i + 1]); else - spi_write_blocking(spi, &buf[i], 2); + write_register(buf[i], buf[i + 1]); } - cs_deselect(); } void PMW3901::read_registers(uint8_t reg, uint8_t *buf, uint16_t len) { diff --git a/drivers/pmw3901/pmw3901.hpp b/drivers/pmw3901/pmw3901.hpp index 538a5d62..31b17f6f 100644 --- a/drivers/pmw3901/pmw3901.hpp +++ b/drivers/pmw3901/pmw3901.hpp @@ -12,8 +12,11 @@ namespace pimoroni { //-------------------------------------------------- // Constants //-------------------------------------------------- + public: + static const uint8_t FRAME_SIZE = 35; + static const uint16_t RAW_DATA_LEN = 1225; private: - static const uint8_t WAIT = -1; + static const uint8_t WAIT = -1; //-------------------------------------------------- // Enums @@ -85,7 +88,7 @@ namespace pimoroni { void set_orientation(bool invert_x = true, bool invert_y = true, bool swap_xy = true); bool get_motion(int16_t& x_out, int16_t& y_out, uint16_t timeout_ms = 5000); bool get_motion_slow(int16_t& x_out, int16_t& y_out, uint16_t timeout_ms = 5000); - void frame_capture(uint16_t timeout_ms = 10000); + bool frame_capture(uint8_t (&raw_data_out)[RAW_DATA_LEN], uint16_t& data_size_out, uint16_t timeout_ms = 10000); protected: virtual void secret_sauce(); diff --git a/examples/breakout_pmw3901/CMakeLists.txt b/examples/breakout_pmw3901/CMakeLists.txt index 2494aac7..ebf22a54 100644 --- a/examples/breakout_pmw3901/CMakeLists.txt +++ b/examples/breakout_pmw3901/CMakeLists.txt @@ -1,16 +1,2 @@ -set(OUTPUT_NAME pmw3901_demo) - -add_executable( - ${OUTPUT_NAME} - demo.cpp -) - -# enable usb output, disable uart output -pico_enable_stdio_usb(${OUTPUT_NAME} 1) -pico_enable_stdio_uart(${OUTPUT_NAME} 1) - -# Pull in pico libraries that we need -target_link_libraries(${OUTPUT_NAME} pico_stdlib breakout_pmw3901) - -# create map/bin/hex file etc. -pico_add_extra_outputs(${OUTPUT_NAME}) +add_subdirectory(motion) +add_subdirectory(frame_capture) \ No newline at end of file diff --git a/examples/breakout_pmw3901/frame_capture/CMakeLists.txt b/examples/breakout_pmw3901/frame_capture/CMakeLists.txt new file mode 100644 index 00000000..72efaeaf --- /dev/null +++ b/examples/breakout_pmw3901/frame_capture/CMakeLists.txt @@ -0,0 +1,16 @@ +set(OUTPUT_NAME pmw3901_framecapture) + +add_executable( + ${OUTPUT_NAME} + frame_capture.cpp +) + +# enable usb output, disable uart output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) +pico_enable_stdio_uart(${OUTPUT_NAME} 1) + +# Pull in pico libraries that we need +target_link_libraries(${OUTPUT_NAME} pico_stdlib breakout_pmw3901) + +# create map/bin/hex file etc. +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/breakout_pmw3901/frame_capture/frame_capture.cpp b/examples/breakout_pmw3901/frame_capture/frame_capture.cpp new file mode 100644 index 00000000..b6c57703 --- /dev/null +++ b/examples/breakout_pmw3901/frame_capture/frame_capture.cpp @@ -0,0 +1,71 @@ +#include +#include "pico/stdlib.h" +#include +#include + +#include "breakout_pmw3901.hpp" + +using namespace pimoroni; + +BreakoutPMW3901 flo(BG_SPI_FRONT); +BreakoutPMW3901::Degrees rotation = BreakoutPMW3901::DEGREES_0; +const uint8_t SIZE = BreakoutPMW3901::FRAME_SIZE; +uint8_t data[BreakoutPMW3901::RAW_DATA_LEN]; + +std::string value_to_char(uint8_t value) { + const std::string charmap = " .:-=+*#%@"; + float val = (float)value / 255.0f; + val *= charmap.length() - 1; + value = (uint8_t)val; + std::string chosen_char = charmap.substr(val, 1); + return chosen_char.append(chosen_char); // Double chars to - sort of - correct aspect ratio +} + +int main() { + stdio_init_all(); + + flo.init(); + flo.set_rotation(rotation); + + uint8_t offset = 0; + uint8_t value = 0; + + while(true) { + printf("Capturing...\n"); + uint16_t data_size = 0; + if(flo.frame_capture(data, data_size)) { + for(uint8_t y = 0; y < SIZE; y++) { + if(rotation == BreakoutPMW3901::DEGREES_180 || rotation == BreakoutPMW3901::DEGREES_270) + y = SIZE - y - 1; + + for(uint8_t x = 0; x < SIZE; x++) { + if(rotation == BreakoutPMW3901::DEGREES_180 || rotation == BreakoutPMW3901::DEGREES_90) + x = SIZE - x - 1; + + if(rotation == BreakoutPMW3901::DEGREES_90 || rotation == BreakoutPMW3901::DEGREES_270) + offset = (x * 35) + y; + else + offset = (y * 35) + x; + + value = data[offset]; + printf("%s", value_to_char(value).c_str()); + } + printf("\n"); + } + } + else { + printf("Capture failed. %d bytes received, of %d. Recapturing in ", data_size, BreakoutPMW3901::RAW_DATA_LEN); + } + printf("5...\n"); + sleep_ms(1000); + printf("4...\n"); + sleep_ms(1000); + printf("3...\n"); + sleep_ms(1000); + printf("2...\n"); + sleep_ms(1000); + printf("Get Ready!\n"); + sleep_ms(1000); + }; + return 0; +} diff --git a/examples/breakout_pmw3901/motion/CMakeLists.txt b/examples/breakout_pmw3901/motion/CMakeLists.txt new file mode 100644 index 00000000..d3b84614 --- /dev/null +++ b/examples/breakout_pmw3901/motion/CMakeLists.txt @@ -0,0 +1,16 @@ +set(OUTPUT_NAME pmw3901_motion) + +add_executable( + ${OUTPUT_NAME} + motion.cpp +) + +# enable usb output, disable uart output +pico_enable_stdio_usb(${OUTPUT_NAME} 1) +pico_enable_stdio_uart(${OUTPUT_NAME} 1) + +# Pull in pico libraries that we need +target_link_libraries(${OUTPUT_NAME} pico_stdlib breakout_pmw3901) + +# create map/bin/hex file etc. +pico_add_extra_outputs(${OUTPUT_NAME}) diff --git a/examples/breakout_pmw3901/demo.cpp b/examples/breakout_pmw3901/motion/motion.cpp similarity index 53% rename from examples/breakout_pmw3901/demo.cpp rename to examples/breakout_pmw3901/motion/motion.cpp index 7728b964..4bf31a9b 100644 --- a/examples/breakout_pmw3901/demo.cpp +++ b/examples/breakout_pmw3901/motion/motion.cpp @@ -5,24 +5,22 @@ using namespace pimoroni; -BreakoutPMW3901 flo; +BreakoutPMW3901 flo(BG_SPI_FRONT); int main() { stdio_init_all(); - sleep_ms(10000); - flo.init(); flo.set_rotation(BreakoutPMW3901::DEGREES_0); - - //uint8_t tx = 0, ty = 0; - + int16_t tx = 0, ty = 0; int16_t x = 0, y = 0; while(true) { - flo.get_motion(x, y); - printf("tick\n"); - //tx; - sleep_ms(1000); + if(flo.get_motion(x, y)) { + tx += x; + ty += y; + printf("Relative: x %6d, y %6d | Absolute: tx %6d, ty %6d\n", x, y, tx, ty); + } + sleep_ms(10); }; return 0; }