amiro-os / devices / DiWheelDrive / userthread.cpp @ 309980f0
History | View | Annotate | Download (36.847 KB)
1 | d4c6efa9 | galberding | #include "userthread.hpp" |
---|---|---|---|
2 | 3c3c3bb9 | galberding | #include "amiro/Constants.h" |
3 | e404e6c0 | galberding | #include "amiro_map.hpp" |
4 | d02c536e | galberding | #include "global.hpp" |
5 | #include "linefollow.hpp" |
||
6 | e404e6c0 | galberding | |
7 | 58fe0e0b | Thomas Schöpping | using namespace amiro; |
8 | |||
9 | extern Global global;
|
||
10 | |||
11 | // a buffer for the z-value of the accelerometer
|
||
12 | int16_t accel_z; |
||
13 | 5d138bca | galberding | bool running = false; |
14 | 58fe0e0b | Thomas Schöpping | |
15 | |||
16 | 181f2892 | galberding | /**
|
17 | * Set speed.
|
||
18 | 8dced1c9 | galberding | *
|
19 | 181f2892 | galberding | * @param rpmSpeed speed for left and right wheel in rounds/min
|
20 | */
|
||
21 | c9fa414d | galberding | void UserThread::setRpmSpeedFuzzy(const int (&rpmSpeed)[2]) { |
22 | 5d138bca | galberding | global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
23 | 58fe0e0b | Thomas Schöpping | } |
24 | |||
25 | c9fa414d | galberding | void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) { |
26 | global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]); |
||
27 | } |
||
28 | |||
29 | 181f2892 | galberding | void UserThread::lightOneLed(Color color, int idx){ |
30 | 5d138bca | galberding | global.robot.setLightColor(idx, Color(color)); |
31 | } |
||
32 | 58fe0e0b | Thomas Schöpping | |
33 | 181f2892 | galberding | void UserThread::lightAllLeds(Color color){
|
34 | 5d138bca | galberding | int led = 0; |
35 | for(led=0; led<8; led++){ |
||
36 | lightOneLed(color, led); |
||
37 | } |
||
38 | 58fe0e0b | Thomas Schöpping | } |
39 | |||
40 | 181f2892 | galberding | void UserThread::showChargingState(){
|
41 | uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
|
||
42 | Color color = Color::GREEN; |
||
43 | if (numLeds <= 2){ |
||
44 | color = Color::RED; |
||
45 | }else if(numLeds <= 6){ |
||
46 | color = Color::YELLOW; |
||
47 | } |
||
48 | for (int i=0; i<numLeds; i++){ |
||
49 | lightOneLed(color, i); |
||
50 | this->sleep(300); |
||
51 | 9c46b728 | galberding | } |
52 | 181f2892 | galberding | this->sleep(1000); |
53 | lightAllLeds(Color::BLACK); |
||
54 | 9c46b728 | galberding | } |
55 | |||
56 | 10bf9cc0 | galberding | void UserThread::chargeAsLED(){
|
57 | uint8_t numLeds = global.robot.getPowerStatus().state_of_charge / 12;
|
||
58 | Color color = Color::GREEN; |
||
59 | if (numLeds <= 2){ |
||
60 | color = Color::RED; |
||
61 | }else if(numLeds <= 6){ |
||
62 | color = Color::YELLOW; |
||
63 | } |
||
64 | for (int i=0; i<numLeds; i++){ |
||
65 | lightOneLed(color, i); |
||
66 | // this->sleep(300);
|
||
67 | } |
||
68 | // this->sleep(1000);
|
||
69 | // lightAllLeds(Color::BLACK);
|
||
70 | } |
||
71 | |||
72 | // ----------------------------------------------------------------
|
||
73 | |||
74 | void UserThread::getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]){ |
||
75 | for (int i=0; i<8; i++){ |
||
76 | sProx[i] = (proxVals[i] < proxVals[(i+1) % 8]) ? proxVals[i] : proxVals[(i+1) % 8]; |
||
77 | // chprintf((BaseSequentialStream*)&global.sercanmux1, "%d: %d, ", i, sProx[i]);
|
||
78 | 8dced1c9 | galberding | |
79 | 10bf9cc0 | galberding | } |
80 | // chprintf((BaseSequentialStream*)&global.sercanmux1, "\n");
|
||
81 | |||
82 | } |
||
83 | |||
84 | |||
85 | void UserThread::getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax){ |
||
86 | for (int i=2; i<5; i++){ |
||
87 | sPMax = (sPMax < sProx[i]) ? sProx[i] : sPMax; |
||
88 | } |
||
89 | } |
||
90 | |||
91 | void UserThread::proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]){ |
||
92 | int i;
|
||
93 | uint16_t sProx[8];
|
||
94 | int32_t sPMax = 0;
|
||
95 | getProxySectorVals(proxVals, sProx); |
||
96 | getMaxFrontSectorVal(sProx, sPMax); |
||
97 | 8dced1c9 | galberding | |
98 | 10bf9cc0 | galberding | int32_t speedL = rpmSpeed[0] - (sPMax * pCtrl.pFactor);
|
99 | int32_t speedR = rpmSpeed[1] - (sPMax * pCtrl.pFactor);
|
||
100 | |||
101 | |||
102 | |||
103 | if(sPMax > pCtrl.threshMid){
|
||
104 | rpmSpeed[0] = 0; |
||
105 | rpmSpeed[1] = 0; |
||
106 | pCtrl.staticCont++; |
||
107 | }else if((speedL > 0) || (speedR > 0)){ |
||
108 | pCtrl.staticCont = 0;
|
||
109 | rpmSpeed[0] = speedL;
|
||
110 | rpmSpeed[1] = speedR;
|
||
111 | }else{
|
||
112 | rpmSpeed[0] = 4000000 + (rpmSpeed[0] - global.rpmForward[0] * 1000000); |
||
113 | rpmSpeed[1] = 4000000 + (rpmSpeed[1] - global.rpmForward[0] * 1000000); |
||
114 | } |
||
115 | |||
116 | for(i=4; i<5; i++){ |
||
117 | if ((proxVals[i] > pCtrl.threshMid) && (proxVals[i+1] > pCtrl.threshLow)){ |
||
118 | rpmSpeed[0] = -5000000 ; |
||
119 | rpmSpeed[1] = -5000000 ; |
||
120 | // pCtrl.staticCont++;
|
||
121 | break;
|
||
122 | } |
||
123 | } |
||
124 | chargeAsLED(); |
||
125 | 8dced1c9 | galberding | |
126 | 10bf9cc0 | galberding | // chprintf((BaseSequentialStream*)&global.sercanmux1, "Max: %d factor: %d, Panel: %d SpeedL: %d SpeedR: %d ActualL: %d ActualR: %d\n",sPMax, pCtrl.pFactor, sPMax * pCtrl.pFactor, speedL, speedR, rpmSpeed[0], rpmSpeed[1]);
|
127 | |||
128 | |||
129 | } |
||
130 | // -------------------------------------------------------------------
|
||
131 | |||
132 | |||
133 | void UserThread::preventCollision( int (&rpmSpeed)[2], uint16_t (&proxVals)[8]) { |
||
134 | |||
135 | if((proxVals[3] > pCtrl.threshLow) || (proxVals[4] > pCtrl.threshLow)){ |
||
136 | rpmSpeed[0] = rpmSpeed[0] / 2; |
||
137 | rpmSpeed[1] = rpmSpeed[1] / 2; |
||
138 | } |
||
139 | |||
140 | if((proxVals[3] > pCtrl.threshMid) || (proxVals[4] > pCtrl.threshMid)){ |
||
141 | d4c6efa9 | galberding | rpmSpeed[0] = rpmSpeed[0] / 3; |
142 | rpmSpeed[1] = rpmSpeed[1] / 3; |
||
143 | 10bf9cc0 | galberding | } |
144 | |||
145 | if((proxVals[3] > pCtrl.threshHigh) || (proxVals[4] > pCtrl.threshHigh)){ |
||
146 | rpmSpeed[0] = 0; |
||
147 | rpmSpeed[1] = 0; |
||
148 | utCount.ringProxCount++; |
||
149 | }else{
|
||
150 | utCount.ringProxCount = 0;
|
||
151 | } |
||
152 | 8dced1c9 | galberding | |
153 | 10bf9cc0 | galberding | } |
154 | |||
155 | |||
156 | 9c46b728 | galberding | /**
|
157 | * Blocks as long as the position changes.
|
||
158 | */
|
||
159 | 181f2892 | galberding | void UserThread::checkForMotion(){
|
160 | 8dced1c9 | galberding | bool motion = true; |
161 | 9c46b728 | galberding | int led = 0; |
162 | types::position oldPos = global.odometry.getPosition(); |
||
163 | while(motion){
|
||
164 | 8dced1c9 | galberding | this->sleep(200); |
165 | 9c46b728 | galberding | types::position tmp = global.odometry.getPosition(); |
166 | 8dced1c9 | galberding | motion = oldPos.x != tmp.x; //abs(oldPos.x - tmp.x)+ abs(oldPos.y - tmp.y)+abs(oldPos.z - tmp.z);
|
167 | 9c46b728 | galberding | oldPos = tmp; |
168 | 8dced1c9 | galberding | global.robot.setLightColor((led + 1) % 8, Color(Color::YELLOW)); |
169 | global.robot.setLightColor(led % 8, Color(Color::BLACK));
|
||
170 | led++; |
||
171 | 9c46b728 | galberding | } |
172 | 10bf9cc0 | galberding | lightAllLeds(Color::BLACK); |
173 | 9c46b728 | galberding | } |
174 | |||
175 | fbcb25cc | galberding | bool UserThread::checkFrontalObject(){
|
176 | 10bf9cc0 | galberding | uint32_t thresh = pCtrl.threshMid; |
177 | fbcb25cc | galberding | uint32_t prox; |
178 | for(int i=0; i<8; i++){ |
||
179 | prox = global.robot.getProximityRingValue(i); |
||
180 | if((i == 3) || (i == 4)){ |
||
181 | if(prox < thresh){
|
||
182 | return false; |
||
183 | } |
||
184 | }else{
|
||
185 | if(prox > thresh){
|
||
186 | return false; |
||
187 | } |
||
188 | } |
||
189 | } |
||
190 | return true; |
||
191 | } |
||
192 | |||
193 | 181f2892 | galberding | bool UserThread::checkPinVoltage(){
|
194 | 8dced1c9 | galberding | return global.ltc4412.isPluggedIn();
|
195 | 181f2892 | galberding | } |
196 | 9c46b728 | galberding | |
197 | 181f2892 | galberding | bool UserThread::checkPinEnabled(){
|
198 | return global.ltc4412.isEnabled();
|
||
199 | } |
||
200 | |||
201 | int UserThread::checkDockingSuccess(){
|
||
202 | // setRpmSpeed(stop);
|
||
203 | checkForMotion(); |
||
204 | 9c46b728 | galberding | int success = 0; |
205 | 10bf9cc0 | galberding | // global.odometry.resetPosition();
|
206 | 9c46b728 | galberding | types::position start = global.startPos = global.odometry.getPosition(); |
207 | 27d4e1fa | galberding | global.motorcontrol.setMotorEnable(false);
|
208 | this->sleep(1000); |
||
209 | 181f2892 | galberding | types::position stop_ = global.endPos = global.odometry.getPosition(); |
210 | 8dced1c9 | galberding | |
211 | 9c46b728 | galberding | // Amiro moved, docking was not successful
|
212 | 10bf9cc0 | galberding | // if ((start.x + stop_.x) || (start.y + stop_.y)){
|
213 | 84b4c632 | galberding | if (abs(start.x - stop_.x) > 200 /* || (start.y + stop_.y) */){ |
214 | 181f2892 | galberding | lightAllLeds(Color::RED); |
215 | // Enable Motor again if docking was not successful
|
||
216 | 27d4e1fa | galberding | global.motorcontrol.setMotorEnable(true);
|
217 | 9c46b728 | galberding | success = 0;
|
218 | }else{
|
||
219 | 181f2892 | galberding | lightAllLeds(Color::GREEN); |
220 | 9c46b728 | galberding | success = 1;
|
221 | } |
||
222 | 8dced1c9 | galberding | |
223 | 61544eee | galberding | // this->sleep(500);
|
224 | 181f2892 | galberding | lightAllLeds(Color::BLACK); |
225 | 9c46b728 | galberding | return success;
|
226 | } |
||
227 | |||
228 | c9fa414d | galberding | int UserThread::getProxyRingSum(){
|
229 | int prox_sum = 0; |
||
230 | e2002d0e | galberding | for(int i=0; i<8;i++){ |
231 | prox_sum += global.robot.getProximityRingValue(i);; |
||
232 | } |
||
233 | return prox_sum;
|
||
234 | } |
||
235 | |||
236 | b24df8ad | galberding | int32_t UserThread::meanDeviation(uint16_t a, uint16_t b){ |
237 | int32_t diff = a - b; |
||
238 | 8dced1c9 | galberding | int32_t res = 0;
|
239 | b24df8ad | galberding | devCor.proxbuf[devCor.pCount] = (diff*100)/((a+b)/2); |
240 | for (int i = 0; i< PROX_DEVIATION_MEAN_WINDOW; i++){ |
||
241 | res += devCor.proxbuf[i]; |
||
242 | } |
||
243 | devCor.pCount++; |
||
244 | devCor.pCount = devCor.pCount % PROX_DEVIATION_MEAN_WINDOW; |
||
245 | |||
246 | devCor.currentDeviation = res / PROX_DEVIATION_MEAN_WINDOW; |
||
247 | return devCor.currentDeviation;
|
||
248 | } |
||
249 | |||
250 | e404e6c0 | galberding | void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES],
|
251 | uint8_t id, uint8_t l, uint8_t r, uint8_t att) { |
||
252 | map[id][0] = l;
|
||
253 | map[id][1] = r;
|
||
254 | map[id][2] = att;
|
||
255 | } |
||
256 | e2002d0e | galberding | |
257 | 58fe0e0b | Thomas Schöpping | UserThread::UserThread() : |
258 | chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
||
259 | { |
||
260 | } |
||
261 | |||
262 | UserThread::~UserThread() |
||
263 | { |
||
264 | } |
||
265 | |||
266 | msg_t |
||
267 | UserThread::main() |
||
268 | { |
||
269 | 5d138bca | galberding | /*
|
270 | * SETUP
|
||
271 | */
|
||
272 | 181f2892 | galberding | // User thread state:
|
273 | |||
274 | 5d138bca | galberding | for (uint8_t led = 0; led < 8; ++led) { |
275 | global.robot.setLightColor(led, Color(Color::BLACK)); |
||
276 | } |
||
277 | 3c3c3bb9 | galberding | |
278 | // State Variables
|
||
279 | ut_states prevState = ut_states::UT_IDLE; |
||
280 | ut_states currentState = ut_states::UT_INACTIVE; |
||
281 | ut_states newState = ut_states::UT_INACTIVE; |
||
282 | |||
283 | |||
284 | 5d138bca | galberding | running = false;
|
285 | 10bf9cc0 | galberding | LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT; |
286 | 5d138bca | galberding | LineFollow lf(&global); |
287 | e404e6c0 | galberding | AmiroMap map(&global); |
288 | 5d138bca | galberding | /*
|
289 | * LOOP
|
||
290 | */
|
||
291 | while (!this->shouldTerminate()) |
||
292 | { |
||
293 | 181f2892 | galberding | /*
|
294 | * read accelerometer z-value
|
||
295 | */
|
||
296 | accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
||
297 | 8dced1c9 | galberding | |
298 | if (accel_z < -900 /*-0.9g*/) { |
||
299 | 181f2892 | galberding | // Start line following when AMiRo is rotated
|
300 | 3c3c3bb9 | galberding | if(currentState == ut_states::UT_INACTIVE){
|
301 | newState = ut_states::UT_FOLLOW_LINE; |
||
302 | 181f2892 | galberding | }else{
|
303 | 3c3c3bb9 | galberding | newState = ut_states::UT_IDLE; |
304 | 181f2892 | galberding | } |
305 | lightAllLeds(Color::GREEN); |
||
306 | this->sleep(1000); |
||
307 | lightAllLeds(Color::BLACK); |
||
308 | c76baf23 | Georg Alberding | |
309 | d607fcef | galberding | // If message was received handle it here:
|
310 | } else if(global.msgReceived){ |
||
311 | global.msgReceived = false;
|
||
312 | 181f2892 | galberding | // running = true;
|
313 | d607fcef | galberding | switch(global.lfStrategy){
|
314 | e404e6c0 | galberding | case msg_content::MSG_START:
|
315 | 3c3c3bb9 | galberding | newState = ut_states::UT_CALIBRATION_CHECK; |
316 | 10bf9cc0 | galberding | break;
|
317 | e404e6c0 | galberding | case msg_content::MSG_STOP:
|
318 | 3c3c3bb9 | galberding | newState = ut_states::UT_IDLE; |
319 | 10bf9cc0 | galberding | break;
|
320 | e404e6c0 | galberding | case msg_content::MSG_EDGE_RIGHT:
|
321 | 3c3c3bb9 | galberding | // newState = ut_states::UT_FOLLOW_LINE;
|
322 | e404e6c0 | galberding | lStrategy = LineFollowStrategy::EDGE_RIGHT; |
323 | 10bf9cc0 | galberding | break;
|
324 | e404e6c0 | galberding | case msg_content::MSG_EDGE_LEFT:
|
325 | 3c3c3bb9 | galberding | // newState = ut_states::UT_FOLLOW_LINE;
|
326 | e404e6c0 | galberding | lStrategy = LineFollowStrategy::EDGE_LEFT; |
327 | 10bf9cc0 | galberding | break;
|
328 | e404e6c0 | galberding | case msg_content::MSG_FUZZY:
|
329 | 3c3c3bb9 | galberding | // newState = ut_states::UT_FOLLOW_LINE;
|
330 | e404e6c0 | galberding | lStrategy = LineFollowStrategy::FUZZY; |
331 | 10bf9cc0 | galberding | break;
|
332 | e404e6c0 | galberding | case msg_content::MSG_DOCK:
|
333 | 3c3c3bb9 | galberding | newState = ut_states::UT_DETECT_STATION; |
334 | 10bf9cc0 | galberding | break;
|
335 | e404e6c0 | galberding | case msg_content::MSG_UNDOCK:
|
336 | 3c3c3bb9 | galberding | newState = ut_states::UT_RELEASE; |
337 | e404e6c0 | galberding | break;
|
338 | case msg_content::MSG_CHARGE:
|
||
339 | 3c3c3bb9 | galberding | newState = ut_states::UT_CHARGING; |
340 | 10bf9cc0 | galberding | break;
|
341 | e404e6c0 | galberding | case msg_content::MSG_RESET_ODOMETRY:
|
342 | global.odometry.resetPosition(); |
||
343 | 10bf9cc0 | galberding | break;
|
344 | e404e6c0 | galberding | case msg_content::MSG_CALIBRATE_BLACK:
|
345 | proxCalib.calibrateBlack = true;
|
||
346 | // global.odometry.resetPosition();
|
||
347 | 3c3c3bb9 | galberding | newState = ut_states::UT_CALIBRATION; |
348 | 10bf9cc0 | galberding | break;
|
349 | e404e6c0 | galberding | case msg_content::MSG_CALIBRATE_WHITE:
|
350 | proxCalib.calibrateBlack = false;
|
||
351 | 3c3c3bb9 | galberding | newState = ut_states::UT_CALIBRATION; |
352 | 10bf9cc0 | galberding | break;
|
353 | e404e6c0 | galberding | case msg_content::MSG_TEST_MAP_STATE:
|
354 | 3c3c3bb9 | galberding | if (AMIRO_MAP_AUTO_TRACKING){
|
355 | newState = ut_states::UT_TEST_MAP_AUTO_STATE; |
||
356 | }else {
|
||
357 | newState = ut_states::UT_TEST_MAP_STATE; |
||
358 | } |
||
359 | 10bf9cc0 | galberding | break;
|
360 | 309980f0 | galberding | case msg_content::MSG_SET_DIST_THRESH:
|
361 | |||
362 | e404e6c0 | galberding | |
363 | 309980f0 | galberding | break;
|
364 | case msg_content::MSG_GET_MAP_INFO:
|
||
365 | break;
|
||
366 | e404e6c0 | galberding | default:
|
367 | 3c3c3bb9 | galberding | newState = ut_states::UT_IDLE; |
368 | 10bf9cc0 | galberding | break;
|
369 | d607fcef | galberding | } |
370 | 5d138bca | galberding | } |
371 | 10bf9cc0 | galberding | // newState = currentState;
|
372 | d607fcef | galberding | |
373 | 8dced1c9 | galberding | // Get sensor data
|
374 | 3c3c3bb9 | galberding | uint16_t WL = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
375 | uint16_t WR = global.vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset(); |
||
376 | 019224ff | galberding | for(int i=0; i<8;i++){ |
377 | rProx[i] = global.robot.getProximityRingValue(i); |
||
378 | } |
||
379 | b24df8ad | galberding | |
380 | // Continously update devication values
|
||
381 | meanDeviation(rProx[0] & 0xFFF0, rProx[7] & 0xFFF0); |
||
382 | 181f2892 | galberding | // int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
383 | // int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
||
384 | 10bf9cc0 | galberding | switch(currentState){
|
385 | 3c3c3bb9 | galberding | case ut_states::UT_INACTIVE:
|
386 | 10bf9cc0 | galberding | // Dummy state to deactivate every interaction
|
387 | break;
|
||
388 | // ---------------------------------------
|
||
389 | 3c3c3bb9 | galberding | case ut_states::UT_CALIBRATION_CHECK:
|
390 | 10bf9cc0 | galberding | // global.robot.calibrate();
|
391 | if(global.linePID.BThresh >= global.linePID.WThresh){
|
||
392 | 3c3c3bb9 | galberding | newState = ut_states::UT_CALIBRATION_ERROR; |
393 | 10bf9cc0 | galberding | }else{
|
394 | 3c3c3bb9 | galberding | newState = ut_states::UT_FOLLOW_LINE; |
395 | 10bf9cc0 | galberding | } |
396 | break;
|
||
397 | // ---------------------------------------
|
||
398 | 3c3c3bb9 | galberding | case ut_states::UT_CALIBRATION:
|
399 | 10bf9cc0 | galberding | /* Calibrate the global thresholds for black or white.
|
400 | This values will be used by the line follow object
|
||
401 | */
|
||
402 | |||
403 | proxCalib.buf = 0;
|
||
404 | if(proxCalib.calibrateBlack){
|
||
405 | chprintf((BaseSequentialStream*)&global.sercanmux1, "Black Calibration, Place AMiRo on black Surface!\n");
|
||
406 | global.robot.calibrate(); |
||
407 | } |
||
408 | for(int i=0; i <= proxCalib.meanWindow; i++){ |
||
409 | 8dced1c9 | galberding | proxCalib.buf += global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset() |
410 | + global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
||
411 | 10bf9cc0 | galberding | this->sleep(CAN::UPDATE_PERIOD);
|
412 | } |
||
413 | proxCalib.buf = proxCalib.buf / (2*proxCalib.meanWindow);
|
||
414 | 8dced1c9 | galberding | |
415 | 10bf9cc0 | galberding | if(proxCalib.calibrateBlack){
|
416 | global.linePID.BThresh = proxCalib.buf; |
||
417 | }else {
|
||
418 | global.linePID.WThresh = proxCalib.buf; |
||
419 | } |
||
420 | chprintf((BaseSequentialStream*)&global.sercanmux1, "Black: %d, White: %d!\n", global.linePID.BThresh, global.linePID.WThresh);
|
||
421 | |||
422 | 3c3c3bb9 | galberding | newState = ut_states::UT_IDLE; |
423 | 10bf9cc0 | galberding | break;
|
424 | // ---------------------------------------
|
||
425 | 3c3c3bb9 | galberding | case ut_states::UT_IDLE:
|
426 | 27d4e1fa | galberding | global.motorcontrol.setMotorEnable(true);
|
427 | 181f2892 | galberding | setRpmSpeed(stop); |
428 | if(/* checkPinVoltage() && */ checkPinEnabled()){ |
||
429 | global.robot.requestCharging(0);
|
||
430 | } |
||
431 | 10bf9cc0 | galberding | // pCtrl.pFactor = 0;
|
432 | pCtrl.staticCont = 0;
|
||
433 | fbcb25cc | galberding | utCount.whiteCount = 0;
|
434 | 10bf9cc0 | galberding | utCount.ringProxCount = 0;
|
435 | 27d4e1fa | galberding | utCount.errorCount = 0;
|
436 | 3c3c3bb9 | galberding | newState = ut_states::UT_INACTIVE; |
437 | 10bf9cc0 | galberding | break;
|
438 | 181f2892 | galberding | // ---------------------------------------
|
439 | 3c3c3bb9 | galberding | case ut_states::UT_FOLLOW_LINE:
|
440 | 181f2892 | galberding | // Set correct forward speed to every strategy
|
441 | if (global.forwardSpeed != global.rpmForward[0]){ |
||
442 | global.forwardSpeed = global.rpmForward[0];
|
||
443 | } |
||
444 | 8dced1c9 | galberding | |
445 | 181f2892 | galberding | if(lf.getStrategy() != lStrategy){
|
446 | 9c46b728 | galberding | lf.setStrategy(lStrategy); |
447 | } |
||
448 | 5d138bca | galberding | |
449 | 181f2892 | galberding | if(lf.followLine(rpmSpeed)){
|
450 | fbcb25cc | galberding | utCount.whiteCount++; |
451 | 10bf9cc0 | galberding | if(utCount.whiteCount >= WHITE_DETETION_TIMEOUT){
|
452 | setRpmSpeed(stop); |
||
453 | utCount.whiteCount = 0;
|
||
454 | 3c3c3bb9 | galberding | newState = ut_states::UT_WHITE_DETECTION_ERROR; |
455 | 9c46b728 | galberding | } |
456 | 181f2892 | galberding | }else{
|
457 | fbcb25cc | galberding | utCount.whiteCount = 0;
|
458 | 9c46b728 | galberding | } |
459 | e2002d0e | galberding | |
460 | 10bf9cc0 | galberding | preventCollision(rpmSpeed, rProx); |
461 | // proxSectorSpeedCorrection(rpmSpeed, rProx);
|
||
462 | 61544eee | galberding | |
463 | 10bf9cc0 | galberding | if(utCount.ringProxCount > RING_PROX_DETECTION_TIMEOUT){
|
464 | utCount.ringProxCount = 0;
|
||
465 | 8dced1c9 | galberding | |
466 | d4c6efa9 | galberding | |
467 | checkForMotion(); |
||
468 | // Check if only front sensors are active
|
||
469 | if (checkFrontalObject()) {
|
||
470 | // global.distcontrol.setTargetPosition(0, 2792526, ROTATION_DURATION);
|
||
471 | // // BaseThread::sleep(8000);
|
||
472 | // checkForMotion();
|
||
473 | this->utCount.whiteCount = 0; |
||
474 | 3c3c3bb9 | galberding | newState = ut_states::UT_TURN; |
475 | d4c6efa9 | galberding | // lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT);
|
476 | } else {
|
||
477 | 3c3c3bb9 | galberding | newState = ut_states::UT_PROXY_DETECTION_ERROR; |
478 | d4c6efa9 | galberding | } |
479 | 61544eee | galberding | } |
480 | |||
481 | c9fa414d | galberding | if (lf.getStrategy() == LineFollowStrategy::FUZZY){
|
482 | setRpmSpeedFuzzy(rpmSpeed); |
||
483 | }else{
|
||
484 | |||
485 | setRpmSpeed(rpmSpeed); |
||
486 | } |
||
487 | 8dced1c9 | galberding | |
488 | 10bf9cc0 | galberding | break;
|
489 | 181f2892 | galberding | // ---------------------------------------
|
490 | 3c3c3bb9 | galberding | case ut_states::UT_TURN:{
|
491 | d4c6efa9 | galberding | // Check the line strategy in order to continue driving on the right side
|
492 | int factor = SPEED_CONVERSION_FACTOR;
|
||
493 | int frontL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
||
494 | int frontR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
||
495 | 8dced1c9 | galberding | int blackSensor = 0; |
496 | d4c6efa9 | galberding | if (lf.getStrategy() == LineFollowStrategy::EDGE_RIGHT) {
|
497 | factor = -factor; |
||
498 | 8dced1c9 | galberding | blackSensor = frontL; |
499 | }else{
|
||
500 | blackSensor = frontR; |
||
501 | d4c6efa9 | galberding | } |
502 | 8dced1c9 | galberding | |
503 | d4c6efa9 | galberding | rpmSpeed[0] = factor * CHARGING_SPEED;
|
504 | rpmSpeed[1] = -factor * CHARGING_SPEED;
|
||
505 | setRpmSpeed(rpmSpeed); |
||
506 | 8dced1c9 | galberding | |
507 | if ((blackSensor >= global.linePID.WThresh )){
|
||
508 | d4c6efa9 | galberding | utCount.whiteCount = 1;
|
509 | 8dced1c9 | galberding | }else {
|
510 | if((utCount.whiteCount == 1) && (blackSensor <= global.linePID.BThresh)){ |
||
511 | d4c6efa9 | galberding | utCount.whiteCount = 0;
|
512 | 3c3c3bb9 | galberding | newState = ut_states::UT_FOLLOW_LINE; |
513 | d4c6efa9 | galberding | setRpmSpeed(stop); |
514 | fbcb25cc | galberding | } |
515 | d4c6efa9 | galberding | } |
516 | 10bf9cc0 | galberding | break;
|
517 | d4c6efa9 | galberding | } |
518 | fbcb25cc | galberding | // ---------------------------------------
|
519 | 3c3c3bb9 | galberding | case ut_states::UT_DETECT_STATION:
|
520 | 8dced1c9 | galberding | |
521 | 61544eee | galberding | if (global.forwardSpeed != DETECTION_SPEED){
|
522 | global.forwardSpeed = DETECTION_SPEED; |
||
523 | } |
||
524 | 181f2892 | galberding | if(lf.getStrategy() != LineFollowStrategy::EDGE_RIGHT){
|
525 | lf.setStrategy(LineFollowStrategy::EDGE_RIGHT); |
||
526 | } |
||
527 | 8dced1c9 | galberding | |
528 | 181f2892 | galberding | lf.followLine(rpmSpeed); |
529 | setRpmSpeed(rpmSpeed); |
||
530 | // // Detect marker before docking station
|
||
531 | 019224ff | galberding | // if ((WL+WR) < PROXY_WHEEL_THRESH){
|
532 | 8dced1c9 | galberding | // Use proxy ring
|
533 | 019224ff | galberding | if ((rProx[3]+rProx[4]) > RING_PROX_FRONT_THRESH){ |
534 | |||
535 | d607fcef | galberding | setRpmSpeed(stop); |
536 | 181f2892 | galberding | checkForMotion(); |
537 | 8dced1c9 | galberding | // 180° Rotation
|
538 | 181f2892 | galberding | global.distcontrol.setTargetPosition(0, ROTATION_180, ROTATION_DURATION);
|
539 | // BaseThread::sleep(8000);
|
||
540 | checkForMotion(); |
||
541 | 3c3c3bb9 | galberding | newState = ut_states::UT_CORRECT_POSITIONING; |
542 | 9c46b728 | galberding | } |
543 | 10bf9cc0 | galberding | break;
|
544 | 181f2892 | galberding | // ---------------------------------------
|
545 | 3c3c3bb9 | galberding | case ut_states::UT_CORRECT_POSITIONING:
|
546 | 019224ff | galberding | if (global.forwardSpeed != CHARGING_SPEED){
|
547 | global.forwardSpeed = CHARGING_SPEED; |
||
548 | } |
||
549 | 181f2892 | galberding | if(lf.getStrategy() != LineFollowStrategy::EDGE_LEFT){
|
550 | lf.promptStrategyChange(LineFollowStrategy::EDGE_LEFT); |
||
551 | 9c46b728 | galberding | } |
552 | 181f2892 | galberding | lf.followLine(rpmSpeed); |
553 | setRpmSpeed(rpmSpeed); |
||
554 | 9c46b728 | galberding | |
555 | 10bf9cc0 | galberding | utCount.stateTime++; |
556 | if (utCount.stateTime >= DOCKING_CORRECTION_TIMEOUT){
|
||
557 | utCount.stateTime = 0;
|
||
558 | 3c3c3bb9 | galberding | newState = ut_states::UT_REVERSE; |
559 | 181f2892 | galberding | setRpmSpeed(stop); |
560 | checkForMotion(); |
||
561 | } |
||
562 | 10bf9cc0 | galberding | break;
|
563 | 181f2892 | galberding | // ---------------------------------------
|
564 | 3c3c3bb9 | galberding | case ut_states::UT_REVERSE:
|
565 | 181f2892 | galberding | if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
566 | lf.setStrategy(LineFollowStrategy::REVERSE); |
||
567 | } |
||
568 | lf.followLine(rpmSpeed); |
||
569 | setRpmSpeed(rpmSpeed); |
||
570 | b24df8ad | galberding | // utCount.stateTime++;
|
571 | 58fe0e0b | Thomas Schöpping | |
572 | b24df8ad | galberding | // Docking is only successful if Deviation is in range and sensors are at their max values.
|
573 | d4c6efa9 | galberding | if((rProx[0] >= PROX_MAX_VAL) |
574 | && (rProx[7] >= PROX_MAX_VAL)
|
||
575 | && ((devCor.currentDeviation > -MAX_DEVIATION_FACTOR) && (devCor.currentDeviation < MAX_DEVIATION_FACTOR) )){ |
||
576 | 61544eee | galberding | // setRpmSpeed(stop);
|
577 | // checkForMotion();
|
||
578 | 10bf9cc0 | galberding | utCount.stateTime = 0;
|
579 | 3c3c3bb9 | galberding | newState = ut_states::UT_PUSH_BACK; |
580 | b24df8ad | galberding | }else if ((devCor.currentDeviation <= -MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){ |
581 | 8dced1c9 | galberding | // Case R
|
582 | b24df8ad | galberding | utCount.stateTime = 0;
|
583 | setRpmSpeed(stop); |
||
584 | devCor.RCase = true;
|
||
585 | lightAllLeds(Color::YELLOW); |
||
586 | 3c3c3bb9 | galberding | newState = ut_states::UT_DEVIATION_CORRECTION; |
587 | b24df8ad | galberding | }else if ((devCor.currentDeviation >= MAX_DEVIATION_FACTOR) && ((rProx[0] > DEVIATION_DIST_THRESH) || (rProx[7] > DEVIATION_DIST_THRESH))){ |
588 | 8dced1c9 | galberding | // Case L
|
589 | b24df8ad | galberding | utCount.stateTime = 0;
|
590 | setRpmSpeed(stop); |
||
591 | devCor.RCase = false;
|
||
592 | lightAllLeds(Color::WHITE); |
||
593 | 3c3c3bb9 | galberding | newState = ut_states::UT_DEVIATION_CORRECTION; |
594 | 10bf9cc0 | galberding | }else if (utCount.stateTime >= REVERSE_DOCKING_TIMEOUT){ |
595 | setRpmSpeed(stop); |
||
596 | utCount.stateTime = 0;
|
||
597 | utCount.errorCount++; |
||
598 | if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
||
599 | 3c3c3bb9 | galberding | newState = ut_states::UT_DOCKING_ERROR; |
600 | 10bf9cc0 | galberding | }else{
|
601 | 3c3c3bb9 | galberding | newState = ut_states::UT_CORRECT_POSITIONING; |
602 | 10bf9cc0 | galberding | } |
603 | 27d4e1fa | galberding | } |
604 | 10bf9cc0 | galberding | |
605 | 84b4c632 | galberding | // if((devCor.currentDeviation <= -10)){
|
606 | // rpmSpeed[0] -= 2000000;
|
||
607 | // }else if(devCor.currentDeviation >= 10){
|
||
608 | // rpmSpeed[1] -= 2000000;
|
||
609 | // }
|
||
610 | // setRpmSpeed(rpmSpeed);
|
||
611 | 10bf9cc0 | galberding | break;
|
612 | 27d4e1fa | galberding | // ---------------------------------------
|
613 | 3c3c3bb9 | galberding | case ut_states::UT_DEVIATION_CORRECTION:
|
614 | b24df8ad | galberding | // if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
615 | // lf.setStrategy(LineFollowStrategy::REVERSE);
|
||
616 | // }
|
||
617 | // lf.followLine(rpmSpeed);
|
||
618 | // setRpmSpeed(rpmSpeed);
|
||
619 | if(utCount.stateTime < DEVIATION_CORRECTION_DURATION / 2 ){ |
||
620 | if(devCor.RCase){
|
||
621 | rpmSpeed[0] = 0; |
||
622 | rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
|
||
623 | }else {
|
||
624 | rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
|
||
625 | rpmSpeed[1] = 0; |
||
626 | } |
||
627 | setRpmSpeed(rpmSpeed); |
||
628 | 84b4c632 | galberding | }else if (((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION +10)) ){ |
629 | b24df8ad | galberding | if(devCor.RCase){
|
630 | rpmSpeed[0] = DEVIATION_CORRECTION_SPEED;
|
||
631 | rpmSpeed[1] = 0; |
||
632 | }else {
|
||
633 | rpmSpeed[0] = 0; |
||
634 | rpmSpeed[1] = DEVIATION_CORRECTION_SPEED;
|
||
635 | } |
||
636 | setRpmSpeed(rpmSpeed); |
||
637 | 84b4c632 | galberding | if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){ |
638 | utCount.stateTime = 0;
|
||
639 | 3c3c3bb9 | galberding | newState = ut_states::UT_REVERSE; |
640 | 84b4c632 | galberding | setRpmSpeed(stop); |
641 | } |
||
642 | b24df8ad | galberding | }else{
|
643 | utCount.stateTime = 0;
|
||
644 | 3c3c3bb9 | galberding | newState = ut_states::UT_REVERSE; |
645 | b24df8ad | galberding | setRpmSpeed(stop); |
646 | } |
||
647 | |||
648 | utCount.stateTime++; |
||
649 | |||
650 | |||
651 | // if (utCount.stateTime > PUSH_BACK_TIMEOUT){
|
||
652 | // utCount.stateTime = 0;
|
||
653 | 3c3c3bb9 | galberding | // newState = ut_states::UT_CHECK_POSITIONING;
|
654 | b24df8ad | galberding | // }
|
655 | break;
|
||
656 | // ---------------------------------------
|
||
657 | 3c3c3bb9 | galberding | case ut_states::UT_PUSH_BACK:
|
658 | 27d4e1fa | galberding | if(lf.getStrategy() != LineFollowStrategy::REVERSE){
|
659 | lf.setStrategy(LineFollowStrategy::REVERSE); |
||
660 | } |
||
661 | lf.followLine(rpmSpeed); |
||
662 | setRpmSpeed(rpmSpeed); |
||
663 | |||
664 | 10bf9cc0 | galberding | utCount.stateTime++; |
665 | if (utCount.stateTime > PUSH_BACK_TIMEOUT){
|
||
666 | utCount.stateTime = 0;
|
||
667 | 3c3c3bb9 | galberding | newState = ut_states::UT_CHECK_POSITIONING; |
668 | 181f2892 | galberding | } |
669 | 10bf9cc0 | galberding | break;
|
670 | 181f2892 | galberding | // ---------------------------------------
|
671 | 3c3c3bb9 | galberding | case ut_states::UT_CHECK_POSITIONING:
|
672 | 61544eee | galberding | setRpmSpeed(stop); |
673 | checkForMotion(); |
||
674 | 27d4e1fa | galberding | if(checkDockingSuccess()){
|
675 | 3c3c3bb9 | galberding | newState = ut_states::UT_CHECK_VOLTAGE; |
676 | 27d4e1fa | galberding | }else{
|
677 | utCount.errorCount++; |
||
678 | 3c3c3bb9 | galberding | newState = ut_states::UT_CORRECT_POSITIONING; |
679 | 10bf9cc0 | galberding | if (utCount.errorCount >= DOCKING_ERROR_THRESH){
|
680 | 3c3c3bb9 | galberding | newState = ut_states::UT_DOCKING_ERROR; |
681 | 27d4e1fa | galberding | } |
682 | } |
||
683 | 10bf9cc0 | galberding | break;
|
684 | 181f2892 | galberding | // ---------------------------------------
|
685 | 3c3c3bb9 | galberding | case ut_states::UT_CHECK_VOLTAGE:
|
686 | ba75ee1d | galberding | if(!checkPinEnabled()){
|
687 | global.robot.requestCharging(1);
|
||
688 | } else {
|
||
689 | if(checkPinVoltage()){
|
||
690 | 8dced1c9 | galberding | // Pins are under voltage -> correctly docked
|
691 | |||
692 | 3c3c3bb9 | galberding | newState = ut_states::UT_CHARGING; |
693 | ba75ee1d | galberding | }else{
|
694 | 27d4e1fa | galberding | utCount.errorCount++; |
695 | ba75ee1d | galberding | // No voltage on pins -> falsely docked
|
696 | // deactivate pins
|
||
697 | 27d4e1fa | galberding | global.motorcontrol.setMotorEnable(true);
|
698 | ba75ee1d | galberding | global.robot.requestCharging(0);
|
699 | 27d4e1fa | galberding | // TODO: Soft release when docking falsely
|
700 | 61544eee | galberding | if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){ |
701 | 3c3c3bb9 | galberding | newState = ut_states::UT_RELEASE_TO_CORRECT; |
702 | 61544eee | galberding | } else {
|
703 | 3c3c3bb9 | galberding | newState = ut_states::UT_RELEASE_TO_CORRECT; //ut_states::UT_CORRECT_POSITIONING;
|
704 | 27d4e1fa | galberding | } |
705 | |||
706 | if (utCount.errorCount > DOCKING_ERROR_THRESH){
|
||
707 | 3c3c3bb9 | galberding | newState = ut_states::UT_DOCKING_ERROR; |
708 | 61544eee | galberding | } |
709 | ba75ee1d | galberding | } |
710 | } |
||
711 | 10bf9cc0 | galberding | break;
|
712 | 019224ff | galberding | // ---------------------------------------
|
713 | 3c3c3bb9 | galberding | case ut_states::UT_RELEASE_TO_CORRECT:
|
714 | 8dced1c9 | galberding | |
715 | 019224ff | galberding | global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
716 | checkForMotion(); |
||
717 | // move 1cm forward
|
||
718 | global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
||
719 | checkForMotion(); |
||
720 | // rotate back
|
||
721 | global.distcontrol.setTargetPosition(0, -2*ROTATION_20, ROTATION_DURATION); |
||
722 | checkForMotion(); |
||
723 | |||
724 | 10bf9cc0 | galberding | global.distcontrol.setTargetPosition(1500, 0, ROTATION_DURATION); |
725 | 019224ff | galberding | checkForMotion(); |
726 | 3c3c3bb9 | galberding | newState = ut_states::UT_CORRECT_POSITIONING; |
727 | 10bf9cc0 | galberding | break;
|
728 | 27d4e1fa | galberding | // ---------------------------------------
|
729 | 3c3c3bb9 | galberding | case ut_states::UT_CHARGING:
|
730 | 27d4e1fa | galberding | global.motorcontrol.setMotorEnable(false);
|
731 | 10bf9cc0 | galberding | utCount.errorCount = 0;
|
732 | 181f2892 | galberding | // Formulate Request to enable charging
|
733 | if(/* checkPinVoltage() && */ !checkPinEnabled()){ |
||
734 | global.robot.requestCharging(1);
|
||
735 | } |
||
736 | if(checkPinEnabled()){
|
||
737 | showChargingState(); |
||
738 | } |
||
739 | 10bf9cc0 | galberding | break;
|
740 | 181f2892 | galberding | // ---------------------------------------
|
741 | 3c3c3bb9 | galberding | case ut_states::UT_RELEASE:
|
742 | e5606def | galberding | |
743 | 61544eee | galberding | if (global.forwardSpeed != DETECTION_SPEED){
|
744 | global.rpmForward[0] = DETECTION_SPEED;
|
||
745 | } |
||
746 | 181f2892 | galberding | if(/* checkPinVoltage() && */ checkPinEnabled()){ |
747 | global.robot.requestCharging(0);
|
||
748 | }else{
|
||
749 | 27d4e1fa | galberding | global.motorcontrol.setMotorEnable(true);
|
750 | 8dced1c9 | galberding | // TODO: Use controlled
|
751 | 181f2892 | galberding | //Rotate -20° to free from magnet
|
752 | global.distcontrol.setTargetPosition(0, ROTATION_20, ROTATION_DURATION);
|
||
753 | 61544eee | galberding | checkForMotion(); |
754 | // move 1cm forward
|
||
755 | global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION); |
||
756 | checkForMotion(); |
||
757 | // rotate back
|
||
758 | 10bf9cc0 | galberding | // global.distcontrol.setTargetPosition(0, -ROTATION_20, ROTATION_DURATION);
|
759 | // checkForMotion();
|
||
760 | 019224ff | galberding | |
761 | 61544eee | galberding | // global.distcontrol.setTargetPosition(5000, 0, ROTATION_DURATION);
|
762 | // checkForMotion();
|
||
763 | 181f2892 | galberding | lStrategy = LineFollowStrategy::EDGE_RIGHT; |
764 | 3c3c3bb9 | galberding | newState = ut_states::UT_FOLLOW_LINE; |
765 | 61544eee | galberding | // whiteBuf = -100;
|
766 | // lf.followLine(rpmSpeed);
|
||
767 | // setRpmSpeed(rpmSpeed);
|
||
768 | 181f2892 | galberding | } |
769 | 61544eee | galberding | // lightAllLeds(Color::BLACK);
|
770 | 10bf9cc0 | galberding | break;
|
771 | // ---------------------------------------
|
||
772 | 3c3c3bb9 | galberding | case ut_states::UT_DOCKING_ERROR:
|
773 | newState = ut_states::UT_RELEASE; |
||
774 | 10bf9cc0 | galberding | break;
|
775 | // ---------------------------------------
|
||
776 | 3c3c3bb9 | galberding | case ut_states::UT_REVERSE_TIMEOUT_ERROR:
|
777 | newState = ut_states::UT_IDLE; |
||
778 | 10bf9cc0 | galberding | break;
|
779 | // ---------------------------------------
|
||
780 | 3c3c3bb9 | galberding | case ut_states::UT_CALIBRATION_ERROR:
|
781 | newState = ut_states::UT_IDLE; |
||
782 | 10bf9cc0 | galberding | break;
|
783 | // ---------------------------------------
|
||
784 | 3c3c3bb9 | galberding | case ut_states::UT_WHITE_DETECTION_ERROR:
|
785 | newState = ut_states::UT_IDLE; |
||
786 | 10bf9cc0 | galberding | break;
|
787 | // ---------------------------------------
|
||
788 | 3c3c3bb9 | galberding | case ut_states::UT_PROXY_DETECTION_ERROR:
|
789 | newState = ut_states::UT_IDLE; |
||
790 | 10bf9cc0 | galberding | break;
|
791 | // ---------------------------------------
|
||
792 | 3c3c3bb9 | galberding | case ut_states::UT_NO_CHARGING_POWER_ERROR:
|
793 | newState = ut_states::UT_IDLE; |
||
794 | 10bf9cc0 | galberding | break;
|
795 | // ---------------------------------------
|
||
796 | 3c3c3bb9 | galberding | case ut_states::UT_UNKNOWN_STATE_ERROR:
|
797 | newState = ut_states::UT_IDLE; |
||
798 | 10bf9cc0 | galberding | break;
|
799 | // ---------------------------------------
|
||
800 | 3c3c3bb9 | galberding | case ut_states::UT_TEST_MAP_AUTO_STATE:
|
801 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
802 | "Start autotracking: \n");
|
||
803 | |||
804 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
805 | "NodeCount: %d\n", map.getNodeCount());
|
||
806 | for (int i=0; i<map.getNodeCount(); i++) { |
||
807 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
808 | "ID: %d, l: %d, r: %d \n", map.getNodeList()[i].id,
|
||
809 | map.getNodeList()[i].left, map.getNodeList()[i].right); |
||
810 | } |
||
811 | newState = UT_IDLE; |
||
812 | break;
|
||
813 | case ut_states::UT_TEST_MAP_STATE:{
|
||
814 | e5606def | galberding | // Test suit for amiro map
|
815 | e404e6c0 | galberding | |
816 | |||
817 | bdac5bec | galberding | |
818 | e404e6c0 | galberding | // AmiroMap map = AmiroMap(&global);
|
819 | |||
820 | e5606def | galberding | // --------------------------------------------------
|
821 | e404e6c0 | galberding | |
822 | global.tcase = 0;
|
||
823 | bdac5bec | galberding | // Set basic valid map configuration
|
824 | setAttributes(global.testmap, 0, 1, 2, 1); |
||
825 | setAttributes(global.testmap, 1, 2, 2, 0); |
||
826 | setAttributes(global.testmap, 2, 1, 0, 0); |
||
827 | setAttributes(global.testmap, 3, 0, 0, 0xff); |
||
828 | chprintf((BaseSequentialStream *)&global.sercanmux1, "Init Case: %d, res: %d\n",global.tcase, map.initialize());
|
||
829 | e404e6c0 | galberding | global.testres[global.tcase] = map.get_state()->valid; |
830 | |||
831 | bdac5bec | galberding | global.tcase++; // 1
|
832 | // Test map fail if first node is flagged with end
|
||
833 | e404e6c0 | galberding | setAttributes(global.testmap, 0, 1, 2, 0xff); |
834 | map.initialize(); |
||
835 | global.testres[global.tcase] = !map.get_state()->valid; |
||
836 | |||
837 | global.tcase++; // 2
|
||
838 | bdac5bec | galberding | // Test if node 2 is set as start node
|
839 | e404e6c0 | galberding | setAttributes(global.testmap, 0, 1, 2, 0); |
840 | setAttributes(global.testmap, 2, 1, 0, 1); |
||
841 | map.initialize(); |
||
842 | global.testres[global.tcase] = map.get_state()->current == 2;
|
||
843 | |||
844 | global.tcase++; // 3
|
||
845 | bdac5bec | galberding | // Test if non reachable nodes will trigger invalid map
|
846 | e404e6c0 | galberding | setAttributes(global.testmap, 3, 0, 0, 0); |
847 | setAttributes(global.testmap, 4, 0, 0, 0xff); |
||
848 | map.initialize(); |
||
849 | global.testres[global.tcase] = !map.get_state()->valid; |
||
850 | |||
851 | bdac5bec | galberding | global.tcase++; // 4
|
852 | // Test Reinitialization
|
||
853 | setAttributes(global.testmap, 0, 1, 2, 1); |
||
854 | setAttributes(global.testmap, 1, 2, 2, 0); |
||
855 | setAttributes(global.testmap, 2, 1, 0, 0); |
||
856 | setAttributes(global.testmap, 3, 0, 0, 0xff); |
||
857 | map.initialize(); |
||
858 | global.testres[global.tcase] = map.get_state()->valid; |
||
859 | |||
860 | global.odometry.resetPosition(); |
||
861 | uint8_t ret = 0;
|
||
862 | global.tcase++; // 5
|
||
863 | // Test update under normal linefollowing without fixpoint
|
||
864 | ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT); |
||
865 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
866 | "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
|
||
867 | map.get_state()->current, map.get_state()->next); |
||
868 | // No case should be true because neither was a node visited nor
|
||
869 | // was a fixpoint detected.
|
||
870 | global.testres[global.tcase] = (ret == 0x4);
|
||
871 | |||
872 | |||
873 | global.odometry.setPosition(1.0, 0.0, 0.0); |
||
874 | d02c536e | galberding | chprintf((BaseSequentialStream *)&global.sercanmux1, "Current Point: %d\n", global.odometry.getPosition().x);
|
875 | bdac5bec | galberding | global.tcase++; // 6
|
876 | // Fixpoint on left side
|
||
877 | ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT); |
||
878 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
879 | "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
|
||
880 | map.get_state()->current, map.get_state()->next); |
||
881 | // No case should be true because neither was a node visited nor
|
||
882 | // was a fixpoint detected.
|
||
883 | // global.odometry
|
||
884 | global.testres[global.tcase] = (ret == 0x1)
|
||
885 | && (map.get_state()->strategy == 0x01)
|
||
886 | && (map.get_state()->dist == 0)
|
||
887 | && (map.get_state()->current == 2);
|
||
888 | |||
889 | |||
890 | global.odometry.setPosition(1.5, 0.0, 0.0); |
||
891 | d02c536e | galberding | chprintf((BaseSequentialStream *)&global.sercanmux1, |
892 | "Current Point: %d\n", global.odometry.getPosition().x);
|
||
893 | bdac5bec | galberding | global.tcase++; // 7
|
894 | // Fixpoint on left side, no update should appear because fixpoint already
|
||
895 | // marked
|
||
896 | ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT); |
||
897 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
898 | "Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret,
|
||
899 | map.get_state()->current, map.get_state()->next); |
||
900 | // No case should be true because neither was a node visited nor
|
||
901 | // was a fixpoint detected.
|
||
902 | global.testres[global.tcase] = (ret == 0x00)
|
||
903 | && (map.get_state()->strategy == 0x01);
|
||
904 | // && (map.get_state()->dist == 0);
|
||
905 | |||
906 | d02c536e | galberding | global.odometry.setPosition(1.2, 0.0, 0.0); |
907 | global.tcase++; // 8
|
||
908 | // Fixpoint on left side, no update should appear because fixpoint already
|
||
909 | // marked
|
||
910 | ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT); |
||
911 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
912 | "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n", global.tcase, ret,
|
||
913 | map.get_state()->current, map.get_state()->next, map.get_state()->dist, map.get_state()->eLength); |
||
914 | // No case should be true because neither was a node visited nor
|
||
915 | // was a fixpoint detected.
|
||
916 | global.testres[global.tcase] = |
||
917 | (ret == 0x04) && (map.get_state()->strategy == 0x01) |
||
918 | && (map.get_state()->dist == 0);
|
||
919 | bdac5bec | galberding | |
920 | d02c536e | galberding | global.odometry.setPosition(.5, 0.0, 0.0); |
921 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
922 | "Current Point: %d\n", global.odometry.getPosition().x);
|
||
923 | global.tcase++; // 9
|
||
924 | // Fixpoint on left side, no update should appear because fixpoint already
|
||
925 | // marked
|
||
926 | ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT); |
||
927 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
928 | "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",
|
||
929 | global.tcase, ret, map.get_state()->current, |
||
930 | map.get_state()->next, map.get_state()->dist, |
||
931 | map.get_state()->eLength); |
||
932 | // No case should be true because neither was a node visited nor
|
||
933 | // was a fixpoint detected.
|
||
934 | global.testres[global.tcase] = |
||
935 | (ret == 9) &&
|
||
936 | (map.get_state()->strategy == 1) &&
|
||
937 | (map.get_state()->dist == 0) &&
|
||
938 | (map.get_state()->eLength == 50);
|
||
939 | bdac5bec | galberding | |
940 | d02c536e | galberding | global.odometry.setPosition(.75, 0.0, 0.0); |
941 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
942 | "Current Point: %d\n", global.odometry.getPosition().x);
|
||
943 | global.tcase++; // 10
|
||
944 | // Fixpoint on left side, no update should appear because fixpoint already
|
||
945 | // marked
|
||
946 | ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT); |
||
947 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
948 | "Update test %d: Ret %d, cur %d, nex %d, dist %d, len %d\n",
|
||
949 | global.tcase, ret, map.get_state()->current, |
||
950 | map.get_state()->next, map.get_state()->dist, |
||
951 | map.get_state()->eLength); |
||
952 | // No case should be true because neither was a node visited nor
|
||
953 | // was a fixpoint detected.
|
||
954 | global.testres[global.tcase] = |
||
955 | (ret == 12) && (map.get_state()->strategy == 1) && |
||
956 | (map.get_state()->dist == 50) && (map.get_state()->eLength == 50); |
||
957 | bdac5bec | galberding | |
958 | e404e6c0 | galberding | int failed = 0; |
959 | int passed = 0; |
||
960 | for (int i = 0; i <= global.tcase; i++) { |
||
961 | if (global.testres[i]) {
|
||
962 | passed++; |
||
963 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
964 | "Test %d Passed!\n", i);
|
||
965 | } else {
|
||
966 | failed++; |
||
967 | chprintf((BaseSequentialStream *)&global.sercanmux1, |
||
968 | "Test %d Failed\n", i);
|
||
969 | } |
||
970 | 27d4e1fa | galberding | } |
971 | e404e6c0 | galberding | chprintf((BaseSequentialStream *)&global.sercanmux1, |
972 | "Total: %d, Passed: %d, Failed: %d\n", global.tcase + 1, passed, |
||
973 | failed); |
||
974 | |||
975 | 3c3c3bb9 | galberding | newState = ut_states::UT_IDLE; |
976 | e404e6c0 | galberding | break;
|
977 | } |
||
978 | // --------------------------------------------------
|
||
979 | default:
|
||
980 | 3c3c3bb9 | galberding | newState = ut_states::UT_UNKNOWN_STATE_ERROR; |
981 | e404e6c0 | galberding | break;
|
982 | } |
||
983 | |||
984 | e5606def | galberding | // In case a new state is set:
|
985 | // 1. Record the state transition
|
||
986 | 3c3c3bb9 | galberding | if (AMIRO_MAP_AUTO_TRACKING) {
|
987 | // map.trackUpdate(WL, WR, lStrategy, currentState);
|
||
988 | } |
||
989 | |||
990 | e404e6c0 | galberding | if (currentState != newState){
|
991 | e5606def | galberding | |
992 | global.stateTransitionCounter++; |
||
993 | // Clear all state transitions to prevent overflow
|
||
994 | if (global.stateTransitionCounter >= 255) { |
||
995 | global.stateTransitionCounter = 0;
|
||
996 | for (int i = 0; i < 24; i++) { |
||
997 | global.stateTracker[i] = 0;
|
||
998 | } |
||
999 | } |
||
1000 | // Transmit the new state over can
|
||
1001 | e404e6c0 | galberding | chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
|
1002 | global.robot.transmitState(newState); |
||
1003 | e5606def | galberding | |
1004 | // Increase state count for specific state
|
||
1005 | // TODO: Improve with dictionary or other than switch case
|
||
1006 | 3c3c3bb9 | galberding | if (newState == ut_states::UT_IDLE)
|
1007 | {global.stateTracker[ut_states::UT_IDLE] += 1;}
|
||
1008 | else if (newState == ut_states::UT_FOLLOW_LINE) |
||
1009 | {global.stateTracker[ut_states::UT_FOLLOW_LINE] += 1;}
|
||
1010 | else if (newState == ut_states::UT_DETECT_STATION) |
||
1011 | {global.stateTracker[ut_states::UT_DETECT_STATION] += 1;}
|
||
1012 | else if (newState == ut_states::UT_REVERSE) |
||
1013 | {global.stateTracker[ut_states::UT_REVERSE] += 1;}
|
||
1014 | else if (newState == ut_states::UT_PUSH_BACK) |
||
1015 | {global.stateTracker[ut_states::UT_PUSH_BACK] += 1;}
|
||
1016 | else if (newState == ut_states::UT_CHECK_POSITIONING) |
||
1017 | {global.stateTracker[ut_states::UT_CHECK_POSITIONING] += 1;}
|
||
1018 | else if (newState == ut_states::UT_CHECK_VOLTAGE) |
||
1019 | {global.stateTracker[ut_states::UT_CHECK_VOLTAGE] += 1;}
|
||
1020 | else if (newState == ut_states::UT_CHARGING) |
||
1021 | {global.stateTracker[ut_states::UT_CHARGING] += 1;}
|
||
1022 | else if (newState == ut_states::UT_RELEASE) |
||
1023 | {global.stateTracker[ut_states::UT_RELEASE] += 1;}
|
||
1024 | else if (newState == ut_states::UT_RELEASE_TO_CORRECT) |
||
1025 | {global.stateTracker[ut_states::UT_RELEASE_TO_CORRECT] += 1;}
|
||
1026 | else if (newState == ut_states::UT_CORRECT_POSITIONING) |
||
1027 | {global.stateTracker[ut_states::UT_CORRECT_POSITIONING] += 1;}
|
||
1028 | else if (newState == ut_states::UT_TURN) |
||
1029 | {global.stateTracker[ut_states::UT_TURN] += 1;}
|
||
1030 | else if (newState == ut_states::UT_INACTIVE) |
||
1031 | {global.stateTracker[ut_states::UT_INACTIVE] += 1;}
|
||
1032 | else if (newState == ut_states::UT_CALIBRATION) |
||
1033 | {global.stateTracker[ut_states::UT_CALIBRATION] += 1;}
|
||
1034 | else if (newState == ut_states::UT_CALIBRATION_CHECK) |
||
1035 | {global.stateTracker[ut_states::UT_CALIBRATION_CHECK] += 1;}
|
||
1036 | else if (newState == ut_states::UT_DEVIATION_CORRECTION) |
||
1037 | {global.stateTracker[ut_states::UT_DEVIATION_CORRECTION] += 1;}
|
||
1038 | else if (newState == ut_states::UT_DOCKING_ERROR) |
||
1039 | {global.stateTracker[16+(-ut_states::UT_DOCKING_ERROR)] += 1;} |
||
1040 | else if (newState == ut_states::UT_REVERSE_TIMEOUT_ERROR) |
||
1041 | {global.stateTracker[16+(-ut_states::UT_REVERSE_TIMEOUT_ERROR)] += 1;} |
||
1042 | else if (newState == ut_states::UT_CALIBRATION_ERROR) |
||
1043 | {global.stateTracker[16+(-ut_states::UT_CALIBRATION_ERROR)] += 1;} |
||
1044 | else if (newState == ut_states::UT_WHITE_DETECTION_ERROR) |
||
1045 | {global.stateTracker[16+(-ut_states::UT_WHITE_DETECTION_ERROR)] += 1;} |
||
1046 | else if (newState == ut_states::UT_PROXY_DETECTION_ERROR) |
||
1047 | {global.stateTracker[16+(-ut_states::UT_PROXY_DETECTION_ERROR)] += 1;} |
||
1048 | else if (newState == ut_states::UT_NO_CHARGING_POWER_ERROR) |
||
1049 | {global.stateTracker[16+(-ut_states::UT_NO_CHARGING_POWER_ERROR)] += 1;} |
||
1050 | else if (newState == ut_states::UT_UNKNOWN_STATE_ERROR) |
||
1051 | {global.stateTracker[16+(-ut_states::UT_UNKNOWN_STATE_ERROR)] += 1;} |
||
1052 | e404e6c0 | galberding | } |
1053 | e5606def | galberding | |
1054 | // Keep track of last state and set the new state for next iteration
|
||
1055 | e404e6c0 | galberding | prevState = currentState; |
1056 | currentState = newState; |
||
1057 | |||
1058 | 5d138bca | galberding | this->sleep(CAN::UPDATE_PERIOD);
|
1059 | } |
||
1060 | 58fe0e0b | Thomas Schöpping | |
1061 | return RDY_OK;
|
||
1062 | } |