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; }
/// called when program starts task main() { setMotorEncoderUnits(encoderRotations); setMotorSpeed(armMotor, 0);// don't let arm move around // drive to object driveInches(13.0, 50); rotate(-90, true); driveInches(12.0, 50); rotate(90, true); driveInches(11.0, 50); rotate(90, true); driveInches(12.0, 50); rotate(-90, true); driveInches(12.0, 50); rotate(-90, true); driveInches(24.0, 50); rotate(-90, true); driveInches(24.0, 50); rotate(90, true); driveInches(12.0, 50); rotate(90, true); // keep going until we hit the object while(!limitSwitchPressed()) { setMotorSpeed(leftMotor, DRIVE_SPEED); setMotorSpeed(rightMotor, DRIVE_SPEED); } // back up driveInches(-4.0, 50); // pick up object clawClose(false); homeArm(); driveInches(1, 50); clawClose(true); driveInches(-4, 50); setArmDegrees(400); // get to line while(getColorGrayscale(colorSensor) > LINE_THRESHOLD){ setMotorSpeed(leftMotor, DRIVE_SPEED); } while(!limitSwitchPressed()) { // follow line displayString(1, "%d", getColorGrayscale(colorSensor)); if(getColorGrayscale(colorSensor) > LINE_THRESHOLD) { setMotorSpeed(leftMotor, 50); setMotorSpeed(rightMotor, 0); }else { setMotorSpeed(leftMotor, 0); setMotorSpeed(rightMotor, 50); } } // back up and put down object driveInches(-3.5, 50); homeArm(); clawClose(false); setArmDegrees(400); }
void dropoff(){ clawOpen(); armUp(); clawClose(); }
void pickup(){ clawOpen(); armDown(); clawClose(); armUpHalfWay(); }
int main() { //wait_for_light(1);//initialization printf("version 1.9.5\n"); shut_down_in(115); int servo_counter=0; while(servo_counter<=3){ enable_servo(servo_counter); servo_counter++; } // initialize routine set_servo_position(0,2047); armUp(); clawClose(); //get in front of transport straight(1.8, 170); rightC(0.28, 100); leftC(0.28, -100); straight(1.65, 170); leftC(0.44, 100); printf("completed first dead reckoning\n"); //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.15, -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++; } /* ========================== ==========End of routine 1====== KIPR at the wall ========================== */ // Pause here to wait for Create to drop POMS msleep(1*1000); // int start=seconds();// start time printf("will get out of corner\n"); back(0.5, -100); while(seconds()-start<=3){//separates back from right angle stop(0.1); } right_angle(left); while(seconds()-start<=5){//separates right angle from straight stop(0.1); } double start_drive=seconds(); straight(0.7, 100);//go to poms stop(0.5); camera_open(LOW_RES); /*while(seconds()-start<=20){ int update_counter=0; while(update_counter<10){ camera_update(); update_counter++; } if(get_object_count(chan)==0){ stop(0.1); } int x=get_object_center(chan, 0).x; int area=get_object_area(chan, 0); if(area<=400){ navigate(x); } if(area>=800){ //if objects are clumped are clumped, area will be automatically larger. //this means that KIPR will start earlier than it should. We counter this here //straight(0.18, 100); straight(0.1, 100); break; } }*/ stop(0.1); printf("object seen\n"); double end_drive=seconds();//only for simulation. real end and start times would have to be taken from whole routine //pick up POMS that are right there pickup(); int offset=(end_drive-start_drive)*(3.0/4.0);;//half of distance from back of launch area to border of launch area back((end_drive-start_drive)+1.5, -100);//go to the back of wall plus a little more to straighten out printf("at back wall\n"); straight(2.0, 100);//goes halway between border and back of launch area right_angle(left);//faces transport int starpof=seconds();//time after right angle while(seconds()-starpof<=2){ motor(motorL, 0); motor(motorR, 0); } while(digital(8)==0){ straight(0.1, 100); } straight(0.1, 100); printf("touching transport\n"); motor(motorL, 0); motor(motorR, 0); dropoff();//drops poms in launch area printf("poms in transport\n"); back(0.25, -100); int starfop=seconds(); while(seconds()-starfop<=2){ motor(motorL, 0); motor(motorR, 0); } int turns=1; while(turns<=2){ right_angle(right); stop(0.3); turns++; } /* ====================== ======Part 2 of routine 2==== ====================== */ while(1){ stop(0.5);// while(digital(8)==0){ straight(0.1, 100); } back(0.3, -100); right_angle(left); pickup(); back((end_drive-start_drive)+1.5, -100); straight(2.2, 100); right_angle(left); while(digital(8)==0){ straight(0.1, 100); } dropoff(); back(0.5, -100); right_angle(left); right_angle(left); } /*while(1){ start=0; while(digital(8)==0){ straight(0.1, 80); } back(0.5, -100); right_angle(right); straight(0.5, 100); right_angle(right); right_angle(right); while(seconds()-start<=20){ int update_counter=0; while(update_counter<10){ camera_update(); update_counter++; } if(get_object_count(chan)==0){ stop(0.1); } int x=get_object_center(chan, 0).x; int area=get_object_area(chan, 0); if(area<=400){ navigate(x); } if(area>=800){ //if objects are clumped are clumped, area will be automatically larger. //this means that KIPR will start earlier than it should. We counter this here //straight(0.18, 100); straight(0.1, 100); break; } } int servo_counter=1; while(servo_counter<=3){ enable_servo(servo_counter); printf("servo %d enabled\n", servo_counter); servo_counter++; } pickup(); int offset=(end_drive-start_drive)*(3.0/4.0);;//half of distance from back of launch area to border of launch area back((end_drive-start_drive)+1.5, -100);//go to the back of wall plus a little more to straighten out printf("at back wall\n"); straight(1.8, 100);//goes halway between border and back of launch area right_angle(left);//faces transport int starpof=seconds();//time after right angle while(seconds()-starpof<=2){ motor(motorL, 0); motor(motorR, 0); } while(digital(8)==0){ straight(0.1, 100); } straight(0.1, 100); printf("touching transport\n"); motor(motorL, 0); motor(motorR, 0); dropoff();//drops poms in launch area printf("poms in transport\n"); back(0.25, -100); int starfop=seconds(); while(seconds()-starfop<=2){ motor(motorL, 0); motor(motorR, 0); } int turns=1; while(turns<=2){ right_angle(right); turns++; } repeat_runs++; }*/ while(1){ int servo_counter=0;//enables servos while(servo_counter<=3){ enable_servo(servo_counter); servo_counter++; } armUp();//raises arm at start stop(0.5); while(digital(TOUCH_SENSOR)==0){//go forward until left wall is contacted straight(0.1, 100); } stop(0.1); back(0.3, -100);//back up stop(0.1); right_angle2(left);//face poms stop(0.1); back(3+1.5, -100);//back up to rocket wall to straighten out stop(0.1); straight(3+1.5, 100);//go to poms stop(0.1); pickup();//pick poms up stop(0.1); back(3+1.5, -100);// back up until rocket wall stop(0.1); straight(2.2, 100);//go forward stop(0.1); right_angle2(left);//turn to face transport stop(0.1); while(digital(TOUCH_SENSOR)==0){//go forward until transport contacted straight(0.1, 100); } stop(0.1); dropoff();//drop poms off stop(0.1); back(0.5, -100);//back up from transpor stop(0.1); right_angle2(right);//orient for next iteration stop(0.1); right_angle2(right); } return 0; }
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); }