void part1(){
turn(45);
beep();
driveAtMm(-500); //forward from SB
run_for(1.0, clawGrab); //while moving claw to Grab positio
create_stop();
turn(-90);//turn towards blocks
clawOpen();
driveAtMmFor(-500,.9); //move fast towards first stack for .6 sec
detectBlock(); //then use ET sensor
grabBlocks();
}
void main()
{
create_connect();
enable_servos();
set_each_analog_state(1,1,0,0,0,0,0,0);
initialize();
armPosBlockStack2();
clawStack2();
clawOpen();
create_drive_straight(300);
run_for(3, clawGrab);
create_stop();
clawGrab();
}
int main(){
armUp();
clawOpen();
camera_open(LOW_RES);
double start_wait=seconds();
while((seconds()-start_wait)<=25){//wait for poms or for 25 seconds
int i=0;
while(i<10){//picks latest image from the buffer
camera_update();
i++;
}
if(get_object_count(chan)>0){
break;
}else{
stop(0.1);
}
}
while((seconds()-start_wait)<=60){
int area=get_object_area(chan, 0);
if(area>=600){
int i=0;
while(i<10){
camera_update();
i++;
}
int x=get_object_center(chan, 0).x;
if(x<65){
rightF(0.1, 100, 80);
}else{
leftF(0.1, 100, 80);
}
}else{
break;
}
}
armDown();
clawClose();
armUp();
}
int main()
{
printf("version 1.9.5\n");
shut_down_in(0.010*1000);
int start=seconds();//starting time for two minutes
enable_servo(0);
set_servo_position(0,2047);
straight(1.8, 170);//gets in front of transport
rightC(0.28, 100);
leftC(0.28, -100);
straight(1.65, 170);
leftC(0.44, 100);
printf("completed first dead reckoning\n");
set_analog_pullup(ETport, 0);
//until distance is 5 centimeter
/*
int claw_threshold = 13;
while(analog10(ETport)>claw_threshold){
back(0.1, -10);
msleep(1000);
printf("move once\n");
printf("%i\n", analog10(ETport));
}*/
back(0.25, -20);//pushes against transport
msleep(2000);
set_servo_position(0, 500);//puts claw on transport
msleep(1000);
printf("done with attaching to transport\n");
while(digital(8)==0)//goes forward with transport until it hits the wall
{
straight(0.1, 100);
msleep(10);
}
printf("at wall\n");
back(0.25, -50);//backs up from wall to deposit transport
set_servo_position(0, 2047);//raises claw
msleep(1500);
while(digital(8)==0){//goes forward until it hits wall for second time
straight(0.1, 100);
msleep(10);
}
printf("at wall 2\n");
back(0.35, -100);//goes back and turns left in order to be oriented with left inner wall
leftC(0.6, 142);
int bump_counter=0;
while(digital(8)==0){//hits side wall
straight(0.1, 100);
msleep(10);
}
bump_counter++;//has one bump
int i=0;
while(i<10){//if hits side wall, pause
stop(0.2);
i++;
}
back(0.25, -100);
leftC(0.46, 100);
camera_open(LOW_RES);
armUp();
clawOpen();
double start_wait=seconds();
while((seconds()-start_wait)<=25){//wait for poms or for 25 seconds
int i=0;
while(i<10){//picks latest image from the buffer
camera_update();
i++;
}
if(get_object_count(chan)>0){//if camera sees objects, skip the 25 seconds and go onto picking up stuff
break;
}else{//if camera doesn't see any objects, keep waiting until 25 seconds is up
stop(0.1);
}
}
int x=get_object_center(chan, 0).x;//declares global unchanging variable for the x location of the largest object
while((seconds()-start)<=120){//while there is still time left
int area=get_object_area(chan, 0);//creates local changing variable. this is th area of the largest object camera sees
if(area<=600){//if the object is small enough(far enough), navigate towards object. 600 is threshold
int i=0;
while(i<10){//buffer updating
camera_update();
i++;
}
navigate_poms(x);//similar to line followig function; gets to poms.
}
armDown();
clawClose();
armUp();
/*
leftF(0.5, 100, 80);//turn and go forward until transport is contacted
while(digital(8)==0){
straight(0.1, 100);*/
}
/*
back(1.5, 100);
leftC(0.44, 100);
while(digital(8)==0 && digital(9)==1){
straight(0.1, 100);
msleep(10);
if(digital(9)==0){
rightF(0.1, 100, 40);
left(0.1, 100);
msleep(10);
}
}
straight(2, 200);
rightC(0.44, 100);
straight(1, 80);
rightC(0.92, 100);
while(analog10(ETport<=600)){
msleep(10);
}
msleep(10000);
straight(1, 80);
leftC(0.44, 100);
straight(2, 200);
*/
return 0;
}
void dropoff(){
clawOpen();
armUp();
clawClose();
}
void pickup(){
clawOpen();
armDown();
clawClose();
armUpHalfWay();
}
int main()
{
enable_servos();
create_connect();
set_each_analog_state(1,0,0,0,0,0,0,0);
/*
while(up_button()==0)
{
while(black_button() ==0) {}
clawSB();
armPosSB();
while(black_button() ==0) {}
clawGrab();
clawOpen();
armPosBlockGrab();
while(black_button() ==0) {}
clawClose();
while(black_button() ==0) {}
armPosBlockStack1();
clawStack1();
while(black_button() ==0) {}
clawOpen();
while(black_button() ==0) {}
clawGrab();
armPosBlockGrab();
while(black_button() ==0) {}
clawClose();
while(black_button() ==0) {}
/*set_servo_position(SERVO_ARM_LEFT_PORT, SERVO_ARM_LEFT_STACK2);
set_servo_position(SERVO_ARM_RIGHT_PORT, SERVO_ARM_RIGHT_STACK2);
set_servo_position(SERVO_CLAW_LEFT_PORT, SERVO_CLAW_LEFT_STACK2);
set_servo_position(SERVO_CLAW_RIGHT_PORT, SERVO_CLAW_RIGHT_STACK2);
sleep(2);
armPosBlockStack2();
clawStack2();
while(black_button() ==0) {}
clawOpen();
}*/
//reach first blocks
armPosBlockStack1();
clawStack1();
clawOpen();
detectBlock();
//back up
backFromTouch(0);
//arm to correct pos
armPosBlockGrab();
clawGrab();
//scoot up and grab
scootUp(0);
clawClose();
while(black_button() == 0) {}
driveAtMmFor(200, 2);
//drive with arm a bit up, place first pair of cubes
armPosBlockStack1();
clawStack1();
detectPVC();
set_servo_position(SERVO_ARM_LEFT_PORT, SERVO_ARM_LEFT_GRAB+300);
set_servo_position(SERVO_ARM_RIGHT_PORT, SERVO_ARM_RIGHT_GRAB-300);
set_servo_position(SERVO_CLAW_LEFT_PORT, SERVO_CLAW_LEFT_GRAB+100);
set_servo_position(SERVO_CLAW_RIGHT_PORT, SERVO_CLAW_RIGHT_GRAB-100);
backFromTouch(1);
//correct pos
armPosBlockGrab();
clawGrab();
//scoot up
scootUp(1);
clawOpen();
driveAtMmFor(200, 2);
while(black_button() == 0) {}
clawClose();
//drive with arm a bit up, place second pair on top of first
armPosBlockStack1();
clawStack1();
set_servo_position(SERVO_ARM_LEFT_PORT, SERVO_ARM_LEFT_STACK1+300);
set_servo_position(SERVO_ARM_RIGHT_PORT, SERVO_ARM_RIGHT_STACK1-300);
set_servo_position(SERVO_CLAW_LEFT_PORT, SERVO_CLAW_LEFT_STACK1+200);
set_servo_position(SERVO_CLAW_RIGHT_PORT, SERVO_CLAW_RIGHT_STACK1-200);
detectBlock();
backFromTouch(2);
//correct pos
clawStack1();
armPosBlockStack1();
clawOpen();
driveAtMmFor(200, 2);
armPosBlockGrab();
clawGrab();
while(black_button() == 0) {}
clawClose();
armPosBlockStack2();
clawStack2();
detectPVC();
backFromTouch(3);
clawOpen();
driveAtMmFor(200, 2);
}