Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse code

add function that make roomba turn specified degree

  • Loading branch information...
commit be98da816c53d7e423176daa40435bb354dacb1b 1 parent 9ec1219
Jun Guo guo0693 authored

Showing 1 changed file with 210 additions and 130 deletions. Show diff stats Hide diff stats

  1. +210 130 prod_with_return/main.c
340 prod_with_return/main.c
@@ -18,51 +18,111 @@
18 18
19 19 #define REGION_RES 40
20 20
21   -#define THRESH 175
22   -#define R_THRESH .25
23   -#define R_COUNT_THRESH 32
  21 +#define D_THRESH 200
  22 +#define R_THRESH .26
  23 +#define R_COUNT_THRESH 100
24 24
25   -// Globals
  25 +#define MODE_SEEK 0
  26 +#define MODE_UTURN 1
  27 +#define MODE_RETURN 2
  28 +#define MODE_FINISH 3
26 29
27   -//char command;
  30 +#define UTURN_THRESH 0.1
28 31
29   -double posX;
30   -double posY;
31   -double posT;
  32 +// Globals
32 33
33   -pthread_mutex_t _lock;
34   -//pthread_mutex_t* lock = &_lock;
  34 +double posX = 0.0;
  35 +double posY = 0.0;
  36 +double posT = 0.0;
35 37
36   -void waitRoombaPlayBtnPush(Roomba* _roomba)
  38 +#define TOLERATE 5 //in degree
  39 +void roomba_spinleft_angle(Roomba* _roomba, int16_t degree) //in degree
37 40 {
38   - printf("Wait for Play Button to be pushed!\n");
  41 + int16_t diff = 0;
  42 + uint8_t* sb;
  43 + int16_t tmp_angle;
39 44 while(1)
40 45 {
41   - roomba_read_sensors(_roomba );
42   - roomba_delay(COMMANDPAUSE_MILLIS);
43   - if((_roomba->sensor_bytes[11]&0x01) == 1)
  46 + if(diff >= degree-TOLERATE && diff <= degree+TOLERATE)
  47 + {
  48 + printf("break!\n");
  49 + roomba_stop(_roomba );
  50 + break;
  51 + }
  52 + else if(diff > degree+TOLERATE)
  53 + {
  54 + roomba_spinright_at(_roomba, 50);
  55 + roomba_delay(COMMANDPAUSE_MILLIS);
  56 + }
  57 + else
44 58 {
45   - printf("Play Button Pushed!\n");
46   - return;
  59 + roomba_spinleft_at(_roomba, 100);
  60 + roomba_delay(COMMANDPAUSE_MILLIS);
47 61 }
  62 + roomba_read_sensors(_roomba);
  63 + sb = ((Roomba*)_roomba)->sensor_bytes;
  64 + tmp_angle = (sb[14]<<8) | sb[15]; // in degrees
  65 + diff += tmp_angle;
  66 + roomba_delay(COMMANDPAUSE_MILLIS);
  67 + printf("turned angle: %d\n", diff);
48 68 }
49 69 }
50 70
51   -void waitRoombaAdvanceBtnPush(Roomba* _roomba)
  71 +void roomba_spinright_angle(Roomba* _roomba, int16_t degree) //in degree
