Revision b24df8ad devices/DiWheelDrive/userthread.hpp
| 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