Revision b24df8ad devices/DiWheelDrive/userthread.hpp

View differences:

devices/DiWheelDrive/userthread.hpp
17 17
// #define MAX_CORRECTION_STEPS 200
18 18
#define DOCKING_CORRECTION_TIMEOUT 200
19 19
#define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT
20
#define REVERSE_ADJUSTMENT_TIMEOUT 200
21
// #define MAX_RING_PROX_VALUE_DEVIATION 
20 22

  
21 23
// Thresh for wheel proxy sensors, when summed values fall below the state changes
22 24
// #define PROXY_WHEEL_THRESH 18000
......
38 40
#define ROTATION_DURATION 10000
39 41

  
40 42
#define RING_PROX_FRONT_THRESH 18000
41
#define PROX_MAX_VAL 65430
43
// #define PROX_MAX_VAL 65430
44
#define PROX_MAX_VAL 0xFFF0
45

  
42 46

  
43 47
// Threshold for failing to dock
44 48
#define DOCKING_ERROR_THRESH 3
45 49
#define CAN_TRANSMIT_STATE_THRESH 50
46

  
50
#define PROX_DEVIATION_MEAN_WINDOW 5
51
#define MAX_DEVIATION_CORRECTIONS 4
52
#define MAX_DEVIATION_FACTOR 45
53
#define DEVIATION_CORRECTION_DURATION 1000
54
#define DEVIATION_CORRECTION_SPEED 2000000
55
#define DEVIATION_CORRECTION_VALUE (DEVIATION_CORRECTION_SPEED / 2)
56
#define DEVIATION_DIST_THRESH 6000
47 57

  
48 58
namespace amiro {
49 59

  
......
71 81
      INACTIVE            = 13,
72 82
      CALIBRATION         = 14,
73 83
      CALIBRATION_CHECK   = 15,
84
      DEVIATION_CORRECTION = 16,
74 85
      DOCKING_ERROR       = -1,
75 86
      REVERSE_TIMEOUT_ERROR   = -2,
76 87
      CALIBRATION_ERROR   = -3,
......
107 118
    uint8_t meanWindow = 150;
108 119
  };
109 120
  
121
  struct deviation_correction {
122
    bool RCase = true;
123
    int8_t pCount = 0;
124
    int32_t proxbuf[PROX_DEVIATION_MEAN_WINDOW] = { 0 };
125
    int32_t currentDeviation = 0;
126
  };
127

  
110 128
  // static const struct ut_counter emptyUtCount;
111 129
  ut_counter utCount;
112 130
  proxy_ctrl pCtrl;
113 131
  bottom_prox_calibration proxCalib; 
132
  deviation_correction devCor;
133

  
134

  
114 135
  explicit UserThread();
115 136

  
116 137
  virtual ~UserThread();
......
132 153
  int getProxyRingSum();
133 154

  
134 155
  /**
156
  * Returns percentage of mean deviation between two given values.
157
  * It is intended to calculate the mean deviation between two proxy sensor
158
  * values. PROX_DEVIATION_MEAN_WINDOW determains the size of the mean window.
159
  * Keep in mind that initial results are wrong. 
160
  * */
161
  int32_t meanDeviation(uint16_t a, uint16_t b);
162

  
163
  /**
135 164
   * Check sectors around and stop if a thresh in one sector is detected.
136 165
   */
137 166
  void preventCollision(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]);

Also available in: Unified diff