52 72 {
53   - printf("Wait for Advance Button to be pushed!\n");
  73 + int16_t diff = 0;
  74 + uint8_t* sb;
  75 + int16_t tmp_angle;
54 76 while(1)
55 77 {
56   - roomba_read_sensors(_roomba );
57   - roomba_delay(COMMANDPAUSE_MILLIS);
58   - if(_roomba->sensor_bytes[11] == 4)
  78 + if(diff >= degree-TOLERATE && diff <= degree+TOLERATE)
  79 + {
  80 + printf("break!\n");
  81 + roomba_stop(_roomba );
  82 + break;
  83 + }
  84 + else if(diff > degree+TOLERATE)
  85 + {
  86 + roomba_spinleft_at(_roomba, 50);
  87 + roomba_delay(COMMANDPAUSE_MILLIS);
  88 + }
  89 + else
59 90 {
60   - printf("Advance Button Pushed!\n");
61   - return;
  91 + roomba_spinright_at(_roomba, 100);
  92 + roomba_delay(COMMANDPAUSE_MILLIS);
62 93 }
  94 + roomba_read_sensors(_roomba);
  95 + sb = ((Roomba*)_roomba)->sensor_bytes;
  96 + tmp_angle = (sb[14]<<8) | sb[15]; // in degrees
  97 + diff -= tmp_angle;
  98 + roomba_delay(COMMANDPAUSE_MILLIS);
  99 + printf("turned angle: %d\n", diff);
63 100 }
64 101 }
65   -//Roomba* roomba;
  102 +
  103 +double normalize_angle(double in)
  104 +{
  105 + double out = in;
  106 +
  107 + while(out > PI) {
  108 + out -= 2*PI;
  109 + printf("normalized posT down\n");
  110 + }
  111 +
  112 + while(out < -PI) {
  113 + out += 2*PI;
  114 + printf("normalized posT up\n");
  115 + }
  116 +
  117 + return out;
  118 +}
  119 +
  120 +double angle_diff(double target, double test)
  121 +{
  122 +
  123 + return normalize_angle(test - target);
  124 +
  125 +}
