wiki:Example6

Examples

Example 6

Make all bots face in about the same direction without a leader bot.

example6/main.c:

#include <all.h>

/* actual number does not matter */
#define DIR_COMMAND 10

#define TIMEOUT 3000 // ms

int
main(void) 
{
        /* initialize devices */
        init_all();

        uint8_t send_times = 1;
        uint8_t turn_dir = LEFT;
        uint8_t turn_cnt = 0;
        uint16_t data;
        int8_t dir;

        for(;;) {
                /* On each side send the dir which should be facing here */
                for (uint8_t t = 0; t < send_times; t++) { // certainty factor of being oriented correctly
                        // wiggle around on spot to compensate for narrow IR beam (i.e. blind areas)
                        if (turn_cnt == 2) {
                                turn_dir = (turn_dir == LEFT) ? RIGHT : LEFT;
                                turn_cnt = 0;
                        }
                        pwmdrive_turn_deg10(turn_dir, 1);
                        turn_cnt++;

                        for (int i = 0; i < 4; i++) {
                                led_set(dirs[i]);
                                rc5_send_command(dirs[i], ANY_DEST, DIR_COMMAND, opposite_dir(dirs[i]));
                        }
                }

                led_set(ANYDIR);
                dir = rc5_recv_specific_command_timeout(ANYDIR, ANY_SOURCE, DIR_COMMAND, &data, NULL, TIMEOUT);
                if (dir == NODIR) { // timeout, nothing received => send again
                        continue;
                }

                // goal: dir == data (i.e. facing about the same direction)
                led_set(dir | data);
                if (dir == data) {
                        // already correct orientation with increasing certainty
                        send_times = send_times < 254 ? send_times + 1 : 254;
                } else {
                        send_times = 1;
                }

                if ((dir == LEFT && data == RIGHT)
                    || (dir == RIGHT && data == LEFT)
                    || (dir == FRONT && data == BACK)
                    || (dir == BACK && data == FRONT)) {
                        pwmdrive_turn_deg10(RIGHT, 9);
                } else if ((dir == LEFT && data == BACK)
                   || (dir == RIGHT && data == FRONT)
                   || (dir == FRONT && data == LEFT)
                   || (dir == BACK && data == RIGHT)) {
                        pwmdrive_turn_deg10(RIGHT, 3);
                } else if ((dir == LEFT && data == FRONT)
                   || (dir == RIGHT && data == BACK)
                   || (dir == FRONT && data == RIGHT)
                   || (dir == BACK && data == LEFT))    {
                        pwmdrive_turn_deg10(LEFT, 3);
                }
                led_set(OFF);
        }

        return 0;
}
Last modified 7 years ago Last modified on Nov 23, 2011, 11:23:30 AM