uint8_t putPaint(uint8_t side) { printf("putPaint \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); asserv_set_vitesse_normal(&asserv); if(side == RED) { set_goalCoor(G_LENGTH * 3 + 7); } else { set_goalCoor(G_LENGTH * 3 + 8); } if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; printf("angle %lf \n",position_get_angle_deg(&pos)); trajectory_goto_a(&traj, END, 180); trajectory_goto_d(&traj, END, 10); while(!trajectory_is_ended(&traj)); trajectory_goto_a(&traj, END, 0); asserv_set_vitesse_normal(&asserv); if(actionIsFailed())return FAILED; disableAvoidance(); printf("angle %lf \n",position_get_angle_deg(&pos)); trajectory_goto_d(&traj, END, -50); while(!trajectory_is_ended(&traj)); trajectory_goto_d(&traj, END, 10); while(!trajectory_is_ended(&traj)); // if(team == RED) // { // findPosition(REDPAINT); // } // else // { // findPosition(YELLOWPAINT); // } enableAvoidance(); return DONE; }
uint8_t putFruit(uint8_t side) { asserv_set_vitesse_normal(&asserv); printf("begin take fruit"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); printf("start pos: %d eps %lf \n", position_get_coor_eps(&pos, &eps),eps); static uint8_t k = 0; if(team == RED) { set_goalCoor(G_LENGTH * 7 + 11); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); printf("start pos: %d eps %lf \n", position_get_coor_eps(&pos, &eps),eps); set_goalCoor(G_LENGTH * 3 + 11); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } actionSucceed(); while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; trajectory_goto_a(&traj, END, 0); while(!trajectory_is_ended(&traj)); if(actionIsFailed())return FAILED; disableSpinning(); trajectory_goto_d(&traj, END, -40); enableSpinning(); while(!trajectory_is_ended(&traj)); // { // if(REPOSITIONING) // { // trajectory_reinit(&traj); // asserv_stop(&asserv); // } // } enableSpinning(); //se recaller? throwSpears(YELLOW); trajectory_goto_d(&traj, END, -4); while(!trajectory_is_ended(&traj)); if(k) { trajectory_goto_a(&traj, END, -90); } else { trajectory_goto_a(&traj, END, -90); } disableAvoidance(); //trajectory_goto_d(&traj, END, -40); while(!trajectory_is_ended(&traj)); enableAvoidance(); //if(actionIsFailed())return FAILED; mecaCom(TIROIR_DEVERSER); wait_ms(2000); mecaCom(TIROIR_FERMER); //trajectory_goto_d(&traj, END, -60); //trajectory_goto_a(&traj, END, 0); //trajectory_goto_d(&traj, END,15); //while(!trajectory_is_ended(&traj)); } else { set_goalCoor(G_LENGTH * 7 + 3); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); printf("start pos: %d eps %lf \n", position_get_coor_eps(&pos, &eps),eps); set_goalCoor(G_LENGTH * 4 + 3); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } actionSucceed(); while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; trajectory_goto_a(&traj, END, 180); trajectory_goto_d(&traj, END, 10); trajectory_goto_a(&traj, END, 0); while(!trajectory_is_ended(&traj)); if(actionIsFailed())return FAILED; trajectory_goto_d(&traj, END, -40); disableSpinning(); enableSpinning();// a tester while(!trajectory_is_ended(&traj)); // { // if(REPOSITIONING) // { // trajectory_reinit(&traj); // asserv_stop(&asserv); // } // } enableSpinning(); throwSpears(RED); trajectory_goto_d(&traj, END, -5); while(!trajectory_is_ended(&traj)); if(k) { trajectory_goto_a(&traj, END, -90); } else { trajectory_goto_a(&traj, END, -90); } disableAvoidance(); while(!trajectory_is_ended(&traj)); enableAvoidance(); // if(actionIsFailed())return FAILED; mecaCom(TIROIR_DEVERSER); wait_ms(2000); mecaCom(TIROIR_FERMER); // if(k) // { // trajectory_goto_a(&traj, END, 0); // trajectory_goto_d(&traj, END, 12); // } // else // { // trajectory_goto_a(&traj, END, 90); // k = 1; // //trajectory_goto_d(&traj, END, 0); // } // while(!trajectory_is_ended(&traj)); } return DONE; }
uint8_t takeFruitYellow(uint8_t type) { double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); printf("start pos: %d eps %lf \n",position_get_coor_eps(&pos, &eps),eps); set_goalCoor(G_LENGTH * 8 + 9); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; printf("take the fruit"); mecaCom(PEIGNE_OUVERT); mecaCom(TIROIR_OUVERT); trajectory_goto_a(&traj, END, 0); if (team == RED) { trajectory_goto_d(&traj, END, 4); } else { trajectory_goto_d(&traj, END, 10); } trajectory_goto_a(&traj, END, 90); while(!trajectory_is_ended(&traj)); if(actionIsFailed())return FAILED; asserv_set_vitesse_normal(&asserv); trajectory_goto_d(&traj, END, 80); trajectory_goto_a(&traj, END, 135); if(team == RED) { trajectory_goto_d(&traj, END, 11 * 1.414); } else { trajectory_goto_d(&traj, END, 11 * 1.414); } // trajectory_goto_a(&traj, END, 90); // while(!trajectory_is_ended(&traj)); // trajectory_goto_d(&traj, END, 12); // while(!trajectory_is_ended(&traj)); trajectory_goto_a(&traj, END, 180); trajectory_goto_d(&traj, END, 55); trajectory_goto_a(&traj, END, -90); if(team == RED) { trajectory_goto_d(&traj, END, 11 * 1.414); } else { trajectory_goto_d(&traj, END, 12 * 1.414); } while(!trajectory_is_ended(&traj)); mecaCom(PEIGNE_FERMER); asserv_set_vitesse_normal(&asserv); if(actionIsFailed())return FAILED; return DONE; }
uint8_t returnFire(uint8_t fireNb) { static uint8_t tab[7] = {0}; for(uint8_t i = 1;i < 7;i++) { if(!(tab[i] & DONE)) { avoidFire(i); } } uint16_t stackend = 0; printf("stack %d \n",&stackend - &stack_begin); if(fireNb == R1) { printf("return R1 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 3 + 3); } else { set_goalCoor(G_LENGTH * 3 + 6); asserv_set_vitesse_normal(&asserv); } //put a wall to avoid the wrong way avoidFire(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 3 + 5); } else { set_goalCoor(G_LENGTH * 3 + 4); } //remove the wall stopAvoid(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; } else if(fireNb == R2) { printf("return R2 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 4 + 2); } else { set_goalCoor(G_LENGTH * 7 + 2); } //put a wall to avoid the wrong way avoidFire(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 6 + 2); } else { set_goalCoor(G_LENGTH * 5 + 2); } //remove the wall stopAvoid(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; } else if(fireNb == R3) { printf("return R3 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 8 + 6); } else { set_goalCoor(G_LENGTH * 8 + 3); } //put a wall to avoid the wrong way avoidFire(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 8 + 4); } else { set_goalCoor(G_LENGTH * 8 + 5); } //remove the wall stopAvoid(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; } else if(fireNb == Y1) { printf("return Y1 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 3 + 9); } else { set_goalCoor(G_LENGTH * 3 + 12); } //put a wall to avoid the wrong way avoidFire(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 3 + 11); } else { set_goalCoor(G_LENGTH * 3 + 10); } //remove the wall stopAvoid(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; } else if(fireNb == Y2) { printf("return Y2 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 7 + 13); } else { set_goalCoor(G_LENGTH * 4 + 13); } //put a wall to avoid the wrong way avoidFire(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 5 + 13); } else { set_goalCoor(G_LENGTH * 6 + 13); } //remove the wall stopAvoid(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; } else if(fireNb == Y3) { printf("return Y3 \n"); double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 8 + 12); } else { set_goalCoor(G_LENGTH * 8 + 9); } //put a wall to avoid the wrong way avoidFire(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; //test if it succeed eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); if(team == RED) { set_goalCoor(G_LENGTH * 8 + 10); } else { set_goalCoor(G_LENGTH * 8 + 11); } //remove the wall stopAvoid(fireNb); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); if(actionIsFailed())return FAILED; } else { } //if succeed tab[fireNb] += DONE; for(uint8_t i = 1;i < 7;i++) { if(!(tab[i] & DONE)) { stopAvoid(i); } } return DONE; }
uint8_t throwSpears(uint8_t side) { if(side == RED) { if(team == RED) { double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); set_goalCoor(G_LENGTH * 3 + 3); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv()); trajectory_goto_a(&traj, END, 90); trajectory_goto_d(&traj, END, 11); while(!trajectory_is_ended(&traj)); disableAvoidance();//test trajectory_goto_a(&traj, END, -180); trajectory_goto_d(&traj, END, 14.8); while(!trajectory_is_ended(&traj)); mecaCom(LANCE_BALLE_AV); trajectory_goto_d(&traj, END, -14.8); while(!trajectory_is_ended(&traj)); enableAvoidance();//test } else { // double eps = 0; // set_startCoor(position_get_coor_eps(&pos,&eps)); // set_goalCoor(G_LENGTH * 3 + 4); // if (eps > EPSILON) // { // go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); // } // while(!astarMv() && !actionIsFailed()); // trajectory_goto_a(&traj,END,0); // trajectory_goto_d(&traj, END, -16.8); // while(!trajectory_is_ended(&traj)); mecaCom(LANCE_BALLE_AR); // trajectory_goto_d(&traj, END, 16.8); // while(!trajectory_is_ended(&traj)); } } else { if(team == RED) { // double eps = 0; // set_startCoor(position_get_coor_eps(&pos,&eps)); // set_goalCoor(G_LENGTH * 3 + 11); // if (eps > EPSILON) // { // go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); // } // while(!astarMv()); // trajectory_goto_a(&traj, END, 0); // trajectory_goto_d(&traj, END, -16.8); // while(!trajectory_is_ended(&traj)); mecaCom(LANCE_BALLE_AR); // trajectory_goto_d(&traj, END, 16.8); // while(!trajectory_is_ended(&traj)); } else { double eps = 0; set_startCoor(position_get_coor_eps(&pos,&eps)); set_goalCoor(G_LENGTH * 3 + 12); if (eps > EPSILON) { go_to_node(fxx_to_double(position_get_y_cm(&pos)),fxx_to_double(position_get_x_cm(&pos))); } while(!astarMv() && !actionIsFailed()); trajectory_goto_a(&traj,END,-90); trajectory_goto_d(&traj, END, 11); while(!trajectory_is_ended(&traj)); disableAvoidance();//test trajectory_goto_a(&traj,END,180); trajectory_goto_d(&traj, END, 14.8); while(!trajectory_is_ended(&traj)); mecaCom(LANCE_BALLE_AV); trajectory_goto_d(&traj, END, -14.8); while(!trajectory_is_ended(&traj)); enableAvoidance();//test } } return DONE; }
// Delete num characters at position pos. Deleting past the end of the string // has no effect. void rope_del(rope *r, size_t pos, size_t length) { #ifdef DEBUG _rope_check(r); #endif assert(r); pos = MIN(pos, r->num_chars); length = MIN(length, r->num_chars - pos); rope_node *nodes[UINT8_MAX]; // the number of characters to skip in the current node. size_t offset; // Search for the node where we'll insert the string. rope_node *e = go_to_node(r, pos, &offset, nodes, NULL); r->num_chars -= length; while (length) { if (e == NULL || offset == e->nexts[0].skip_size) { // Skip this node. e = (nodes[0] ? nodes[0]->nexts[0] : r->heads[0]).node; offset = 0; } size_t num_chars = e->nexts[0].skip_size; size_t removed = MIN(length, num_chars - offset); int i; if (removed < num_chars) { // Just trim this node down to size. size_t leading_bytes = count_bytes_in_chars(e->str, offset); size_t removed_bytes = count_bytes_in_chars(e->str + leading_bytes, removed); size_t trailing_bytes = e->num_bytes - leading_bytes - removed_bytes; if (trailing_bytes) { memmove(e->str + leading_bytes, e->str + leading_bytes + removed_bytes, trailing_bytes); } e->num_bytes -= removed_bytes; r->num_bytes -= removed_bytes; for (i = 0; i < e->height; i++) { e->nexts[i].skip_size -= removed; } } else { // Remove the node. for (i = 0; i < e->height; i++) { rope_next_node *next_node = (nodes[i] ? &nodes[i]->nexts[i] : &r->heads[i]); next_node->node = e->nexts[i].node; next_node->skip_size += e->nexts[i].skip_size - removed; } r->num_bytes -= e->num_bytes; // TODO: Recycle e. rope_node *next = e->nexts[0].node; r->free(e); e = next; } for (; i < r->height; i++) { if (nodes[i]) { nodes[i]->nexts[i].skip_size -= removed; } else { r->heads[i].skip_size -= removed; } } length -= removed; } #ifdef DEBUG _rope_check(r); #endif }
// Insert the given utf8 string into the rope at the specified position. void rope_insert(rope *r, size_t pos, const uint8_t *str) { assert(r); assert(str); #ifdef DEBUG _rope_check(r); #endif pos = MIN(pos, r->num_chars); // There's a good chance we'll have to rewrite a bunch of next pointers and a bunch // of offsets. This variable will store pointers to the elements which need to // be changed. rope_node *nodes[UINT8_MAX]; size_t tree_offsets[UINT8_MAX]; // This is the number of characters to skip in the current node. size_t offset; // First we need to search for the node where we'll insert the string. rope_node *e = go_to_node(r, pos, &offset, nodes, tree_offsets); // offset contains how far (in characters) into the current element to skip. // Figure out how much that is in bytes. size_t offset_bytes = 0; if (e && offset) { assert(offset <= e->num_bytes); offset_bytes = count_bytes_in_chars(e->str, offset); } // Maybe we can insert the characters into the current node? size_t num_inserted_bytes = strlen((char *)str); // Can we insert into the current node? bool insert_here = e && e->num_bytes + num_inserted_bytes <= ROPE_NODE_STR_SIZE; // Can we insert into the subsequent node? bool insert_next = false; rope_node *next = NULL; if (!insert_here) { next = e ? e->nexts[0].node : (r->num_chars ? r->heads[0].node : NULL); // We can insert into the subsequent node if: // - We can't insert into the current node // - There _is_ a next node to insert into // - The insert would be at the start of the next node // - There's room in the next node insert_next = next && (e == NULL || offset_bytes == e->num_bytes) && next->num_bytes + num_inserted_bytes <= ROPE_NODE_STR_SIZE; } if (insert_here || insert_next) { if (insert_next) { offset = offset_bytes = 0; for (int i = 0; i < next->height; i++) { nodes[i] = next; // tree offset nodes not used. } e = next; } // First move the current bytes later on in the string. if (offset_bytes < e->num_bytes) { memmove(&e->str[offset_bytes + num_inserted_bytes], &e->str[offset_bytes], e->num_bytes - offset_bytes); } // Then copy in the string bytes memcpy(&e->str[offset_bytes], str, num_inserted_bytes); e->num_bytes += num_inserted_bytes; r->num_bytes += num_inserted_bytes; size_t num_inserted_chars = strlen_utf8(str); r->num_chars += num_inserted_chars; // .... aaaand update all the offset amounts. update_offset_list(r, nodes, num_inserted_chars); } else { // There isn't room. We'll need to add at least one new node to the rope. // If we're not at the end of the current node, we'll need to remove // the end of the current node's data and reinsert it later. size_t num_end_bytes = 0, num_end_chars; if (e) { num_end_bytes = e->num_bytes - offset_bytes; e->num_bytes = offset_bytes; if (num_end_bytes) { // Count out characters. num_end_chars = e->nexts[0].skip_size - offset; update_offset_list(r, nodes, -num_end_chars); r->num_chars -= num_end_chars; r->num_bytes -= num_end_bytes; } } // Now, we insert new node[s] containing the data. The data must // be broken into pieces of with a maximum size of ROPE_NODE_STR_SIZE. // Node boundaries do not occur in the middle of a utf8 codepoint. size_t str_offset = 0; while (str_offset < num_inserted_bytes) { size_t new_node_bytes = 0; size_t new_node_chars = 0; while (str_offset + new_node_bytes < num_inserted_bytes) { size_t cs = codepoint_size(str[str_offset + new_node_bytes]); if (cs + new_node_bytes > ROPE_NODE_STR_SIZE) { break; } else { new_node_bytes += cs; new_node_chars++; } } insert_at(r, pos, &str[str_offset], new_node_bytes, new_node_chars, nodes, tree_offsets); pos += new_node_chars; str_offset += new_node_bytes; } if (num_end_bytes) { insert_at(r, pos, &e->str[offset_bytes], num_end_bytes, num_end_chars, nodes, tree_offsets); } } #ifdef DEBUG _rope_check(r); #endif }