66 126
67 127 uint32_t get_region(uint32_t offset)
68 128 {
@@ -134,61 +194,22 @@ int getRedCount() {
134 194
135 195 int red_regions = 0;
136 196 int i,j;
137   - //int base_row = 7;
138   - //int base_col = 5;
139 197 for (i = 0; i < (H / REGION_RES); i++) {
140 198
141   - //mvaddstr(base_row + 3 * i, base_col, "|");
142   -
143 199 for (j = 0; j < (W / REGION_RES); j++) {
144 200
145 201 double region_avg =
146 202 regions[(i * (W / REGION_RES)) + j] / pow(REGION_RES, 2);
147 203
148   - //char buf[16];
149   -
150 204 if (region_avg > R_THRESH)
151 205 {
152   - //obstacle = 1;
153 206 red_regions ++;
154   - //sprintf(buf, "(%.2f) ", region_avg);
155   - }
156   - else
157   - {
158   - //sprintf(buf, "%.2f ", region_avg);
159 207 }
160 208
161   - /*
162   - mvaddstr( base_row + 3 * i,
163   - base_col + 6 * (j+1),
164   - buf );
165   - */
166   -
167   -
168 209 }
169 210
170   - /*
171   - mvaddstr( base_row + 3 * i,
172   - base_col + 6 * ((W/REGION_RES)+1),
173   - "|");
174   - */
175 211 }
176 212
177   - /*
178   - char buf[8];
179   -
180   - sprintf(buf, "%d ", red_regions);
181   - mvaddstr(5,3,buf);
182   -
183   - if(obstacle) {
184   - mvaddstr(5,3,"WARNING!\n");
185   - } else {
186   - mvaddstr(5,3,"Safe....\n");
187   - }
188   - */
189   -
190   - //refresh();
191   -
192 213 free(regions);
193 214
194 215 return red_regions;
@@ -273,15 +294,6 @@ uint8_t* findObstacles() {
273 294 void exc_one(Roomba* _roomba, char command)
274 295 {
275 296
276   - int err = 0;
277   -
278   - if (err != 0)
279   - {
280   - printf("err: %d\n",err);
281   - perror("exc sem wait");
282   - exit(1);
283   - }
284   -
285 297 printf("exc command: %c\n", command);
286 298
287 299 switch(command)
@@ -296,12 +308,14 @@ void exc_one(Roomba* _roomba, char command)
296 308 break;
297 309
298 310 case'a':
299   - roomba_spinleft_at(_roomba,120);
  311 + //roomba_spinleft_at(_roomba,200);
  312 + roomba_spinleft_angle(_roomba, 90);
300 313 break;
301 314
302 315 case'd':
303   - roomba_spinright_at(_roomba,120);
304   - break;
  316 + //roomba_spinright_at(_roomba,200);
  317 + roomba_spinright_angle(_roomba,90);
  318 + break;
305 319
306 320 case'p':
307 321 roomba_stop(_roomba);
@@ -309,7 +323,7 @@ void exc_one(Roomba* _roomba, char command)
309 323
310 324 case'e':
311 325 roomba_stop(_roomba);
312   - //pthread_exit(NULL);
  326 + pthread_exit(NULL);
313 327 break;
314 328
315 329 default:
@@ -323,20 +337,21 @@ void exc_one(Roomba* _roomba, char command)
323 337 int16_t dist_i = (sb[12]<<8) | sb[13];
324 338 int16_t angle_i = (sb[14]<<8) | sb[15]; // in degrees
325 339
  340 + printf("delta dist:%d angle:%d\n", dist_i, angle_i);
  341 +
326 342 double dist = dist_i;
327 343 double angle = angle_i / 360.0 * 2.0 * PI;
328 344
329 345 posT += angle;
330 346
  347 + posT = normalize_angle(posT);
  348 +
331 349 posX += dist * cos(posT);
332 350 posY += dist * sin(posT);
333 351
334 352 break;
335 353 }
336 354
337   - printf("exc unlock\n");
338   -
339   -
340 355 }
341 356
342 357 int main(int argc, char* argv[])
@@ -346,49 +361,38 @@ int main(int argc, char* argv[])
346 361 init();
347 362
348 363 Roomba* roomba = roomba_init( argv[1] );
349   - roomba_set_velocity(roomba, 400);
350   - /*
351   - pthread_mutex_init(&_lock, NULL);
352   - pthread_t texc_cmd;
353   - pthread_create(&texc_cmd, NULL, exc_cmd, (void*)roomba);
354   - */
355   -
356   - //printf("Threads going\n");
357   - waitRoombaPlayBtnPush(roomba);
358   - printf("Start loop\n");
359 364
360   - //search for the object
  365 + exc_one(roomba, 'q');
  366 +
  367 + posX = 0.0;
  368 + posY = 0.0;
  369 + posT = 0.0;
  370 +
  371 + uint8_t mode = MODE_SEEK;
  372 +
  373 + printf("Start loop");
  374 +
361 375 while(1)
362 376 {
363 377
364   - // printf("Getting obstacles\n");
  378 + /*
  379 + * Get obstacles and print the status of each column
  380 + */
365 381
366 382 uint8_t* obs = findObstacles();
367   -
368 383 uint8_t* cols = calloc ( (W / REGION_RES),
369 384 sizeof(uint8_t));
370 385
371   - //printf("Walking obstacles\n");
372   -
373 386 int i,j;
374 387 for (i = 0; i < (H / REGION_RES); i++){
375 388 for (j = 0; j < (W / REGION_RES); j++){
376   -
377   - //printf("checking obstacle %d %d\n", i, j);
378   -
379 389 uint8_t region_avg = obs[i * (W / REGION_RES) + j];
380   -
381   - if (region_avg > THRESH) {
  390 + if (region_avg > D_THRESH) {
382 391 cols[j] = 1;
383 392 }
384   -
385   - // printf("%d ", region_avg);
386 393 }
387   - // printf("\n");
388 394 }
389 395
390   - //printf("Finished with raw obstacles");
391   -
392 396 free(obs);
393 397
394 398 printf("Cols: ");
@@ -409,8 +413,6 @@ int main(int argc, char* argv[])
409 413
410 414 double centerofmass = (double)weight/(double)area;
411 415
412   - int red_count = getRedCount();
413   -
414 416 char tempcmd = 'q';
415 417
416 418 if (anyobs) {
@@ -426,36 +428,114 @@ int main(int argc, char* argv[])
426 428 tempcmd = 'w';
427 429 }
428 430
  431 + /*
  432 + * Get the red count to decide if we can leave the SEEK mode
  433 + */
  434 +
  435 + int red_count = getRedCount();
  436 +
  437 + printf("red count: %d\n", red_count);
  438 +
429 439 if (red_count > R_COUNT_THRESH)
430 440 {
431   - printf("RED THING!!!!!111\n");
  441 + printf("RED OBJECT DETECTED\n");
432 442 tempcmd = 'p';
  443 + if (mode == MODE_SEEK)
  444 + {
  445 + mode = MODE_UTURN;
  446 + }
  447 + else if (mode == MODE_RETURN)
  448 + {
  449 + mode = MODE_FINISH;
  450 + }
  451 + else
  452 + {
  453 + printf("mode error");
  454 + }
433 455 }
434   -
435   -#ifndef FAKE
436   - exc_one(roomba, tempcmd);
  456 +
  457 + /*
  458 + * Query the distanced traveled, which updates our position
  459 + */
  460 +
437 461 exc_one(roomba, 'q');
438   -#endif
439   - }
440   - roomba_play_note(roomba, 57, 32);
441   - roomba_delay(100);
442   - roomba_play_note(roomba, 59, 32);
443   - roomba_delay(100);
444   - roomba_play_note(roomba, 60, 32);
445   - roomba_delay(100);
446   - roomba_play_note(roomba, 64, 32);
447   - roomba_delay(100);
448   - waitRoombaAdvanceBtnPush(roomba);
449   - //turn around to return
450   - double return_angle = 180 - posT;
451   - while(posT != return_angle)
  462 + printf("x:%f, y:%f, t:%f\n", posX, posY, posT);
  463 +
  464 + switch(mode)
452 465 {
453   - exc_one(roomba, 'a');
454   - exc_one(roomba, 'q');
455   - }
456   - //ToDo:return and search
457 466
458   - //pthread_join(texc_cmd, NULL);
  467 + case MODE_SEEK:
  468 + //printf("fake send %c\n", tempcmd);
  469 + exc_one(roomba, tempcmd);
  470 + break;
  471 +
  472 + case MODE_RETURN:
  473 + exc_one(roomba, tempcmd);
  474 + break;
  475 +
  476 + case MODE_FINISH:
  477 + exc_one(roomba, 'p');
  478 +
  479 + int i;
  480 + for (i = 0; i < 5; i++)
  481 + {
  482 + roomba_play_note(roomba, 10, 10);
  483 + roomba_delay(10);
  484 + }
  485 +
  486 + printf("returned!\n");
  487 +
  488 + exit(0);
  489 + break;
  490 +
  491 + case MODE_UTURN:
  492 + printf("ToDo: turning around\n");
  493 +
  494 + double return_bearing = atan2(-posY, -posX);
  495 +
  496 + double diff = angle_diff(return_bearing, posT);
  497 + double abs_diff = (diff < 0) ? -diff : diff;
  498 +
  499 + printf("return_bearing:%f posT:%f diff:%f (%f)\n",
  500 + return_bearing, posT, diff, abs_diff);
  501 +
  502 + while (abs_diff > UTURN_THRESH)
  503 + {
  504 +
  505 + if (diff > 0)
  506 + {
  507 + //exc_one(roomba, 'd');
  508 + roomba_spinright_at(roomba, 50);
  509 + }
  510 + else
  511 + {
  512 + //exc_one(roomba, 'a');
  513 + roomba_spinleft_at(roomba, 50);
  514 + }
  515 +
  516 + exc_one(roomba, 'q');
  517 +
  518 + diff = angle_diff(return_bearing, posT);
  519 + abs_diff = (diff < 0) ? -diff : diff;
  520 +
  521 + printf("return_bearing:%f posT:%f diff:%f (%f)\n",
  522 + return_bearing, posT, diff, abs_diff);
  523 +
  524 + }
  525 +
  526 + printf("turned around!\n");
  527 +
  528 + mode = MODE_RETURN;
  529 + break;
  530 +
  531 + default:
  532 + printf("todo: mode %d", mode);
  533 + exit(1);
  534 + break;
  535 +
  536 + }
  537 +
  538 + }
459 539
460 540 return 0;
461 541

0 comments on commit be98da8

Please sign in to comment.
Something went wrong with that request. Please try again.