AMiRo-OS

#ifndef AMIRO_USERTHREAD_H_

#define AMIRO_USERTHREAD_H_

#include <ch.hpp>

#include <amiroosconf.h>

#include <amiro/Color.h>

// Speed when driving towards the docking station

#define CHARGING_SPEED 5

#define DETECTION_SPEED 10

#define DIST_THRESH 100

#define RELEASE_COUNT 200

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

#define MAX_CORRECTION_STEPS 300

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

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

#define WHITE_COUNT_THRESH 150

// Rotation around 180 degrees in microradian

#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

// Threshold for failing to dock

#define DOCKING_ERROR_THRESH 4

namespace amiro {

class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>

{

  // Messages which can be received and trigger state changes

  public:

    enum msg_content{

      STOP, 

      START,

      EDGE_LEFT,

      EDGE_RIGHT,

      FUZZY,

      DOCK,

      UNDOCK,

      CHARGE

    };

    // States of user thread state machine

    enum states : uint8_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,

      ERROR               = 11,

      TURN                =12

    };

  struct ut_counter{

      int whiteCount = 0;

      int proxyCount = 0;

      // int correctionCount = 0;

      int errorCount = 0;

      int idleCount = 0;

      int stepCount = 0;

  };

  // static const struct ut_counter emptyUtCount;

  ut_counter utCount;

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

  states utState = states::IDLE;

  states newState = states::IDLE;

  bool continue_on_obstacle = true;

  /**

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

};

} // end of namespace amiro

#endif // AMIRO_USERTHREAD_H_