Gordon Sulc
bees.cpp@0:178a07cd3e39, 2011-12-16 (annotated)
- Committer:
- gsulc
- Date:
- Fri Dec 16 03:45:52 2011 +0000
- Revision:
- 0:178a07cd3e39
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| gsulc | 0:178a07cd3e39 | 1 | #include "bees.h" |
| gsulc | 0:178a07cd3e39 | 2 | |
| gsulc | 0:178a07cd3e39 | 3 | int find_pollen() { |
| gsulc | 0:178a07cd3e39 | 4 | return 1; |
| gsulc | 0:178a07cd3e39 | 5 | } |
| gsulc | 0:178a07cd3e39 | 6 | int return_home() { |
| gsulc | 0:178a07cd3e39 | 7 | return 1; |
| gsulc | 0:178a07cd3e39 | 8 | } |
| gsulc | 0:178a07cd3e39 | 9 | int send_directions() { |
| gsulc | 0:178a07cd3e39 | 10 | //while(!xbee.readable()) {} |
| gsulc | 0:178a07cd3e39 | 11 | //read header |
| gsulc | 0:178a07cd3e39 | 12 | |
| gsulc | 0:178a07cd3e39 | 13 | //read contents |
| gsulc | 0:178a07cd3e39 | 14 | int i = 0; |
| gsulc | 0:178a07cd3e39 | 15 | while(1){ |
| gsulc | 0:178a07cd3e39 | 16 | xbee.putc(directions[i] + 2); |
| gsulc | 0:178a07cd3e39 | 17 | if(DEBUG) |
| gsulc | 0:178a07cd3e39 | 18 | pc.printf("Sending %d\n\r", directions[i]); |
| gsulc | 0:178a07cd3e39 | 19 | if(directions[i] == TERMINATE_CHAR) { |
| gsulc | 0:178a07cd3e39 | 20 | if (DEBUG) |
| gsulc | 0:178a07cd3e39 | 21 | pc.printf("Successfully transfered directions.\n\r"); |
| gsulc | 0:178a07cd3e39 | 22 | return 1; |
| gsulc | 0:178a07cd3e39 | 23 | } |
| gsulc | 0:178a07cd3e39 | 24 | i++; |
| gsulc | 0:178a07cd3e39 | 25 | } |
| gsulc | 0:178a07cd3e39 | 26 | |
| gsulc | 0:178a07cd3e39 | 27 | if (DEBUG) |
| gsulc | 0:178a07cd3e39 | 28 | pc.printf("Error writing directions.\n\r"); |
| gsulc | 0:178a07cd3e39 | 29 | return 0; |
| gsulc | 0:178a07cd3e39 | 30 | } |
| gsulc | 0:178a07cd3e39 | 31 | |
| gsulc | 0:178a07cd3e39 | 32 | /********************************************************* |
| gsulc | 0:178a07cd3e39 | 33 | Name: get_directions |
| gsulc | 0:178a07cd3e39 | 34 | inputs: |
| gsulc | 0:178a07cd3e39 | 35 | void |
| gsulc | 0:178a07cd3e39 | 36 | outputs: |
| gsulc | 0:178a07cd3e39 | 37 | int: 1 = completed sucessfully; 0 = unsuccessful |
| gsulc | 0:178a07cd3e39 | 38 | Purpose: gets directions on where to move |
| gsulc | 0:178a07cd3e39 | 39 | *********************************************************/ |
| gsulc | 0:178a07cd3e39 | 40 | int get_directions() { |
| gsulc | 0:178a07cd3e39 | 41 | //int buff[DIRECTIONS]; |
| gsulc | 0:178a07cd3e39 | 42 | int i; |
| gsulc | 0:178a07cd3e39 | 43 | // wait until the xbee can be read |
| gsulc | 0:178a07cd3e39 | 44 | //while(!xbee.readable()) {} |
| gsulc | 0:178a07cd3e39 | 45 | //read header |
| gsulc | 0:178a07cd3e39 | 46 | |
| gsulc | 0:178a07cd3e39 | 47 | //read contents |
| gsulc | 0:178a07cd3e39 | 48 | i = 0; |
| gsulc | 0:178a07cd3e39 | 49 | while(1){ |
| gsulc | 0:178a07cd3e39 | 50 | directions[i] = xbee.getc() - 2; |
| gsulc | 0:178a07cd3e39 | 51 | if (DEBUG) |
| gsulc | 0:178a07cd3e39 | 52 | pc.printf("dir %d\n\r", directions[i]); |
| gsulc | 0:178a07cd3e39 | 53 | if(directions[i] == TERMINATE_CHAR) |
| gsulc | 0:178a07cd3e39 | 54 | return 1; //successfully transfered directions |
| gsulc | 0:178a07cd3e39 | 55 | i++; |
| gsulc | 0:178a07cd3e39 | 56 | } |
| gsulc | 0:178a07cd3e39 | 57 | |
| gsulc | 0:178a07cd3e39 | 58 | if (DEBUG) |
| gsulc | 0:178a07cd3e39 | 59 | pc.printf("Error reading directions.\n\r"); |
| gsulc | 0:178a07cd3e39 | 60 | return 0; |
| gsulc | 0:178a07cd3e39 | 61 | } |
| gsulc | 0:178a07cd3e39 | 62 | |
| gsulc | 0:178a07cd3e39 | 63 | int gather() { |
| gsulc | 0:178a07cd3e39 | 64 | return 1; |
| gsulc | 0:178a07cd3e39 | 65 | } |
| gsulc | 0:178a07cd3e39 | 66 | |
| gsulc | 0:178a07cd3e39 | 67 | int goto_pollen() { |
| gsulc | 0:178a07cd3e39 | 68 | return 1; |
| gsulc | 0:178a07cd3e39 | 69 | } |
| gsulc | 0:178a07cd3e39 | 70 | |
| gsulc | 0:178a07cd3e39 | 71 | void blaze() { |
| gsulc | 0:178a07cd3e39 | 72 | int i; |
| gsulc | 0:178a07cd3e39 | 73 | for (i = 0; i < (2*DIRECTIONS); i++) { |
| gsulc | 0:178a07cd3e39 | 74 | directions[i] = rand() % 3 - 1; |
| gsulc | 0:178a07cd3e39 | 75 | directions[++i] = rand() % 20 + 51; |
| gsulc | 0:178a07cd3e39 | 76 | if(DEBUG){ |
| gsulc | 0:178a07cd3e39 | 77 | pc.printf("Directions: %d, %d\n\r", directions[i-1], directions[i]); |
| gsulc | 0:178a07cd3e39 | 78 | } |
| gsulc | 0:178a07cd3e39 | 79 | } |
| gsulc | 0:178a07cd3e39 | 80 | directions[i] = TERMINATE_CHAR; |
| gsulc | 0:178a07cd3e39 | 81 | if(DEBUG) |
| gsulc | 0:178a07cd3e39 | 82 | pc.printf("Temrinate\n\r"); |
| gsulc | 0:178a07cd3e39 | 83 | } |
| gsulc | 0:178a07cd3e39 | 84 | |
| gsulc | 0:178a07cd3e39 | 85 | int copy() { |
| gsulc | 0:178a07cd3e39 | 86 | static int direct = 0; |
| gsulc | 0:178a07cd3e39 | 87 | static int state = 0; |
| gsulc | 0:178a07cd3e39 | 88 | |
| gsulc | 0:178a07cd3e39 | 89 | /*if(dotype == GetMove) { |
| gsulc | 0:178a07cd3e39 | 90 | current_move = directions[direct]; |
| gsulc | 0:178a07cd3e39 | 91 | |
| gsulc | 0:178a07cd3e39 | 92 | }*/ |
| gsulc | 0:178a07cd3e39 | 93 | |
| gsulc | 0:178a07cd3e39 | 94 | //else if (dotype == DoMove) { |
| gsulc | 0:178a07cd3e39 | 95 | |
| gsulc | 0:178a07cd3e39 | 96 | static int current_move; |
| gsulc | 0:178a07cd3e39 | 97 | |
| gsulc | 0:178a07cd3e39 | 98 | if(state == 0) { |
| gsulc | 0:178a07cd3e39 | 99 | current_move = directions[direct]; |
| gsulc | 0:178a07cd3e39 | 100 | if(DEBUG) |
| gsulc | 0:178a07cd3e39 | 101 | pc.printf("Oh! got the Move! %d\n\r", current_move); |
| gsulc | 0:178a07cd3e39 | 102 | state = 1; |
| gsulc | 0:178a07cd3e39 | 103 | } |
| gsulc | 0:178a07cd3e39 | 104 | else if (state == 2) { |
| gsulc | 0:178a07cd3e39 | 105 | if(move_state == Stopped) |
| gsulc | 0:178a07cd3e39 | 106 | state = 0; |
| gsulc | 0:178a07cd3e39 | 107 | if(DEBUG) |
| gsulc | 0:178a07cd3e39 | 108 | pc.printf("Waiting for Next Move\n\r"); |
| gsulc | 0:178a07cd3e39 | 109 | } |
| gsulc | 0:178a07cd3e39 | 110 | |
| gsulc | 0:178a07cd3e39 | 111 | else if (state == 1) { |
| gsulc | 0:178a07cd3e39 | 112 | if (current_move == TERMINATE_CHAR) { |
| gsulc | 0:178a07cd3e39 | 113 | l_motor.speed(NEUTRAL); |
| gsulc | 0:178a07cd3e39 | 114 | r_motor.speed(NEUTRAL); |
| gsulc | 0:178a07cd3e39 | 115 | move_state = Stopped; |
| gsulc | 0:178a07cd3e39 | 116 | return 1; |
| gsulc | 0:178a07cd3e39 | 117 | } |
| gsulc | 0:178a07cd3e39 | 118 | else if (current_move <= 1) { |
| gsulc | 0:178a07cd3e39 | 119 | if(DEBUG) |
| gsulc | 0:178a07cd3e39 | 120 | pc.printf("I like to rotate! \n\r"); |
| gsulc | 0:178a07cd3e39 | 121 | rotate(current_move); |
| gsulc | 0:178a07cd3e39 | 122 | //if((r_tics >= r_goal) || (l_tics >= l_goal)) |
| gsulc | 0:178a07cd3e39 | 123 | direct++; |
| gsulc | 0:178a07cd3e39 | 124 | } |
| gsulc | 0:178a07cd3e39 | 125 | else if (current_move >= 20) { |
| gsulc | 0:178a07cd3e39 | 126 | if(DEBUG) |
| gsulc | 0:178a07cd3e39 | 127 | pc.printf("I like to move it move it! \n\r"); |
| gsulc | 0:178a07cd3e39 | 128 | |
| gsulc | 0:178a07cd3e39 | 129 | move(0.5, current_move); |
| gsulc | 0:178a07cd3e39 | 130 | //if((r_tics >= r_goal) || (l_tics >= l_goal)) |
| gsulc | 0:178a07cd3e39 | 131 | direct++; |
| gsulc | 0:178a07cd3e39 | 132 | } |
| gsulc | 0:178a07cd3e39 | 133 | state = 2; |
| gsulc | 0:178a07cd3e39 | 134 | } |
| gsulc | 0:178a07cd3e39 | 135 | |
| gsulc | 0:178a07cd3e39 | 136 | return 0; |
| gsulc | 0:178a07cd3e39 | 137 | } |
| gsulc | 0:178a07cd3e39 | 138 |