void checkarm(void)
{
while(1)
{
arm_up();
_delay_ms(2000);
arm_down();
_delay_ms(2000);
}
}
void run(void)
{
data='w';
while(1)
{
while(data == 'w') //waiting for signal
{
}
while(data == '\0' )
{
stop();
_delay_ms(4000);
comintersection();
_delay_ms(100);
}
if(data=='f') // forward
{
data='\0';
}
if(data=='r') // right
{
data='\0';
turn_right();
}
if(data=='l') // left
{
data='\0';
turn_left();
}
if(data == 'h') // halt
{
data='\0';
stop();
_delay_ms(100);
continue;
}
if(data == 'o') // origin
{
data='w';
stop();
_delay_ms(100);
continue;
}
if(data == 'c') //collect item data -> i when the bot is trying to pick up an rfid
{
data='w';
arm_down();
_delay_ms(4200);
stop_arm();
_delay_ms(2000);
grab();
arm_up();
_delay_ms(4750);
stop_arm();
_delay_ms(2000);
lcd_cursor(2,1);
lcd_string("Collecting");
_delay_ms(3500);
readrfidtag();
clearrfid();
continue;
}
if(data == 'd') //drop_item
{
data='w';
go_down();
release();
go_up();
lcd_cursor(2,1);
lcd_string("Dropping");
_delay_ms(3500);
senddroppedsig();
continue;
}
while(1)
{
if(move_bot() == 1)
{
continue;
}
else
{
break;
}
}
}
}
int main() {
light_start(0);
shut_down_in(119.5);
arm_up();
claw_open();
bin_down();
enable_servos();
collect_poms();
printf("Collected Initial Poms in base\n");
/*
forward(70);
msleep(500);
left(35, 0);
msleep(300);
//face 1st pile
forward(45);
pomPileOne();
*/
forward(50);
msleep(500);
left(28, 0);
//face 1st pile
forward(59); //needs to be exact or else claw will hit table divider
msleep(500);
pomPileOne();
/*
//4/9/16 Test Code
forward(20);
msleep(500);
left(15, 0);
msleep(300);
//face 1st pile
forward(80);
pomPileOne();
*/
//go to pile 2
multforward(45, 0.50);
msleep(100);
forward(15);
msleep(300);
left(85, ks/2);
msleep(300);
backward(2);
//forward(2);
pomPileTwo();
/*
if(green == 1 && red == 1) {
printf("Collected all\n");
//go to bin
right(60, ks/2);
forward(100);
right(100, ks/2);
msleep(500);
bin_mid();
msleep(500);
bin_dump();
}
else {
//do pom pile three only if robot does not have both one red and one green
pomPileThree();
//go to bin
left(60, ks/2);
forward(60);
}
*/
backward(5);
right(80, ks/2);
msleep(300);
forward(130);
msleep(300);
right(50, ks/2); //turn against the wall
msleep(300);
backward(5);
msleep(500);
/*
arm_mid();
msleep(300);
bin_mid();
msleep(500);
bin_dump();
msleep(1000);
*/
right(40, ks/2);
msleep(500);
backward(15);
servo(MAIN_ARM, 1200); //move away arm from box
msleep(300);
bin_mid();
msleep(500);
bin_dump();
msleep(1000);
bin_mid();
msleep(500);
bin_dump();
msleep(1000);
/*
int i = 0;
for(i = 0; i < 3; i++) {
forward(2);
msleep(300);
backward(2);
msleep(300);
}
*/
msleep(1000);
right(30, ks/2);
forward(90);
msleep(2000);
disable_servos();
ao();
return 0;
}
int main() {
//light_start(0);
shut_down_in(119.5);
arm_up();
claw_open();
bin_down();
enable_servos();
collect_poms();
printf("Collected Initial Poms in base\n");
/*
forward(70);
msleep(500);
left(35, 0);
msleep(300);
//face 1st pile
forward(45);
pomPileOne();
*/
forward(47);
msleep(500);
left(30, 0);
//face 1st pile
forward(60); //needs to be exact or else claw will hit table divider
pomPileOne();
/*
//4/9/16 Test Code
forward(20);
msleep(500);
left(15, 0);
msleep(300);
//face 1st pile
forward(80);
pomPileOne();
*/
//go to pile 2
multforward(45, 0.50);
msleep(100);
forward(15);
msleep(300);
left(85, ks/2);
msleep(300);
forward(10);
pomPileTwo();
if(green == 1 && red == 1) {
printf("Collected all\n");
//go to bin
right(60, ks/2);
forward(100);
right(100, ks/2);
msleep(500);
bin_mid();
msleep(500);
bin_dump();
}
else {
//do pom pile three only if robot does not have both one red and one green
pomPileThree();
//go to bin
left(60, ks/2);
forward(60);
}
msleep(2000);
disable_servos();
ao();
return 0;
}
