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"

#include <cmath>

// 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

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 states : int8_t{

      IDLE                    = 0,

      FOLLOW_LINE             = 1,

      DETECT_STATION          = 2,

      REVERSE                 = 3,

      PUSH_BACK               = 4,

      CHECK_POSITIONING       = 5,

      CHECK_VOLTAGE           = 6,

      CHARGING                = 7,

      RELEASE                 = 8,

      RELEASE_TO_CORRECT      = 9,

      CORRECT_POSITIONING     = 10,

      TURN                    = 12,

      INACTIVE                = 13,

      CALIBRATION             = 14,

      CALIBRATION_CHECK       = 15,

      DEVIATION_CORRECTION    = 16,

      TEST_MAP_STATE          = 17,

      DOCKING_ERROR           = -1,

      REVERSE_TIMEOUT_ERROR   = -2,

      CALIBRATION_ERROR       = -3,

      WHITE_DETECTION_ERROR   = -4,

      PROXY_DETECTION_ERROR   = -5,

      NO_CHARGING_POWER_ERROR = -6,

      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();

  // State Variables

  states prevState = states::IDLE;

  states currentState = states::IDLE;

  states newState = states::IDLE;

  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_