/*---------------------------------------------------------------------------*/ int c_multihop_send(struct pipe *p, struct stackmodule_i *module) { PRINTF("c_multihop_send \n"); printaddr(module->stack_id); packetbuf_compact(); rimeaddr_t *nexthop; rimeaddr_t *tmpaddr = get_node_addr(module->stack_id, 0, 3); nexthop = c_forward(stack[module->stack_id].pip, stack[module->stack_id].amodule, module->module_id); if (nexthop == NULL) { PRINTF("multihop_send: no route\n"); return 0; } else { set_node_addr(module->stack_id, 0, 2, nexthop); PRINTF("multihop_send: sending data towards %d.%d\n", nexthop->u8[0], nexthop->u8[1]); packetbuf_set_addr(PACKETBUF_ADDR_ERECEIVER, tmpaddr); rimeaddr_t *tmpaddr1 = get_node_addr(module->stack_id, 0, 1); packetbuf_set_addr(PACKETBUF_ADDR_ESENDER, tmpaddr1); rimeaddr_t esender, ereceiver; rimeaddr_copy(&esender, packetbuf_addr(PACKETBUF_ADDR_ESENDER)); rimeaddr_copy(&ereceiver, packetbuf_addr(PACKETBUF_ADDR_ERECEIVER)); packetbuf_set_attr(PACKETBUF_ATTR_HOPS, 1); return 1; } }
void c_multihop_recv(struct pipe *p, struct stackmodule_i *module) { PRINTF("c_multihop_recv \n"); printaddr(module->stack_id); rimeaddr_t *nexthop = NULL; rimeaddr_t esender, ereceiver; /* Copy the packet attributes to avoid them being overwritten or cleared by an application program that uses the packet buffer for its own needs. */ //rimeaddr_copy(&esender, packetbuf_addr(PACKETBUF_ADDR_ESENDER)); rimeaddr_copy(&ereceiver, packetbuf_addr(PACKETBUF_ADDR_ERECEIVER)); if (rimeaddr_cmp(packetbuf_addr(PACKETBUF_ADDR_ERECEIVER), &rimeaddr_node_addr)) { stack[module->stack_id].resend_flg = 1; PRINTF("for us!\n"); p->multihop_param.hop_no = packetbuf_attr(PACKETBUF_ATTR_HOPS); } else { nexthop = c_forward(p, stack[module->stack_id].amodule, stack[module->stack_id].modno); PRINTF("nexthop: %d.%d \n", nexthop->u8[0], nexthop->u8[1]); packetbuf_set_attr(PACKETBUF_ATTR_HOPS, packetbuf_attr(PACKETBUF_ATTR_HOPS) + 1); if (nexthop) { set_node_addr(module->stack_id, 0, 2, nexthop); PRINTF("forwarding to %d.%d\n", nexthop->u8[0], nexthop->u8[1]); if (module->stack_id < STACKNO) { //stack_send(&stack[module->stack_id], module->module_id); stack_send(&stack[module->stack_id], module->module_id); } } } }
int main() { printf("Online%\n\r"); wait_ms(500); /*inturupts*/ w1_cha.fall(&count_w1_a); w1_chb.fall(&count_w1_b); w2_cha.fall(&count_w2_a); w2_chb.fall(&count_w2_b); c_forward(); //sets the robot to go forward while(1) { s1_check = s1; s2_check = s2; s3_check = s3; //s4_check = s4; if(s1_check != 1) { printf("S1 Hit \n\r"); } if(s2_check != 1) { printf("S2 Hit \n\r"); } if(s3_check != 1) { printf("S3 Hit \n\r"); } // if(s4_check != 1) //{ //printf("S4 Hit \n\r"); //} if (w1_count_a != w1_dif_a){ w1_dif_a = w1_count_a; printf("w1 count a = %d\n\r", w1_dif_a); } if (w1_count_b != w1_dif_b){ w1_dif_b = w1_count_b; printf("w1 count b = %d\n\r", w1_dif_b); } if (w2_count_a != w2_dif_a){ w2_dif_a = w2_count_a; printf("w2 count a = %d\n\r", w2_dif_a); } if (w2_count_b != w2_dif_b){ w2_dif_b = w2_count_b; printf("w2 count b = %d\n\r", w2_dif_b); } /*goes backwards if it hits a detects a wall*/ if (s1 != 1 && s2 !=1) { backward();//to correct when aligning c_backward(); } /*goes forwards when detecting the wall*/ if (s3 != 1){ c_forward(); } /*tries to aligin with the wall*/ if (s1 != 1 && s2 != 0) { while(s1 != 1 && s2 != 0){ c_right(); } backward(); //dir = 0; //c_backward(); } /*tries to aligin with the wall*/ if (s1 != 0 && s2 != 1) { while(s1 != 0 && s2 != 1){ c_left(); } backward(); //dir = 0; //c_backward(); } /*if all sensors triggured tries to turn to escape*/ if (s1 != 1 && s2 != 1 && s3 != 1) { turnl(); } /*if no senors triggred tries to contiue on*/ if (s1 != 0 && s2 != 0 && s3 != 0) { if (dir == 0){ c_backward(); } else{ c_forward(); } } } }