void DropletCustomOne::DropletMainLoop()
{
if ( !is_moving(NULL) )
{
switch ( rand_byte() % 3 )
{
case 0:
{
int8_t sign = rand_byte() % 2 ? 1 : -1;
rotate_degrees ( 90 * sign );
set_rgb_led ( 50, 0, 0 );
}
break;
case 1:
move_duration ( rand_byte() % 6, 3000 );
set_rgb_led ( 0, 50, 0 );
break;
case 2:
move_steps ( rand_byte() % 6, 100 );
set_rgb_led ( 0, 0, 50 );
break;
default:
break;
}
}
}
void DropletCustomOne::random_walk ( void )
{
if ( !is_moving(NULL) )
{
// Either walk or rotate
if ( rand_byte() % 3 == 0 )
{
int8_t sign = rand_byte() % 2 ? 1 : -1;
uint8_t degrees;
while ( (degrees = rand_byte()) <= 90 );
rotate_degrees ( sign * degrees );
}
else
{
last_move_dir = rand_byte() % 6;
move_duration ( last_move_dir, rand_byte() * MOVE_DIST_SCALAR );
}
}
}
void move(unsigned char direction)
{
volatile unsigned int distance_mm_1 =267;
volatile unsigned int distance_mm_2 = 30;
volatile unsigned int distance_mm_3 = 237;
volatile unsigned int distance_mm_4 = 90;
volatile unsigned int distance_mm_5 = 95;
volatile unsigned int distance_mm_hor =140;
volatile unsigned int distance_mm_back = 108;
volatile unsigned int degrees_r = 90;
volatile unsigned int degrees_l = 90;
volatile unsigned int degrees_pi= 180;
if(direction == 0x00)
{
PORTA = 0x00; //stop
}
else if(direction == 0x01)
{
velocity(225,205);
PORTA = 0x06; //forward
linear_distance(distance_mm_1);
}
else if(direction == 0x02)
{
velocity(225,205);
PORTA = 0x09; //backward
linear_distance(distance_mm_back);
}
else if(direction == 0x03)
{
velocity(160,150);
PORTA = 0x06; //forward
linear_distance(distance_mm_2);
PORTA = 0x0A; //zero right
rotate_degrees(degrees_r);
}
else if(direction == 0x04)
{
velocity(156,149);
PORTA = 0x06; //forward
linear_distance(distance_mm_2);
PORTA = 0x05; //zero left
rotate_degrees(degrees_l);
}
else if(direction == 0x05)
{
velocity(225,205);
PORTA = 0x06; //forward
linear_distance(distance_mm_hor);
}
else if(direction == 0x13)
{
velocity(153,149);
PORTA = 0x0A; //zero right
rotate_degrees(degrees_pi);
}
else if(direction == 0x14)
{
velocity(225,205);
PORTA = 0x06; //forward
linear_distance(distance_mm_3);
}
else if(direction == 0x15)
{
velocity(154,149);
PORTA = 0x05; //zero left
rotate_degrees(degrees_pi);
}
else if(direction == 0x16)
{
velocity(225,205);
PORTA = 0x06; //forward
linear_distance(distance_mm_4);
}
else if(direction == 0x17)
{
velocity(225,205);
PORTA = 0x06; //forward
linear_distance(distance_mm_5);
}
}
void DropletCustomFive::DropletMainLoop()
{
switch(state)
{
case WANDER:
if(!wander_rgb)
{
set_rgb_led(255,0,255);
wander_rgb = true;
}
if(!is_moving())
{
move_steps((rand_byte() % 6) + 1, rand_byte() * 50);
}
if(!first_set)
{
uint8_t r, g, b;
get_rgb_sensor(&r, &g, &b);
if(r > 200)
{
first_set = true;
char msg = 'R';
ir_broadcast(&msg, sizeof(msg));
cancel_move();
cancel_rotate();
guitar_id = 0;
row_num = 0;
num_in_row = 1;
state = CALL;
break;
}
}
while(check_for_new_messages())
{
char let = global_rx_buffer.buf[0];
uint8_t num;
uint16_t idnum;
switch(let)
{
case 'V':
float dist, theta, phi;
range_and_bearing(global_rx_buffer.sender_ID, &dist, &theta, &phi);
num = global_rx_buffer.buf[1];
if(dist<20 && dist>10)
{
char msg[2];
memset(&msg[0], 0, 2);
msg[0] = 'C';
msg[1] = num;
ir_broadcast(&msg[0], sizeof(msg));
}
break;
case 'S':
memcpy(&idnum, &global_rx_buffer.buf[1], 2);
if(idnum == get_droplet_id())
{
target = global_rx_buffer.sender_ID;
slot_num = (uint8_t)global_rx_buffer.buf[3];
guitar_id = (uint8_t)global_rx_buffer.buf[4];
cancel_move();
cancel_rotate();
state = DOCK;
break;
}
break;
case 'R':
first_set = true;
break;
case 'D':
range_and_bearing(global_rx_buffer.sender_ID, &dist, &theta, &phi);
if(dist<30)
{
rotate_degrees(static_cast<int16_t>(theta+160));
move_steps(NORTH, 30);
}
break;
default:
break;
}
global_rx_buffer.read=true;
}
break;
case DOCK:
if(!dock_rgb)
{
set_rgb_led(150,150,150);
dock_rgb = true;
}
if(!is_moving())
{
float dist, theta, phi;
if(!range_and_bearing(target, &dist, &theta, &phi))
{
state = WANDER;
set_rgb_led(0, 0, 0);
move_steps((rand_byte() % 6) + 1, rand_byte()*50);
break;
}
if(!is_rotating() && abs(theta) > 2.5f)
{
rotate_degrees(static_cast<int16_t>(theta));
}
if(!is_rotating() && abs(theta) <= 2.5f)
{
move_steps(NORTH, static_cast<uint16_t>(dist));
}
if(dist <= 20)
{
if(!clock_wise) // rotate around target counter clockwise
{
rotate_degrees(static_cast<int16_t>(theta-90));
move_steps(NORTH, 2);
char msg = 'D';
ir_broadcast(&msg, 1);
if(theta>85)
{
switch(slot_num)
{
case 0:
if(phi < -75 && phi > -180)
{
cancel_move();
cancel_rotate();
rotate_degrees(static_cast<int16_t>(-180));
clock_wise = true;
break;
}
if(phi<-59 && phi>-61)
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
case 1:
if(phi < -75 && phi > -180)
{
cancel_move();
cancel_rotate();
rotate_degrees(static_cast<int16_t>(-180));
clock_wise = true;
break;
}
if(phi>-1 && phi<1)
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
case 2:
if(phi>59 && phi<61)
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
case 3:
if(phi<121 && phi>119)
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
case 4:
if((phi>179 && phi<181)||(phi>-181 && phi<-179))
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
case 5:
if(phi>-121 && phi<-119)
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
default:
break;
}
}
}
else // rotate around target clockwise
{
rotate_degrees(static_cast<int16_t>(theta+90));
move_steps(NORTH, 2);
char msg = 'D';
ir_broadcast(&msg, 1);
if(theta<-85)
{
switch(slot_num)
{
case 0:
if(phi < -30 && phi > -75)
{
cancel_move();
cancel_rotate();
rotate_degrees(static_cast<int16_t>(180));
clock_wise = false;
break;
}
if(phi<121 && phi>119)
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
case 1:
if(phi < -50 && phi > -75)
{
cancel_move();
cancel_rotate();
rotate_degrees(static_cast<int16_t>(180));
clock_wise = false;
break;
}
if((phi>177 && phi<181)||(phi<-177 && phi>-181))
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
case 2:
if(phi>-121 && phi<-119)
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
case 3:
if(phi>-31 && phi<-29)
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
case 4:
if(phi>-1 && phi<1)
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
case 5:
if(phi>59 && phi<61)
{
cancel_move();
cancel_rotate();
state = ALIGN;
break;
}
break;
default:
break;
}
}
}
}
}
if(check_for_new_messages())
{
char let = (char)global_rx_buffer.buf[0];
uint8_t num = (uint8_t)global_rx_buffer.buf[1];
switch(let)
{
case 'A':
if(num == guitar_id)
{
wander_rgb = false;
state = WANDER;
}
case 'F':
if(num == guitar_id)
{
wander_rgb = false;
state = WANDER;
}
break;
case 'T':
float dist, theta, phi;
range_and_bearing(global_rx_buffer.sender_ID, &dist, &theta, &phi);
if(dist<5)
{
move_steps(SOUTH, 5);
if(clock_wise)
{
cancel_move();
cancel_rotate();
clock_wise = false;
rotate_degrees(static_cast<int16_t>(theta+165));
move_steps(NORTH, 20);
break;
}
else
{
cancel_move();
cancel_rotate();
clock_wise = true;
rotate_degrees(static_cast<int16_t>(theta+165));
move_steps(NORTH, 20);
break;
}
}
break;
default:
break;
}
global_rx_buffer.read = true;
}
break;
case ALIGN:
if(!align_rgb)
{
set_rgb_led(255,0,0);
align_rgb = true;
}
float dist, theta, phi;
range_and_bearing(target, &dist, &theta, &phi);
rotate_degrees(static_cast<int16_t>(theta));
move_steps(NORTH, static_cast<uint16_t>(dist));
if(dist<5)
{
cancel_move();
rotate_degrees(static_cast<int16_t>(phi));
if(phi < 2 && phi >-2)
{
char msg[4];
msg[0] = 'F';
memcpy(&msg[1], &target, sizeof(uint16_t));
msg[3] = slot_num;
ir_broadcast(&msg[0], sizeof(msg));
state = PRESET;
break;
}
}
while(check_for_new_messages())
{
char let = (char)global_rx_buffer.buf[0];
uint8_t num = (uint8_t)global_rx_buffer.buf[1];
switch(let)
{
case 'A':
if(num == guitar_id)
{
wander_rgb = false;
state = WANDER;
}
case 'F':
if(num == guitar_id)
{
wander_rgb = false;
state = WANDER;
}
break;
default:
break;
}
global_rx_buffer.read = true;
}
break;
case CALL:
if(!call_rgb)
{
set_rgb_led(0,255,255);
call_rgb = true;
}
if(!first_search)
{
find_empty_neighbors(guitar_id, row_num, num_in_row);
first_search = true;
set_timer(10000, 0);
}
if(check_timer(0))
{
for(uint8_t i=0; i<6; i++)
{
if((neighbors[i]>0) && (neighbors[i]<200))
{
/* this message calls needed droplets */
char msg[2];
memset(&msg[0], 0, 2);
msg[0] = 'V';
msg[1] = i;
ir_broadcast(&msg[0], sizeof(msg));
}
}
}
else
{
for(uint8_t i=0; i<6; i++)
{
if((neighbors[i]>0))
{
/* timer hasn't gone off, we're list building and calling out
all possible neighbors to see which ones are already there */
char msg[2];
memset(&msg[0], 0, 2);
msg[0] = 'N';
msg[1] = neighbors[i];
ir_broadcast(&msg[0], sizeof(msg));
}
}
}
while(check_for_new_messages())
{
char let = global_rx_buffer.buf[0];
uint8_t num;
uint16_t send_id;
switch(let)
{
case 'C':
send_id = global_rx_buffer.sender_ID;
num = (uint8_t)global_rx_buffer.buf[1];
if((neighbors[num]>0) && (neighbors[num]<200))
{
char msg[5];
memset(&msg[0], 0, 3);
msg[0] = 'S';
memcpy(&msg[1], &send_id, sizeof(uint16_t));
msg[3] = num;
msg[4] = neighbors[num];
ir_broadcast(&msg[0], sizeof(msg));
neighbors[num] = 255;
}
break;
case 'F':
memcpy(&send_id, &global_rx_buffer.buf[1], sizeof(uint16_t));
num = (uint8_t)global_rx_buffer.buf[3];
if(send_id == get_droplet_id())
{
set_rgb_led(255, 0, 0);
call_rgb = false;
neighbors[num] = 0;
slots_set++;
}
break;
case 'A':
num = (uint8_t)global_rx_buffer.buf[1];
for(uint8_t i=0; i<6; i++)
{
if((neighbors[i] > 0) && (neighbors[i] == num))
{
neighbors[i] = 0;
slots_needed--;
break;
}
}
break;
default:
break;
}
global_rx_buffer.read = true;
}
if(slots_set == slots_needed)
{
char msg[2];
memset(&msg[0], 0, 2);
msg[0] = 'L';
msg[1] = (guitar_id + 1);
ir_broadcast(&msg[0], sizeof(msg));
is_set_rgb = false;
state = SET;
break;
}
/*if((neighbors[1])==0 && (neighbors[2]==0) && (neighbors[3]==0) && (neighbors[4]==0) && (neighbors[5]==0))
{
char msg[2];
memset(&msg[0], 0, 2);
msg[0] = 'L';
msg[1] = (guitar_id + 1);
ir_broadcast(&msg[0], sizeof(msg));
is_set_rgb = false;
state = SET;
break;
}*/
break;
case PRESET:
if(!preset_rgb)
{
set_rgb_led(0, 255, 0);
preset_rgb = true;
}
set_rgb_led(0, 255, 0);
while(check_for_new_messages())
{
char let = (char)global_rx_buffer.buf[0];
uint8_t num = (uint8_t)global_rx_buffer.buf[1];
uint8_t num2;
switch(let)
{
case 'S':
num2 = (uint8_t)global_rx_buffer.buf[4];
if(num2 == guitar_id)
{
set_rgb_led(255, 0, 0);
char msg[2];
memset(&msg[0], 0, 2);
msg[0] = 'A';
msg[1] = guitar_id;
ir_broadcast(&msg[0], sizeof(msg));
}
break;
case 'L':
if(num==guitar_id)
{
row_num = get_row_num(guitar_id);
num_in_row = get_num_in_row(guitar_id);
call_rgb = false;
state = CALL;
}
break;
case 'N':
if(num==guitar_id)
{
set_rgb_led(255, 0, 0);
char msg[2];
memset(&msg[0], 0, 2);
msg[0] = 'A';
msg[1] = guitar_id;
ir_broadcast(&msg[0], sizeof(msg));
}
break;
case 'D':
float dist, theta, phi;
range_and_bearing(global_rx_buffer.sender_ID, &dist, &theta, &phi);
if(dist<5)
{
char msg = 'T';
ir_broadcast(&msg, 1);
}
break;
default:
break;
}
global_rx_buffer.read = true;
}
break;
case SET:
if(!is_set_rgb)
{
set_rgb_led(0,0,255);
is_set_rgb = true;
}
while(check_for_new_messages())
{
char let = (char)global_rx_buffer.buf[0];
uint8_t num = (uint8_t)global_rx_buffer.buf[1];
switch(let)
{
case 'N':
if(num==guitar_id)
{
set_rgb_led(255, 0 , 0);
is_set_rgb = false;
char msg[2];
memset(&msg[0], 0, 2);
msg[0] = 'A';
msg[1] = guitar_id;
ir_broadcast(&msg[0], sizeof(msg));
}
break;
case 'D':
float dist, theta, phi;
range_and_bearing(global_rx_buffer.sender_ID, &dist, &theta, &phi);
if(dist<5)
{
char msg = 'T';
ir_broadcast(&msg, 1);
}
break;
default:
break;
}
global_rx_buffer.read = true;
}
break;
}
}