Revision 10bf9cc0 devices/DiWheelDrive/userthread.hpp

View differences:

devices/DiWheelDrive/userthread.hpp
4 4 
#include <ch.hpp>
5 5 
#include <amiroosconf.h>
6 6 
#include <amiro/Color.h>
7 

  		




  
  7
  
    
// #include "global.hpp"


  		




  
  8
  
    
// #include "linefollow.hpp" 


  		




  
  9
  
    
#include <cmath>


  		




  8
  10
  
    

  		




  9
  11
  
    
// Speed when driving towards the docking station


  		




  10
  12
  
    
#define CHARGING_SPEED 5


  		




  ......




  12
  14
  
    
#define DIST_THRESH 100


  		




  13
  15
  
    
#define RELEASE_COUNT 200


  		




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


  		




  15
  
  
    
#define MAX_CORRECTION_STEPS 300


  		




  
  17
  
    
// #define MAX_CORRECTION_STEPS 200


  		




  
  18
  
    
#define DOCKING_CORRECTION_TIMEOUT 200


  		




  
  19
  
    
#define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT


  		




  
  20
  
    

  		




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


  		




  17
  
  
    
#define PROXY_WHEEL_THRESH 18000


  		




  
  22
  
    
// #define PROXY_WHEEL_THRESH 18000


  		




  18
  23
  
    
// Thresh for detecting obsticles


  		




  19
  
  
    
#define PROXY_RING_THRESH 15000


  		




  
  24
  
    
// #define PROXY_RING_THRESH 15000


  		




  20
  25
  
    

  		




  21
  
  
    
#define PUSH_BACK_COUNT 5


  		




  
  26
  
    
// #define PUSH_BACK_COUNT 5


  		




  
  27
  
    
#define PUSH_BACK_TIMEOUT 5


  		




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


  		




  23
  
  
    
#define WHITE_COUNT_THRESH 150


  		




  
  29
  
    
// #define WHITE_COUNT_THRESH 150


  		




  
  30
  
    
#define WHITE_DETETION_TIMEOUT 150


  		




  
  31
  
    
// #define RING_PROX_COUNT_THRESH 1000


  		




  
  32
  
    
#define RING_PROX_DETECTION_TIMEOUT 800


  		




  24
  33
  
    
// Rotation around 180 degrees in microradian


  		




  
  34
  
    
// #define ROTATION_180 3141592


  		




  25
  35
  
    
#define ROTATION_180 3141592


  		




  26
  36
  
    
// Rotation around -20 degrees in microradian


  		




  27
  37
  
    
#define ROTATION_20 -349065


  		




  ......




  31
  41
  
    
#define PROX_MAX_VAL 65430


  		




  32
  42
  
    

  		




  33
  43
  
    
// Threshold for failing to dock


  		




  34
  
  
    
#define DOCKING_ERROR_THRESH 4


  		




  
  44
  
    
#define DOCKING_ERROR_THRESH 3


  		




  
  45
  
    
#define CAN_TRANSMIT_STATE_THRESH 50


  		




  
  46
  
    

  		




  35
  47
  
    

  		




  36
  48
  
    
namespace amiro {


  		




  37
  49
  
    

  		




  ......




  41
  53
  
    

  		




  42
  54
  
    
  // Messages which can be received and trigger state changes


  		




  43
  55
  
    
  public:


  		




  44
  
  
    
    enum msg_content{


  		




  45
  
  
    
      STOP, 


  		




  46
  
  
    
      START,


  		




  47
  
  
    
      EDGE_LEFT,


  		




  48
  
  
    
      EDGE_RIGHT,


  		




  49
  
  
    
      FUZZY,


  		




  50
  
  
    
      DOCK,


  		




  51
  
  
    
      UNDOCK,


  		




  52
  
  
    
      CHARGE


  		




  53
  
  
    
    };


  		




  54
  56
  
    

  		




  55
  57
  
    
    // States of user thread state machine


  		




  56
  
  
    
    enum states : uint8_t{


  		




  
  58
  
    
    enum states : int8_t{


  		




  57
  59
  
    
      IDLE                = 0,


  		




  58
  60
  
    
      FOLLOW_LINE         = 1,


  		




  59
  61
  
    
      DETECT_STATION      = 2,


  		




  ......




  65
  67
  
    
      RELEASE             = 8,


  		




  66
  68
  
    
      RELEASE_TO_CORRECT  = 9,


  		




  67
  69
  
    
      CORRECT_POSITIONING = 10,


  		




  68
  
  
    
      ERROR               = 11,


  		




  69
  
  
    
