Skip to content

Commit

Permalink
[driver] rewrite adns9800
Browse files Browse the repository at this point in the history
  • Loading branch information
TomSaw committed Aug 3, 2024
1 parent 9b4b64c commit 6ceed8e
Show file tree
Hide file tree
Showing 11 changed files with 1,122 additions and 450 deletions.
141 changes: 32 additions & 109 deletions examples/blue_pill_f103/adns_9800/main.cpp
Original file line number Diff line number Diff line change
@@ -1,8 +1,5 @@
/*
* Copyright (c) 2011, Fabian Greif
* Copyright (c) 2013, Kevin Läufer
* Copyright (c) 2013-2017, Niklas Hauser
* Copyright (c) 2014, 2016, 2018, Sascha Schade
* Copyright (c) 2024, Thomas Sommer
*
* This file is part of the modm project.
*
Expand All @@ -14,143 +11,69 @@

#include <modm/board.hpp>
#include <modm/debug/logger.hpp>
#include <modm/processing/timer.hpp>
#include <modm/processing/protothread.hpp>

#include <modm/processing/fiber.hpp>
#include <modm/driver/motion/adns9800.hpp>
#include <modm/math/geometry/vector2.hpp>

#include <inttypes.h>

// ----------------------------------------------------------------------------
// Set the log level
#undef MODM_LOG_LEVEL
#define MODM_LOG_LEVEL modm::log::DEBUG
#undef MODM_LOG_LEVEL
#define MODM_LOG_LEVEL modm::log::DEBUG

using Usart2 = BufferedUart<UsartHal2, UartTxBuffer<256>>;
// Create an IODeviceWrapper around the Uart Peripheral we want to use
modm::IODeviceWrapper< Usart2, modm::IOBuffer::BlockIfFull > loggerDevice;
modm::IODeviceWrapper<Usart2, modm::IOBuffer::BlockIfFull> loggerDevice;

// Set all four logger streams to use the UART
modm::log::Logger modm::log::debug(loggerDevice);
modm::log::Logger modm::log::info(loggerDevice);
modm::log::Logger modm::log::warning(loggerDevice);
modm::log::Logger modm::log::error(loggerDevice);

class BlinkThread : public modm::pt::Protothread
{
public:
BlinkThread()
{
timeout.restart(100ms);
}

bool
update()
{
PT_BEGIN();

while (true)
{
Board::LedGreen::reset();
using Cs = GpioOutputA4;

PT_WAIT_UNTIL(timeout.isExpired());
timeout.restart(100ms);
modm::Fiber<> adns9800_fiber([]() {
modm::Adns9800<SpiMaster1, Cs> adns9800;
modm::Vector2i position;

Board::LedGreen::set();
Cs::setOutput(modm::Gpio::High);

PT_WAIT_UNTIL(timeout.isExpired()) ;
timeout.restart(4.9s);
SpiMaster1::connect<GpioOutputA5::Sck, GpioInputA6::Miso, GpioOutputA7::Mosi>();
SpiMaster1::initialize<Board::SystemClock, 2.25_MHz>();
SpiMaster1::setDataMode(SpiMaster1::DataMode::Mode3);

MODM_LOG_INFO << "Seconds since reboot: " << uptime << modm::endl;

uptime += 5;
}

PT_END();
if(not adns9800.initialize()) {
MODM_LOG_INFO << "Failed to initialize ADNS9800" << modm::endl;
return;
}

private:
modm::ShortTimeout timeout;
uint32_t uptime;
};

class Adns9800Thread : public modm::pt::Protothread
{
public:
Adns9800Thread() : timer(10ms), x(0), y(0)
{
}
adns9800.set(modm::adns9800::Resolution<8200>{});
adns9800.set(modm::adns9800::ShutterConfig{
period_min: 10000,
period_max: 40000,
exposure_max: 50000
});

bool
update()
while (true)
{
PT_BEGIN();

Cs::setOutput(modm::Gpio::High);
const auto data {adns9800.read<modm::adns9800::Data_FailFlags_Monitoring>()};
position += data.delta;

SpiMaster1::connect<GpioOutputA7::Mosi, GpioOutputA5::Sck, GpioInputA6::Miso>();
SpiMaster1::initialize<Board::SystemClock, 2.25_MHz>();
SpiMaster1::setDataMode(SpiMaster1::DataMode::Mode3);
MODM_LOG_INFO << "delta: " << data.delta << modm::endl;
MODM_LOG_INFO << "position: " << position << modm::endl;
MODM_LOG_INFO << modm::endl;

adns9800::initialise();

while (true)
{
PT_WAIT_UNTIL(timer.execute());

{
int16_t delta_x, delta_y;
adns9800::getDeltaXY(delta_x, delta_y);
MODM_LOG_INFO.printf("dx = %5" PRId16 ", dy = %5" PRId16"; x = %9" PRId32", y=%9" PRId32 "\n", delta_x, delta_y, x, y);

x += delta_x;
y += delta_y;
}
}

PT_END();
modm::this_fiber::sleep_for(100ms);
}
});

private:
modm::ShortPeriodicTimer timer;
int32_t x, y;

using Cs = GpioOutputA4;

using adns9800 = modm::Adns9800<
/* Spi = */ SpiMaster1,
/* Ncs = */ Cs >;
};


BlinkThread blinkThread;
Adns9800Thread adns9800Thread;


// ----------------------------------------------------------------------------
int
main()
{
Board::initialize();

// initialize Uart2 for MODM_LOG_*
Usart2::connect<GpioOutputA2::Tx>();
Usart2::initialize<Board::SystemClock, 115200_Bd>();

// Use the logging streams to print some messages.
// Change MODM_LOG_LEVEL above to enable or disable these messages
MODM_LOG_DEBUG << "debug" << modm::endl;
MODM_LOG_INFO << "info" << modm::endl;
MODM_LOG_WARNING << "warning" << modm::endl;
MODM_LOG_ERROR << "error" << modm::endl;

MODM_LOG_INFO << "Welcome to ADNS 9800 demo." << modm::endl;

while (true)
{
blinkThread.update();
adns9800Thread.update();
}

modm::fiber::Scheduler::run();
return 0;
}
2 changes: 1 addition & 1 deletion examples/blue_pill_f103/adns_9800/project.xml
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
<module>modm:platform:gpio</module>
<module>modm:platform:spi:1</module>
<module>modm:platform:uart:2</module>
<module>modm:processing:protothread</module>
<module>modm:processing:fiber</module>
<module>modm:processing:timer</module>
<module>modm:build:scons</module>
</modules>
Expand Down
Loading

0 comments on commit 6ceed8e

Please sign in to comment.