twoEncoderOdometry.hpp

Go to the documentation of this file.

/*
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */
#pragma once
 
#include "okapi/api/odometry/odometry.hpp"
#include "okapi/api/units/QSpeed.hpp"
#include "okapi/api/util/abstractRate.hpp"
#include "okapi/api/util/logging.hpp"
#include "okapi/api/util/timeUtil.hpp"
#include <atomic>
#include <memory>
#include <valarray>
 
namespace okapi {
class TwoEncoderOdometry : public Odometry {
  public:
  TwoEncoderOdometry(const TimeUtil &itimeUtil,
                     const std::shared_ptr<ReadOnlyChassisModel> &imodel,
                     const ChassisScales &ichassisScales,
                     const std::shared_ptr<Logger> &ilogger = Logger::getDefaultLogger());
 
  virtual ~TwoEncoderOdometry() = default;
 
  void setScales(const ChassisScales &ichassisScales) override;
 
  void step() override;
 
  OdomState getState(const StateMode &imode = StateMode::FRAME_TRANSFORMATION) const override;
 
  void setState(const OdomState &istate,
                const StateMode &imode = StateMode::FRAME_TRANSFORMATION) override;
 
  std::shared_ptr<ReadOnlyChassisModel> getModel() override;
 
  ChassisScales getScales() override;
 
  protected:
  std::shared_ptr<Logger> logger;
  std::unique_ptr<AbstractRate> rate;
  std::unique_ptr<AbstractTimer> timer;
  std::shared_ptr<ReadOnlyChassisModel> model;
  ChassisScales chassisScales;
  OdomState state;
  std::valarray<std::int32_t> newTicks{0, 0, 0}, tickDiff{0, 0, 0}, lastTicks{0, 0, 0};
  const std::int32_t maximumTickDiff{1000};
 
  virtual OdomState odomMathStep(const std::valarray<std::int32_t> &itickDiff,
                                 const QTime &ideltaT);
};
} // namespace okapi