      TURN                =12


  		




  
  70
  
    
      TURN                = 12,


  		




  
  71
  
    
      INACTIVE            = 13,


  		




  
  72
  
    
      CALIBRATION         = 14,


  		




  
  73
  
    
      CALIBRATION_CHECK   = 15,


  		




  
  74
  
    
      DOCKING_ERROR       = -1,


  		




  
  75
  
    
      REVERSE_TIMEOUT_ERROR   = -2,


  		




  
  76
  
    
      CALIBRATION_ERROR   = -3,


  		




  
  77
  
    
      WHITE_DETECTION_ERROR   = -4,


  		




  
  78
  
    
      PROXY_DETECTION_ERROR   = -5,


  		




  
  79
  
    
      NO_CHARGING_POWER_ERROR   = -6,


  		




  
  80
  
    
      UNKNOWN_STATE_ERROR   = -7


  		




  70
  81
  
    
    };


  		




  71
  82
  
    

  		




  72
  83
  
    
  struct ut_counter{


  		




  73
  84
  
    
      int whiteCount = 0;


  		




  74
  
  
    
      int proxyCount = 0;


  		




  
  85
  
    
      int ringProxCount = 0;


  		




  75
  86
  
    
      // int correctionCount = 0;


  		




  76
  87
  
    
      int errorCount = 0;


  		




  77
  
  
    
      int idleCount = 0;


  		




  
  88
  
    
      int stateCount = 0;


  		




  78
  89
  
    
      int stepCount = 0;


  		




  
  90
  
    
      uint32_t stateTime = 0;


  		




  
  91
  
    
  };


  		




  
  92
  
    

  		




  
  93
  
    
  struct proxy_ctrl {


  		




  
  94
  
    
    int threshLow = 100;


  		




  
  95
  
    
    int threshMid = 500;


  		




  
  96
  
    
    int threshHigh = 1500;


  		




  
  97
  
    
    // int threshHigh = 1500;


  		




  
  98
  
    
    int penalty = 100000;


  		




  
  99
  
    
    int pFactor = 310000;


  		




  
  100
  
    
    int staticCont = 0;


  		




  79
  101
  
    
  };


  		




  80
  102
  
    

  		




  
  103
  
    

  		




  
  104
  
    
  struct bottom_prox_calibration {


  		




  
  105
  
    
    bool calibrateBlack = true;


  		




  
  106
  
    
    uint32_t buf = 0;


  		




  
  107
  
    
    uint8_t meanWindow = 150;


  		




  
  108
  
    
  };


  		




  
  109
  
    
  


  		




  81
  110
  
    
  // static const struct ut_counter emptyUtCount;


  		




  82
  111
  
    
  ut_counter utCount;


  		




  83
  
  
    

  		




  
  112
  
    
  proxy_ctrl pCtrl;


  		




  
  113
  
    
  bottom_prox_calibration proxCalib; 


  		




  84
  114
  
    
  explicit UserThread();


  		




  85
  115
  
    

  		




  86
  116
  
    
  virtual ~UserThread();


  		




  ......




  100
  130
  
    
  bool checkPinVoltage();


  		




  101
  131
  
    
  bool checkPinEnabled();


  		




  102
  132
  
    
  int getProxyRingSum();


  		




  
  133
  
    

  		




  
  134
  
    
  /**


  		




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


  		




  
  136
  
    
   */


  		




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


  		




  
  138
  
    
  


  		




  
  139
  
    
  /**


  		




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


  		




  
  141
  
    
   */


  		




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


  		




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


  		




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


  		




  
  145
  
    
  void chargeAsLED();


  		




  103
  146
  
    
  


  		




  104
  147
  
    
  /**


  		




  105
  148
  
    
   * Returns true when front sensors reaching high values


  		




  ......




  108
  151
  
    
   */


  		




  109
  152
  
    
  bool checkFrontalObject();


  		




  110
  153
  
    

  		




  111
  
  
    
  states utState = states::IDLE;


  		




  112
  
  
    
  states newState = states::IDLE;


  		




  113
  
  
    
  bool continue_on_obstacle = true;


  		




  114
  154
  
    
  /**


  		




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


  		




  116
  156
  
    
   * 


  		




  ......




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


  		




  121
  161
  
    
   */


  		




  122
  162
  
    
  int checkDockingSuccess();


  		




  
  163
  
    

  		




  
  164
  
    
  // State Variables


  		




  
  165
  
    
  states prevState = states::IDLE;


  		




  
  166
  
    
  states currentState = states::IDLE;


  		




  
  167
  
    
  states newState = states::IDLE;


  		




  
  168
  
    

  		




  
  169
  
    
  bool continue_on_obstacle = true;


  		




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


  		




  
  171
  
    
  int rpmSpeed[2] = {0};


  		




  
  172
  
    
  int stop[2] = {0};


  		




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


  		




  
  174
  
    

  		




  123
  175
  
    
};


  		




  124
  176
  
    

  		




  125
  177
  
    
} // end of namespace amiro


  		












Also available in:  Unified diff