AMiRo-OS

#include <amiroosconf.h>

#include <amiro/Color.h>

// #include "global.hpp"

#include <cmath>

#define DOCKING_CORRECTION_TIMEOUT 200

#define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT

#define REVERSE_ADJUSTMENT_TIMEOUT 200

// Thresh for wheel proxy sensors, when summed values fall below the state changes

// #define PROXY_WHEEL_THRESH 18000

// #define WHITE_COUNT_THRESH 150

#define WHITE_DETETION_TIMEOUT 150

// #define RING_PROX_COUNT_THRESH 1000

// Rotation around 180 degrees in microradian

// #define ROTATION_180 3141592

#define ROTATION_180 3141592

    uint32_t buf = 0;

    uint8_t meanWindow = 150;

  };

  struct deviation_correction {

    bool RCase = true;

    int8_t pCount = 0;

  // static const struct ut_counter emptyUtCount;

  ut_counter utCount;

  proxy_ctrl pCtrl;

  deviation_correction devCor;

  * Returns percentage of mean deviation between two given values.

  * It is intended to calculate the mean deviation between two proxy sensor

  * values. PROX_DEVIATION_MEAN_WINDOW determains the size of the mean window.

  * */

  int32_t meanDeviation(uint16_t a, uint16_t b);

   * Check sectors around and stop if a thresh in one sector is detected.

   */

  void preventCollision(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]);

  /**

   * Same as prevent collision but also lowers the speed when object is detected.

   */

  void getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]);

  void getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax);

  void chargeAsLED();

  /**

   * Returns true when front sensors reaching high values

   * and all others are low. This indicates that the loading station is ahead.

  /**

   * Check if current position changes when the wheel are deactivated.

   * When AMiRo drives towards the loading station, it stops when a specific marker is reached.

   * In order to validate that the AMiRo is correctly positioned in the loading station

   * the wheels are turned off. When the position remains the same the docking procedure

   */

  int checkDockingSuccess();