/*---------------------------------------------------------------------------*/
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();
}
}
}
}
