amiro-os / devices / DiWheelDrive / userthread.hpp @ 64cba697
History | View | Annotate | Download (5.213 KB)
1 | 58fe0e0b | Thomas Schöpping | #ifndef AMIRO_USERTHREAD_H_
|
---|---|---|---|
2 | #define AMIRO_USERTHREAD_H_
|
||
3 | |||
4 | #include <ch.hpp> |
||
5 | #include <amiroosconf.h> |
||
6 | 181f2892 | galberding | #include <amiro/Color.h> |
7 | 10bf9cc0 | galberding | // #include "global.hpp"
|
8 | 8dced1c9 | galberding | // #include "linefollow.hpp"
|
9 | 181f2892 | galberding | |
10 | d4c6efa9 | galberding | |
11 | |||
12 | // TODO: Clean up and sort defines!
|
||
13 | 181f2892 | galberding | // Speed when driving towards the docking station
|
14 | #define CHARGING_SPEED 5 |
||
15 | 61544eee | galberding | #define DETECTION_SPEED 10 |
16 | #define DIST_THRESH 100 |
||
17 | #define RELEASE_COUNT 200 |
||
18 | d4c6efa9 | galberding | #define SPEED_CONVERSION_FACTOR 1000000 //Convert value to um/s |
19 | 181f2892 | galberding | // Thresh to determain how much update steps should pass while alining
|
20 | 10bf9cc0 | galberding | // #define MAX_CORRECTION_STEPS 200
|
21 | #define DOCKING_CORRECTION_TIMEOUT 200 |
||
22 | #define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT |
||
23 | b24df8ad | galberding | #define REVERSE_ADJUSTMENT_TIMEOUT 200 |
24 | 8dced1c9 | galberding | // #define MAX_RING_PROX_VALUE_DEVIATION
|
25 | 10bf9cc0 | galberding | |
26 | 181f2892 | galberding | // Thresh for wheel proxy sensors, when summed values fall below the state changes
|
27 | 10bf9cc0 | galberding | // #define PROXY_WHEEL_THRESH 18000
|
28 | e2002d0e | galberding | // Thresh for detecting obsticles
|
29 | 10bf9cc0 | galberding | // #define PROXY_RING_THRESH 15000
|
30 | 27d4e1fa | galberding | |
31 | 10bf9cc0 | galberding | // #define PUSH_BACK_COUNT 5
|
32 | #define PUSH_BACK_TIMEOUT 5 |
||
33 | 181f2892 | galberding | // Thresh for how long (update steps) the front sensors are allowed to detect white
|
34 | 10bf9cc0 | galberding | // #define WHITE_COUNT_THRESH 150
|
35 | #define WHITE_DETETION_TIMEOUT 150 |
||
36 | // #define RING_PROX_COUNT_THRESH 1000
|
||
37 | 8dced1c9 | galberding | #define RING_PROX_DETECTION_TIMEOUT 400 |
38 | 181f2892 | galberding | // Rotation around 180 degrees in microradian
|
39 | 10bf9cc0 | galberding | // #define ROTATION_180 3141592
|
40 | 181f2892 | galberding | #define ROTATION_180 3141592 |
41 | // Rotation around -20 degrees in microradian
|
||
42 | #define ROTATION_20 -349065 |
||
43 | #define ROTATION_DURATION 10000 |
||
44 | |||
45 | 61544eee | galberding | #define RING_PROX_FRONT_THRESH 18000 |
46 | b24df8ad | galberding | // #define PROX_MAX_VAL 65430
|
47 | #define PROX_MAX_VAL 0xFFF0 |
||
48 | |||
49 | 58fe0e0b | Thomas Schöpping | |
50 | 27d4e1fa | galberding | // Threshold for failing to dock
|
51 | 10bf9cc0 | galberding | #define DOCKING_ERROR_THRESH 3 |
52 | #define CAN_TRANSMIT_STATE_THRESH 50 |
||
53 | 84b4c632 | galberding | #define PROX_DEVIATION_MEAN_WINDOW 3 |
54 | b24df8ad | galberding | #define MAX_DEVIATION_CORRECTIONS 4 |
55 | 84b4c632 | galberding | #define MAX_DEVIATION_FACTOR 35 |
56 | #define DEVIATION_CORRECTION_DURATION 900 |
||
57 | b24df8ad | galberding | #define DEVIATION_CORRECTION_SPEED 2000000 |
58 | #define DEVIATION_CORRECTION_VALUE (DEVIATION_CORRECTION_SPEED / 2) |
||
59 | 84b4c632 | galberding | #define DEVIATION_DIST_THRESH 25000 |
60 | 27d4e1fa | galberding | |
61 | 3c3c3bb9 | galberding | |
62 | |||
63 | // Map Tracking Parameters
|
||
64 | // enable amiro map to continuously build internal map representation
|
||
65 | 64cba697 | galberding | #define AMIRO_MAP_AUTO_TRACKING false |
66 | 3c3c3bb9 | galberding | |
67 | |||
68 | |||
69 | 58fe0e0b | Thomas Schöpping | namespace amiro {
|
70 | |||
71 | 181f2892 | galberding | |
72 | 58fe0e0b | Thomas Schöpping | class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE> |
73 | { |
||
74 | 181f2892 | galberding | |
75 | // Messages which can be received and trigger state changes
|
||
76 | public:
|
||
77 | |||
78 | 64cba697 | galberding | struct ut_counter {
|
79 | int whiteCount = 0; |
||
80 | int ringProxCount = 0; |
||
81 | // int correctionCount = 0;
|
||
82 | int errorCount = 0; |
||
83 | int stateCount = 0; |
||
84 | int stepCount = 0; |
||
85 | uint32_t stateTime = 0;
|
||
86 | 10bf9cc0 | galberding | }; |
87 | |||
88 | struct proxy_ctrl {
|
||
89 | int threshLow = 100; |
||
90 | int threshMid = 500; |
||
91 | int threshHigh = 1500; |
||
92 | // int threshHigh = 1500;
|
||
93 | int penalty = 100000; |
||
94 | int pFactor = 310000; |
||
95 | int staticCont = 0; |
||
96 | 27d4e1fa | galberding | }; |
97 | 181f2892 | galberding | |
98 | 10bf9cc0 | galberding | |
99 | struct bottom_prox_calibration {
|
||
100 | bool calibrateBlack = true; |
||
101 | uint32_t buf = 0;
|
||
102 | uint8_t meanWindow = 150;
|
||
103 | }; |
||
104 | 8dced1c9 | galberding | |
105 | b24df8ad | galberding | struct deviation_correction {
|
106 | bool RCase = true; |
||
107 | int8_t pCount = 0;
|
||
108 | int32_t proxbuf[PROX_DEVIATION_MEAN_WINDOW] = { 0 };
|
||
109 | int32_t currentDeviation = 0;
|
||
110 | }; |
||
111 | |||
112 | 27d4e1fa | galberding | // static const struct ut_counter emptyUtCount;
|
113 | ut_counter utCount; |
||
114 | 10bf9cc0 | galberding | proxy_ctrl pCtrl; |
115 | 8dced1c9 | galberding | bottom_prox_calibration proxCalib; |
116 | b24df8ad | galberding | deviation_correction devCor; |
117 | |||
118 | |||
119 | 58fe0e0b | Thomas Schöpping | explicit UserThread();
|
120 | |||
121 | virtual ~UserThread();
|
||
122 | |||
123 | virtual msg_t main();
|
||
124 | 181f2892 | galberding | |
125 | private:
|
||
126 | void setRpmSpeed(const int (&rpmSpeed)[2]); |
||
127 | c9fa414d | galberding | void setRpmSpeedFuzzy(const int (&rpmSpeed)[2]); |
128 | 181f2892 | galberding | void lightOneLed(Color color, int idx); |
129 | void lightAllLeds(Color color);
|
||
130 | /**
|
||
131 | * Uses light ring indicate the state of charge.
|
||
132 | */
|
||
133 | void showChargingState();
|
||
134 | void checkForMotion();
|
||
135 | bool checkPinVoltage();
|
||
136 | bool checkPinEnabled();
|
||
137 | c9fa414d | galberding | int getProxyRingSum();
|
138 | 10bf9cc0 | galberding | |
139 | /**
|
||
140 | b24df8ad | galberding | * Returns percentage of mean deviation between two given values.
|
141 | * It is intended to calculate the mean deviation between two proxy sensor
|
||
142 | * values. PROX_DEVIATION_MEAN_WINDOW determains the size of the mean window.
|
||
143 | 8dced1c9 | galberding | * Keep in mind that initial results are wrong.
|
144 | b24df8ad | galberding | * */
|
145 | int32_t meanDeviation(uint16_t a, uint16_t b); |
||
146 | |||
147 | /**
|
||
148 | 10bf9cc0 | galberding | * Check sectors around and stop if a thresh in one sector is detected.
|
149 | */
|
||
150 | void preventCollision(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]); |
||
151 | 8dced1c9 | galberding | |
152 | 10bf9cc0 | galberding | /**
|
153 | * Same as prevent collision but also lowers the speed when object is detected.
|
||
154 | */
|
||
155 | void proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]); |
||
156 | void getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]); |
||
157 | void getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax); |
||
158 | void chargeAsLED();
|
||
159 | 8dced1c9 | galberding | |
160 | fbcb25cc | galberding | /**
|
161 | * Returns true when front sensors reaching high values
|
||
162 | * and all others are low. This indicates that the loading station is ahead.
|
||
163 | * If other sensores are blocked too, indicates that someone grabs the robot.
|
||
164 | */
|
||
165 | bool checkFrontalObject();
|
||
166 | c9fa414d | galberding | |
167 | 181f2892 | galberding | /**
|
168 | * Check if current position changes when the wheel are deactivated.
|
||
169 | 8dced1c9 | galberding | *
|
170 | 181f2892 | galberding | * When AMiRo drives towards the loading station, it stops when a specific marker is reached.
|
171 | * In order to validate that the AMiRo is correctly positioned in the loading station
|
||
172 | * the wheels are turned off. When the position remains the same the docking procedure
|
||
173 | 8dced1c9 | galberding | * was successful (return 1) otherwise a correction is needed (return 0).
|
174 | 181f2892 | galberding | */
|
175 | int checkDockingSuccess();
|
||
176 | 10bf9cc0 | galberding | |
177 | 3c3c3bb9 | galberding | |
178 | 10bf9cc0 | galberding | |
179 | bool continue_on_obstacle = true; |
||
180 | uint16_t rProx[8]; // buffer for ring proxy values |
||
181 | int rpmSpeed[2] = {0}; |
||
182 | int stop[2] = {0}; |
||
183 | int turn[2] = {5,-5}; |
||
184 | |||
185 | 58fe0e0b | Thomas Schöpping | }; |
186 | |||
187 | } // end of namespace amiro
|
||
188 | |||
189 | #endif // AMIRO_USERTHREAD_H_ |