AMiRo-OS

#ifndef AMIRO_USERTHREAD_H_

#define AMIRO_USERTHREAD_H_

#include <ch.hpp>

#include <amiroosconf.h>

#include <amiro/Color.h>

// #include "global.hpp"

// #include "linefollow.hpp"

// TODO: Clean up and sort defines!

// Speed when driving towards the docking station

#define CHARGING_SPEED 5

#define DETECTION_SPEED 10

#define DIST_THRESH 100

#define RELEASE_COUNT 200

#define SPEED_CONVERSION_FACTOR 1000000 //Convert value to um/s

// Thresh to determain how much update steps should pass while alining

// #define MAX_CORRECTION_STEPS 200

#define DOCKING_CORRECTION_TIMEOUT 200

#define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT

#define REVERSE_ADJUSTMENT_TIMEOUT 200

// #define MAX_RING_PROX_VALUE_DEVIATION

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

// #define PROXY_WHEEL_THRESH 18000

// Thresh for detecting obsticles

// #define PROXY_RING_THRESH 15000

// #define PUSH_BACK_COUNT 5

#define PUSH_BACK_TIMEOUT 5

// Thresh for how long (update steps) the front sensors are allowed to detect white

// #define WHITE_COUNT_THRESH 150

#define WHITE_DETETION_TIMEOUT 150

// #define RING_PROX_COUNT_THRESH 1000

#define RING_PROX_DETECTION_TIMEOUT 400

// Rotation around 180 degrees in microradian

// #define ROTATION_180 3141592

#define ROTATION_180 3141592

// Rotation around -20 degrees in microradian

#define ROTATION_20 -349065

#define ROTATION_DURATION 10000

#define RING_PROX_FRONT_THRESH 18000

// #define PROX_MAX_VAL 65430

#define PROX_MAX_VAL 0xFFF0

// Threshold for failing to dock

#define DOCKING_ERROR_THRESH 3

#define CAN_TRANSMIT_STATE_THRESH 50

#define PROX_DEVIATION_MEAN_WINDOW 3

#define MAX_DEVIATION_CORRECTIONS 4

#define MAX_DEVIATION_FACTOR 35

#define DEVIATION_CORRECTION_DURATION 900

#define DEVIATION_CORRECTION_SPEED 2000000

#define DEVIATION_CORRECTION_VALUE (DEVIATION_CORRECTION_SPEED / 2)

#define DEVIATION_DIST_THRESH 25000

// Map Tracking Parameters

// enable amiro map to continuously build internal map representation

#define AMIRO_MAP_AUTO_TRACKING true

namespace amiro {

class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>

{

  // Messages which can be received and trigger state changes

  public:

    // States of user thread state machine

    // enum ut_states : int8_t {

    //   UT_IDLE = 0,

    //   UT_FOLLOW_LINE = 1,

    //   UT_DETECT_STATION = 2,

    //   UT_REVERSE = 3,

    //   UT_PUSH_BACK = 4,

    //   UT_CHECK_POSITIONING = 5,

    //   UT_CHECK_VOLTAGE = 6,

    //   UT_CHARGING = 7,

    //   UT_RELEASE = 8,

    //   UT_RELEASE_TO_CORRECT = 9,

    //   UT_CORRECT_POSITIONING = 10,

    //   UT_TURN = 12,

    //   UT_INACTIVE = 13,

    //   UT_CALIBRATION = 14,

    //   UT_CALIBRATION_CHECK = 15,

    //   UT_DEVIATION_CORRECTION = 16,

    //   UT_TEST_MAP_STATE = 17,

    //   UT_TEST_MAP_AUTO_STATE = 18,

    //   UT_DOCKING_ERROR = -1,

    //   UT_REVERSE_TIMEOUT_ERROR = -2,

    //   UT_CALIBRATION_ERROR = -3,

    //   UT_WHITE_DETECTION_ERROR = -4,

    //   UT_PROXY_DETECTION_ERROR = -5,

    //   UT_NO_CHARGING_POWER_ERROR = -6,

    //   UT_UNKNOWN_STATE_ERROR = -7

    // };

    struct ut_counter {

      int whiteCount = 0;

      int ringProxCount = 0;

      // int correctionCount = 0;

      int errorCount = 0;

      int stateCount = 0;

      int stepCount = 0;

      uint32_t stateTime = 0;

  };

  struct proxy_ctrl {

    int threshLow = 100;

    int threshMid = 500;

    int threshHigh = 1500;

    // int threshHigh = 1500;

    int penalty = 100000;

    int pFactor = 310000;

    int staticCont = 0;

  };

  struct bottom_prox_calibration {

    bool calibrateBlack = true;

    uint32_t buf = 0;

    uint8_t meanWindow = 150;

  };

  struct deviation_correction {

    bool RCase = true;

    int8_t pCount = 0;

    int32_t proxbuf[PROX_DEVIATION_MEAN_WINDOW] = { 0 };

    int32_t currentDeviation = 0;

  };

  // static const struct ut_counter emptyUtCount;

  ut_counter utCount;

  proxy_ctrl pCtrl;

  bottom_prox_calibration proxCalib;

  deviation_correction devCor;

  explicit UserThread();

  virtual ~UserThread();

  virtual msg_t main();

private:

  void setRpmSpeed(const int (&rpmSpeed)[2]);

  void setRpmSpeedFuzzy(const int (&rpmSpeed)[2]);

  void lightOneLed(Color color, int idx);

  void lightAllLeds(Color color);

  /**

   * Uses light ring indicate the state of charge.

   */

  void showChargingState();

  void checkForMotion();

  bool checkPinVoltage();

  bool checkPinEnabled();

  int getProxyRingSum();

  /**

  * 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.

  * Keep in mind that initial results are wrong.

  * */

  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 proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]);

  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.

   * If other sensores are blocked too, indicates that someone grabs the robot.

   */

  bool checkFrontalObject();

  /**

   * 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

   * was successful (return 1) otherwise a correction is needed (return 0).

   */

  int checkDockingSuccess();

  bool continue_on_obstacle = true;

  uint16_t rProx[8]; // buffer for ring proxy values

  int rpmSpeed[2] = {0};

  int stop[2] = {0};

  int turn[2] = {5,-5};

};

} // end of namespace amiro

#endif // AMIRO_USERTHREAD_H_