void egg_box_task(robot_state &state) { std::cout << "entered egg box task\n"; // align to box move(state, -1, 0); delay(1300); move(state, 0, -1); delay(500); move(state, 1, 0); delay(500); // eject the shells open_claw(state); flap_flapper(state); unflap_flapper(state); close_claw(state); state.have_egg = false; update_status_leds(state); // realign to track move(state, -1, 0); delay(500); move(state, 0, 1); wait_for_crossing(state); move(state, 1, 0); delay(1000); move(state, 1, 1); wait_for_crossing(state); move(state, 0, -1); delay(100); state.eggs_processed++; state.map->vs[VERT_ABOVE_EGGS]->ignore_junction = false; set_next_target(state); std::cout << "egg box task complete\n"; }
void egg_task(robot_state &state) { // Align with podium // due to length of robot, need to use the "other" end of the line // turn west, follow this line until no line detected. Keep going a bit further. // turn 180* until line detected again. Follow line back up to egg junction // Go through junction, and now use edge following to slowly approach the podium // (edge following has less slack than line following) // Edge follow until one whisker hits the wall, then drive the opposite motor until both // whiskers are on wall std::cout << "entered egg task (" << state.eggs_processed << " eggs processed so far)\n"; if (state.eggs_processed >= 1 && state.eggs_processed <= 3) { std::cout << "Turning to west\n"; move(state, -1, -0.5); delay(600); move(state, 1, -1); wait_for_crossing(state); move(state, 1, 0); delay(300); move(state, 0.5, 1); std::cout << "Turning east\n"; wait_for_crossing(state); move(state, 0, -1); delay(100); std::cout << "Alignment reached.\n"; } open_claw(state); state.integral = 0.f; while (!(state.bump_left || state.bump_right)) { update_sensor_values(state); follow_line_ignore_junctions(state); } std::cout << " Bumped into wall: " << (state.bump_left ? "l" : "-") << (state.bump_right ? "r" : "-") << "\n"; move(state, 0, 0); // Close claw, back away close_claw(state); delay(500); move(state, -1, 0); while (state.line_state != LINE_JUNCTION) update_sensor_values(state); delay(600); move(state, 0, 0); state.have_egg = true; update_status_leds(state); // if claw can't close, egg is fake. can skip to realignment step. if (state.claw_closed) { // Split claw (dropping contents into bucket) // Use LDR to identify contents. // set have_chick or have_white accordingly widen_claw(state); delay(1000); for (int i = 0; i < 3; ++i) { narrow_claw(state); delay(200); widen_claw(state); delay(300); } move(state, 0, 0); read_ldr(state); if (state.light_sensor > 0.2f) state.have_white = true; else state.have_chick = true; std::cout << (state.have_white ? "Egg white " : "Chick ") << "detected.\n"; update_status_leds(state); } std::cout << "Egg picked up, realigning to track\n"; // realign to track // (may possibly have to set current vertex to egg_n+1 due to turning during realignment) move(state, 1, 1); wait_for_crossing(state); delay(100); std::cout << "On line, straightening...\n"; move(state, 1, 0); while (state.line_state != LINE_STRAIGHT) { update_sensor_values(state); } /*move(state, 1, 0.5); delay(500); do { update_sensor_values(state); } while (state.line_state < -1 || state.line_state > 1); move(state, 0, 0); state.integral = 0.f;*/ /*float time = state.watch.read(); while (state.watch.read() < time + 3000.f) { update_sensor_values(state); follow_edge(state, 3, false); }*/ narrow_claw(state); advance_current_egg_position(state); state.map->vs[VERT_ABOVE_EGGS]->ignore_junction = true; set_next_target(state); std::cout << "Egg task complete\n"; }
int main() { struct cbcRobot robot = {0,3,150,55,1100}; //Define the robot. 0 and 3 are the motor ports, //150 mm is the wheel distance //55 is wheel diameter, 1100 is ticks per rotation light_it_up(4); //Wait for the light to turn on set_servo_position(arm_servo, top_pos); //Set the arm servo to the top position shut_down_in(118.0); //Shut down in 118 seconds //msleep(5000); //Wait 5 seconds float initial_time=seconds(); //Create a variable that records the initial time cbc_align(400); //Align with both of the top hats on the black line ao(); cbc_align_white(300); lower_arm(600); //Lower claw and open arm open_claw(600); cbc_straight(140, 800); //Straight out to get away from pvc wait_for_cbc(); cbc_arc_left(160, 135, 100); //Get past pvc to lower arm and open claw wait_for_cbc(); cbc_straight(180, 800); //Go straight into tribbles wait_for_cbc(); close_claw(1000); //Close claw on tribbles cbc_straight(280, 800); wait_for_cbc(); raise_arm(1200); //Raise the arm msleep(800); cbc_arc_right(180, 34, 80); //Arc right to get into position to approach the lattice wall wait_for_cbc(); cbc_touch(700, 5000); //Bump into lattice wall msleep(300); cbc_straight(-100, 600); //Back up 10 cm from wall wait_for_cbc(); open_claw(800); //Open claw to release tribbles and turn 94 degrees right cbc_spin(-94, 500); wait_for_cbc(); cbc_straight(120, 700); //Go forward into the turnstile and lower arm to mid position arm_mid(2000); msleep(1200); cbc_spin(20, 500); //Turn 20 degrees left to hit the turnstile out of the way wait_for_cbc(); msleep(200); cbc_spin(-16, 700); //Turn back 16 degrees to the right and close the claw close_claw(800); wait_for_cbc(); cbc_straight(-280, 800); //Go backwards to get into position to grab the tribbles wait_for_cbc(); lower_arm(1000); //Lower the arm and open the claw msleep(800); open_claw(800); msleep(1000); cbc_spin(8,700); //Turn 8 degrees to get ready to grab the tribbles wait_for_cbc(); cbc_arc_left(1500, 6, 100); //Go nearly straight into the tribbles wait_for_cbc(); close_claw(200); //Go forward and grab the tribbles cbc_straight(200, 600); wait_for_cbc(); close_claw(600); msleep(800); cbc_straight(-60, 600); //Back up to clear the turnstile raise_arm(1000); wait_for_cbc(); cbc_spin(4,700); //Turn slightly left wait_for_cbc(); follow_tape_left(600); //Run into the turnstile for horizonal alignment cbc_straight(-100, 400); //Back up before crossing over to the other side wait_for_cbc(); cbc_spin(90, 500); //Turn 90 degrees to the left wait_for_cbc(); cbc_straight(-80, 600); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); cbc_straight(-50, 300); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); cbc_straight(540,800); //Go forward to the other side wait_for_cbc(); open_claw(1000); //Open the claw to release tribbles and arc right approximately 100 degrees mrp(robot.leftWheel,780,2340); mrp(robot.rightWheel,400,1200); bmd(robot.leftWheel); bmd(robot.rightWheel); msleep(200L); lower_arm(1400);//Go forward and lower the arm msleep(500L); cbc_straight(370, 800); wait_for_cbc(); close_claw(1000); //Go forward and close the claw cbc_straight(300,800); wait_for_cbc(); raise_arm(1500); //Raise the arm msleep(800); cbc_spin(-73,500); //Turn 73 degrees right wait_for_cbc(); open_claw(1000); //Run into the lattice wall and open the claw to release the tribbles cbc_touch(500, 5000); cbc_straight(-80, 600); //Back up 8 cm from wall wait_for_cbc(); cbc_spin(-92, 500); //Turn 92 degrees right and close the claw close_claw(1000); wait_for_cbc(); cbc_straight(-80, 600); //Go backwards to get into position to grab the tribbles wait_for_cbc(); lower_arm(1500); //Lower the arm and open the claw msleep(800); open_claw(800); msleep(1000); cbc_spin(4,700); //Turn a bit in order to put the robot in an optimal tribble gathering position wait_for_cbc(); cbc_arc_left(1500, 6, 100); //Go nearly straight into the tribbles wait_for_cbc(); close_claw(200); //Go forward and grab tribbles cbc_straight(180, 600); wait_for_cbc(); close_claw(600); msleep(800); cbc_straight(-80, 400); //Back up to clear the turnstile wait_for_cbc(); raise_arm(1200); //Raise the arm msleep(500); cbc_straight(220, 700); //Go forward into the turnstile wait_for_cbc(); cbc_spin(80, 500); //Turn 80 degrees to the left wait_for_cbc(); cbc_straight(-80, 600); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); cbc_straight(-50, 300); //Back up and align with the black tape wait_for_cbc(); cbc_align(400); ao(); //cbc_straight(1080, 800); //Old go forward function //wait_for_cbc(); mrp(robot.rightWheel,825,7700); //Go forward and correct the non-straightness of the motors mrp(robot.leftWheel,800,7700); wait_for_cbc(); cbc_touch(500, 5000); //Run into the wall msleep(300); cbc_straight(-125, 600); //Back up 12.5 cm from wall wait_for_cbc(); cbc_spin(90, 500); //Turn 90 degrees to the left wait_for_cbc(); while(analog10(et_right)<850) //While the tape is not seen by the right top hat { mav(robot.rightWheel,615); //Go forward mav(robot.leftWheel,600); msleep(25L); } off(robot.rightWheel); //Turn the motors off off(robot.leftWheel); cbc_straight(240, 700); //Go forward to get under the injection chute wait_for_cbc(); msleep(8000); //Wait for the injection chute tribbles to be loaded up cbc_straight(250, 700); //Go forward into the wall wait_for_cbc(); cbc_straight(-40, 600); //Back up 4 cm from pvc wait_for_cbc(); arm_mid(1000); //Lower the arm to the mid position and open the claw msleep(800); open_claw(800); msleep(800); cbc_straight(-40, 600); //Back up 4 cm from pvc wait_for_cbc(); close_claw(1000); //Turn around 180 degrees and close the claw cbc_spin(180,600); wait_for_cbc(); cbc_straight(-80, 480); //Back up to wall wait_for_cbc(); lower_arm(1400); //Lower the arm msleep(800L); mrp(dump_mot, 200, 500); //Dump the tribble into the MPA bmd(dump_mot); msleep(1000); mrp(dump_mot, 200, -500); //Dump the tribble into the MPA bmd(dump_mot); mrp(dump_mot, 200, 500); //Dump the tribble into the MPA bmd(dump_mot); mrp(dump_mot, 200, -500); //Dump the tribble into the MPA bmd(dump_mot); float time = seconds() - initial_time; //Create a variable that represents the time taken for the robot to complete its program cbc_display_clear(); //Clear the CBC display and then print the time the robot took to complete its program printf("Time: %f",time); return 0; }