int main()
{
camera_open();
ssp(SORT_SERVO,START);
ssp(RED_SERVO,RED_START);
ssp(GREEN_SERVO,GREEN_START);
wait_for_light(0);
start();
shut_down_in(119);
enable_servos();
msleep(5000);
reset();
right(47,0);
forward(35);
right(10,0);
backward(46);
full_sort();
other_side();
ssp(GREEN_SERVO,GREEN_DUMP);
msleep(1000);
ssp(GREEN_SERVO,GREEN_START);
msleep(250);
now();
ao();
disable_servos();
}
int main()
{
wait_for_light(4);
printf("Hello, World!\n");
create_connect();
create_drive_direct(200 , 200);
msleep(2000);
create_spin_CCW ( 100 );
msleep(2000);
create_stop();
create_drive_direct(200 , 200 );
msleep(4000);
create_stop();
create_drive_direct (200 , 200);
msleep(5000);
create_stop();
create_spin_CCW ( 100 );
msleep(2000);
create_drive_direct (200 , 200);
msleep(3000);
create_spin_CCW ( 100 );
msleep(2000);
create_cover();
create_stop();
create_disconnect();
return 0;
}
void Start (void)
{
wait_for_light(LightPort);
shut_down_in (117); //TEST THIS
create_connect();
set_create_total_angle(0);
}
void StartUp() {
enable_servos();
set_servo_position(ARM_PORT, ARM_DOWN);
set_servo_position(CARDBOARD_PORT, CARDBOARD_START);
set_servo_position(2, 800);
msleep(300);
wait_for_light(5);
shut_down_in(119);
}
int main() {
wait_for_light(LIGHT_SENS);
//shake();
//startingT();
start();
shut_down_in(119);
Abyssal_Voyage();
Chomp();
now();
Acquired_Taste();
Devour();
Tougue_Lash();
Acquired_Taste();
//dance();
now();
}
void start(int servo_position)
{
int k;
camera_open();
for (k = 0; k < 10; k = k+1)
{
camera_update();
msleep(200);
}
set_servo_position(1, servo_position);
enable_servos();
msleep(1000);
wait_for_light(7);
shut_down_in(115);
}
// Created on Sat April 13 2013
// This is a transport move test.
int main()
{
wait_for_light(3); // start the robot after the light is turned on
shut_down_in(120); // shutdown the robot in 2 Minutes
enable_servo();
printf("Hello, World!\n");
set_servo_position(1, 0);
motor(1,100); // move the right wheel
motor(3,100); // move the left wheel
msleep(3500); // move for 3.5 seconds
ao();
set_servo_position(1, 2048); // kick bot guy out
motor(1,-100);
motor(3,-100);
disable_servo();
return 0;
}
//start 48500
//down 58000
void main()
{
set_servo_position(Servo_Back,Servo_Back_Up);
set_servo_position(Servo_Left,Servo_Left_Closed);
set_servo_position(Servo_Right,Servo_Right_Closed);
enable_servos();
calibrate();
//wait for light
//printf("wait for light oida");
//set_b_button_text("I am the Twilight");
//while(!b_button()){}
wait_for_light(Sensor_Light);
//shutdown stuff
shut_down_in(115);
start=seconds();
//start position for create
start_position();
//we see everything
camera_open();
//drive in front of the cubes
take_position();
//watch for cubes
cube_is_near();
ao();
//bring the first cube to the pipes
bringback();
//take the 2nd cube and bring them to the pipes
bringback2cube();
}
int main()
{
printf("checklist");
printf("hit black button when ready");
while(black_button());
printf("MAKE SURE CREATE IS ON!!!!!!!!");
while(!black_button());
printf("MAKE SURE ARM IS IN DOWN POSITION!!!!!!!!");
while(!black_button());
printf("MAKE SURE ARM AND CLAW WORK");
while(!black_button());
printf("CHECK POSITION OF CLAW AND ARM");
while(!black_button());
printf("MAKE SURE LIGHT SENSORT IS CALIBRATED");
while(!black_button());
// this starts the robot SECTion 1
wait_for_light(0);
shut_down_in(120.0);
//this shuts the robot down after 120.0 seconds
while(create_connect());
//connects create to cbc
set_servo_position(3, 0);
set_servo_position(0, 1800);
enable_servos();
msleep(500);
create_full();
//section 2
create_drive_direct(SUPER_FAST, SUPER_FAST);
msleep(1646);
//leave beach
create_stop();
msleep(300);
create_drive_direct(SUPER_FAST, -SUPER_FAST);
msleep(435);
//turn towards center
create_stop();
msleep(300);
create_drive_direct(SUPER_FAST, SUPER_FAST);
msleep(1600);
//move towards botguy
create_stop();
msleep(500);
//section 3
set_servo_position(0, 1424);
msleep(1800);
set_servo_position(0, 1100);
msleep(1800);
set_servo_position(0, 924);
msleep(1800);
set_servo_position(0, 724);
msleep(1800);
set_servo_position(3, 1024);
msleep(1800);
set_servo_position(0, 924);
msleep(1800);
set_servo_position(0, 1100);
msleep(1800);
set_servo_position(0, 1200);
msleep(2000);
//retrieve botguy
//section 4
create_drive_direct(-FAST, FAST);
msleep(445);
create_stop();
create_drive_direct(-FAST, -FAST);
msleep(2000);
create_stop();
create_drive_direct(FAST, -FAST);
msleep(445);
create_stop();
create_drive_direct(-FAST, -FAST);
msleep(900);
create_stop();
msleep(1600);
create_stop();
set_servo_position(3, 0);
set_servo_position(0, 1800);
msleep(1200);
set_servo_position(0, 1200);
msleep(1000);
set_servo_position(0, 1800);
msleep(1000);
create_drive_direct(1000, 1000);
msleep(800);
create_drive_direct(-1000, -1000);
msleep(800);
DRIVE(SUPER_FAST);
MSLEEP(800);
TURN(-SUPER_FAST, SUPER_FAST);
MSLEEP(460);
DRIVE(SUPER_FAST);
MSLEEP(1380);
TURN(SUPER_FAST, -SUPER_FAST);
MSLEEP(425);
DRIVE(SUPER_FAST);
SLEEP(1.5);
while(get_create_lbump(0.01) == 0)
{
DRIVE(SUPER_FAST);
}
TURN(-SUPER_FAST, SUPER_FAST);
MSLEEP(435);
TURN(SUPER_FAST, 975);
MSLEEP(500);
while(get_create_rbump(0.01) == 0)
{
TURN(SUPER_FAST, 990);
}
create_stop();
create_disconnect();
}
int main()
{
/* FIRST CALIBRATION: based on poms position adjust starting position
*/
printf("test 3.11, calibration value: \nclaw down : %d, \nright ang forward right: %d, \nright ang forward left : %d, \nback right angle %d ...\n", DOWN_SERVO, RIGHT_ANGLE_CLICKS_MRP_RIGHT, RIGHT_ANGLE_CLICKS_MRP_LEFT, RIGHT_ANGLE_CLICKS_BACK_MRP);
double start_time = seconds();
enable_servos();
clear_motor_position_counter(RIGHT_MOTOR);
clear_motor_position_counter(LEFT_MOTOR);
wait_for_light(1);
shut_down_in(118);
clawUp();
//**************************************
//* PART 1 : get first set of poms to lower storage area
//*
//**************************************
moveBackward(1, NO_DEBUG);
moveForwardHighSpeed(23, DEBUG);
clawDown();
//moveForward(8,NO_DEBUG);
//we have the Poms
rightAngleBwd(RIGHT, NO_DEBUG);
//bump against the upper storage area
moveBackwardHighSpeed(15, NO_DEBUG);
moveForwardHighSpeed(17, NO_DEBUG);
rightAngleBwd(RIGHT, NO_DEBUG);
//bump against upper PVC side
moveBackwardHighSpeed(18, NO_DEBUG);
//move towards dropping poms
moveForward(1.5, NO_DEBUG);
msleep(300);
rightAngleFwd(LEFT, NO_DEBUG);
msleep(300);
clawUp();
//uses IR sensor to stop the forward movement
moveForwardTilBlackLine(15, NO_DEBUG);
printf("====> elapsed time end of part 1: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 1 : get first set of poms to lower storage area
//*
//**************************************
//**************************************
//* PART 2 : get the blue cube to the corner
//*
//**************************************
moveBackwardHighSpeed(25, NO_DEBUG);
//recalibrate against top PVC next to botguy
rightAngleBwd(LEFT, NO_DEBUG);
moveBackwardHighSpeed(11, NO_DEBUG);
// SECOND CALIBRATION: based on blackline/blue position
moveForward(7.5, NO_DEBUG);
rightAngleFwd(RIGHT, NO_DEBUG);
//get the cube now
//====>>>>> may have to be adjust the day of competition based on position of 2nd
//====>>>>> set of poms
moveForwardHighSpeed(15, NO_DEBUG);
//====>>>>>
clawAlmostDownCube();
twentyTwoAngleFwd(LEFT, NO_DEBUG);
clawUp();
//====>>>>> calibration to get to the corner 22.5 fwd, 22.5 more forward, aiming at bottom left hand corner
moveForwardHighSpeed(2.5, NO_DEBUG);
twentyTwoAngleFwd(LEFT, NO_DEBUG);
//clawAlmostDownCube();
moveForwardHighSpeed(30, NO_DEBUG);
//====>>>>>
//moveForwardHighSpeed(2, NO_DEBUG);
//====>>>>>
moveBackwardHighSpeed(9, NO_DEBUG);
fortyFiveAngleFwd(LEFT, NO_DEBUG);
printf("====> elapsed time end of part 2: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 2 : get the blue cube in the corner
//*
//**************************************
//**************************************
//* PART 3 : prepare to get second set of poms
//*
//**************************************
//recalibrate against side PVC
rightAngleBwd(RIGHT, NO_DEBUG);
moveBackwardHighSpeed(25, NO_DEBUG);
moveForward(5, NO_DEBUG);
//calibrating with top PVC
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(23,NO_DEBUG);
// THIRD CALIBRATION: based on second set of Poms
//below is the number of inches from the side PVC
//
moveForwardHighSpeed(10, NO_DEBUG);
rightAngleBwd(RIGHT, NO_DEBUG);
//recalibrate with side pvc
moveBackwardHighSpeed(5,NO_DEBUG);
printf("====> elapsed time end of part 3: %f\n", (seconds() - start_time));
//**************************************
//* END OF PART 3 : prepare to get second set of poms
//*
//**************************************
//**************************************
//* PART 4 : bring second set of poms home
//*
//**************************************
//grabs the poms, bring down claw and go home
moveForwardHighSpeed(12.5,NO_DEBUG);
clawDown();
moveForwardHighSpeed(59.5, NO_DEBUG);
//here is the code to recalibrate one more time... but do we have enough time?
/*//recalibrate against top PVC
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(10, NO_DEBUG);
//move towards dropping poms
moveForwardHighSpeed(10, NO_DEBUG);
//msleep(500);
printf("==> moving righ angle bwd\n");
rightAngleFwd(LEFT, NO_DEBUG);
*/
//recalibrate against top PVC
//moveForwardHighSpeed(41,NO_DEBUG);
//last calibration before dropping second set of poms
rightAngleBwd(LEFT,NO_DEBUG);
moveBackwardHighSpeed(14, NO_DEBUG);
//move towards dropping poms
moveForwardSlow(1.5, NO_DEBUG);
msleep(300);
rightAngleFwd(LEFT, NO_DEBUG);
msleep(300);
clawUp();
moveForwardTilBlackLine(15, DEBUG);
//**************************************
//* END OF PART 4 : bring second set of poms home
//*
//**************************************
printf("====> elapsed time: %f\n", (seconds() - start_time));
ao();
disable_servos();
return 0;
}
int main()
{
double start = 0.0;
// initialize servo, camera, etc here?.
// if you are using a Create you should uncomment the next line
//create_connect();
enable_servos ();
set_servo_position(3, ARM_UP);
//goto GO_DOWN_CAVE;
wait_for_light(2); // change the port number to match where your robot
shut_down_in(119); // shut off the motors and stop the Create after 119 seconds
// note that shut_down_in should immediately follow wait_for_light!!
printf("My program should score more points than this template!\n");
// replace printf above with your code
//create_disconnect();
// Created on Mon March 2 2015
int a1;
int a2;
//turn on robot
motor(0,-700);
motor(2,-700);
msleep(700);
#if 0
//go forward
motor(0,400);
motor(2,400);
msleep(1000);
//turn to aline
motor(0, -100);
motor(2, 100);
msleep(500);
//aline robot
motor(0,-100);
motor(2,-100);
msleep(2000);
go(0, 0);
msleep(1500);
#else
//go forward
motor(0,400);
motor(2,400);
msleep(400);
//turn to aline
motor(0,0);
motor(2,200);
msleep(1300);
//aline robot
motor(0,-100);
motor(2,-100);
msleep(2000);
#endif
// ao(); return 0;
//go forwad
motor(0,85);
motor(2,85); //tweek if wheel goes sideways
//msleep(6000);
printf("before 3000 delay\n");
msleep (1500);
set_servo_position(3, ARM_UP_PEG);
printf("after 3000 delay\n");
msleep(400);
printf("%d\n",analog(1));
start=seconds ();
while ((a1 = analog(1)) <500) {
// printf("%d\n",a1);
//msleep(5);
if(seconds()-start>3.5){
printf("time_out\n");
break;
}
}
printf("Found black %d\n", a1);
//turn to go into middle
turn_right();
//go forward
motor(0,60);
motor(2,60);
printf("go forward\n");
//msleep(2000);
while ((a1 = analog(1)) <600 || (a2 = analog(3)) <600) {
// printf("%4d %4d\n",a1, a2);
//msleep(5);
if (seconds()-start >10.5) {
break;
}
}
printf("%4d %4d\n",a1, a2);
//turn in cave
motor(0,0);
motor(2,0);
turn_right();
set_servo_position(3, ARM_UP);
//go to pick up cubes
//motor(0,75);
//motor(2,0);
printf("turn right\n");
//msleep(1500);
//line follow
start=seconds ();
while(1){
if(seconds()-start>4.9){
break;
}
if (analog(1)>500) {
motor(0,80);
motor(2,100);
}else if (analog(3)>500) {
motor(0,100);
motor(2,80);
}else {
motor(0,100);
motor(2,100);
}
}
//motor(0,85);
//motor(2,85);
//msleep(5500);
//put arm downto grab
GO_DOWN_CAVE:
go(0,0);
set_servo_position(3, ARM_DOWN);
msleep(500);
//begining of picking up poms
go(75,75);
msleep(2500);
go_for(-50,-90, 1000); // back up/turn/pull pom-poms
go_for(90, -90, 500);
go_for(-100, -90, 600);
set_servo_position(3, ARM_UP);
go_for(100, 100, 2000);
go_for(-100, -100,2000);
set_servo_position(3, ARM_DOWN);
msleep(700);
go_for(100,100,1000);
go(-10, -10);
set_servo_position(3, ARM_DOWN);
msleep(100);
set_servo_position(3, ARM_DOWN + 50);
msleep(100);
set_servo_position(3, ARM_DOWN + 100);
msleep(100);
set_servo_position(3, ARM_DOWN + 150);
msleep(100);
set_servo_position(3, ARM_DOWN + 200);
msleep(100);
//set_servo_position(3, ARM_DOWN + 250);
//msleep(100);
//set_servo_position(3, ARM_DOWN + 400);
//msleep(100);
//push the poms over the top of the PVC
go_for(20, 20, 1000);
//StOp
go(0,0);
//wiggle the arm up and down so the poms fall off
set_servo_position(3, ARM_DOWN+350);
msleep(250);
set_servo_position(3, ARM_DOWN+300);
msleep(250);
set_servo_position(3, ARM_DOWN+350);
msleep(250);
// Raise the arm
set_servo_position(3, ARM_UP);
msleep(300);
go_for(0, -100, 1000);
//line follow
start=seconds ();
while(1){
if(seconds()-start>1.5){
break;
}
if (analog(1)>500) {
motor(0,50);
motor(2,80);
}else if (analog(3)>500) {
motor(0,80);
motor(2,50);
}else {
motor(0,80); //why is this 60 and 80
motor(2,60);
}
}
//line follow
start=seconds ();
while(1){
if(seconds()-start>9.1){
break;
}
if (analog(1)>500) {
motor(0,80);
motor(2,100);
}else if (analog(3)>500) {
motor(0,100);
motor(2,80);
}else {
motor(0,100);
motor(2,100);
}
}
set_servo_position(3,ARM_DOWN);
go_for(100, 100, 3500); // Ram into PVC at the end of the cave
#if 1
go_for(-50, -90, 1000); // Back up with poms
go_for(90, -90, 1000);
set_servo_position(3, ARM_DOWNISH);
// TODO: Run back across the cave to the good end
start=seconds ();
while(1){
if(seconds()-start>1.5){
break;
}
if (analog(1)>500) {
motor(0,50);
motor(2,80);
}else if (analog(3)>500) {
motor(0,80);
motor(2,50);
}else {
motor(0,80); //why is this 60 and 80
motor(2,60);
}
}
start=seconds ();
while(1){
if(seconds()-start>8.5){ // change from 8 sec
break;
}
if (analog(1)>500) {
motor(0,80);
motor(2,100);
}else if (analog(3)>500) {
motor(0,100);
motor(2,80);
}else {
motor(0,100);
motor(2,100);
}
}
set_servo_position(3, ARM_DOWN);
go(75,75);
msleep(2500);
go_for(-50,-90, 1000); // back up/turn/pull pom-poms
go_for(90, -90, 500);
go_for(-100, -90, 600);
set_servo_position(3, ARM_UP);
go_for(100, 100, 2000);
go_for(-100, -100,2000);
set_servo_position(3, ARM_DOWN);
msleep(700);
go_for(100,100,1000);
go(-10, -10);
set_servo_position(3, ARM_DOWN);
msleep(100);
set_servo_position(3, ARM_DOWN + 50);
msleep(100);
set_servo_position(3, ARM_DOWN + 100);
msleep(100);
set_servo_position(3, ARM_DOWN + 150);
msleep(100);
set_servo_position(3, ARM_DOWN + 200);
msleep(100);
//set_servo_position(3, ARM_DOWN + 250);
//msleep(100);
//set_servo_position(3, ARM_DOWN + 400);
//msleep(100);
//push the poms over the top of the PVC
go_for(20, 20, 1000);
//StOp
go(0,0);
//wiggle the arm up and down so the poms fall off
set_servo_position(3, ARM_DOWN+350);
msleep(250);
set_servo_position(3, ARM_DOWN+300);
msleep(250);
set_servo_position(3, ARM_DOWN+350);
msleep(250);
//backs up to push poms second timeb
set_servo_position(3, ARM_UP);
go_for(-100, -100,2000);
//push poms second time
go_for(100, 100, 2000);
go_for(-100, -100,2000);
set_servo_position(3, ARM_DOWN);
msleep(700);
go_for(100,100,1000);
go(-10, -10);
set_servo_position(3, ARM_DOWN);
msleep(100);
set_servo_position(3, ARM_DOWN + 50);
msleep(100);
set_servo_position(3, ARM_DOWN + 100);
msleep(100);
set_servo_position(3, ARM_DOWN + 150);
msleep(100);
set_servo_position(3, ARM_DOWN + 200);
msleep(100);
//set_servo_position(3, ARM_DOWN + 250);
//msleep(100);
//set_servo_position(3, ARM_DOWN + 400);
//msleep(100);
//push the poms over the top of the PVC
go_for(20, 20, 1000);
//StOp
go(0,0);
//wiggle the arm up and down so the poms fall off
set_servo_position(3, ARM_DOWN+350);
msleep(250);
set_servo_position(3, ARM_DOWN+300);
msleep(250);
set_servo_position(3, ARM_DOWN+350);
msleep(250);
//motor(0,-75);
//motor(2,-75);
//msleep(8500);
//turn_left();
//go_for(100, 100, 1800);
//turn_left();
//go_for(100, 100, 5000);
#endif
ao();
return 0;
}
int main(int argc, char **argv)
{
//init the node and create a node handle
ros::init(argc, argv, "test_find_grab_ball_client");
ros::NodeHandle n;
//setting up the service clients to use the arm and blob services
blob_client = n.serviceClient<this_basic::GetBlob>("get_blob");
blobs_client = n.serviceClient<this_basic::GetBlobs>("get_blobs");
set_position_client = n.serviceClient<crustcrawler_arm::SetPosition>("set_position");
set_servo_position_client = n.serviceClient<crustcrawler_arm::SetServoPosition>("set_servo_position");
get_servo_position_client = n.serviceClient<crustcrawler_arm::GetServoPosition>("get_servo_position");
set_all_servo_positions_client = n.serviceClient<crustcrawler_arm::SetAllServoPositions>("set_all_servo_positions");
set_new_pose_client = n.serviceClient<this_basic::SetPose>("set_new_pose");
set_new_goal_client = n.serviceClient<this_basic::SetGoal>("set_new_goal");
set_new_state_client = n.serviceClient<this_basic::SetState>("set_new_state");
set_new_vel_client = n.serviceClient<this_basic::SetVel>("set_new_vel");
get_light_client = n.serviceClient<crustcrawler_arm::GetLight>("get_light");
get_pose_client = n.serviceClient<localization_cu::GetPose>("/cu/get_pose_cu");
get_goal_client = n.serviceClient<localization_cu::GetPose>("/cu/get_goal_cu");
ros::Subscriber speeds_subscriber = n.subscribe("speeds_bus",1,speeds_subscriber_callback);
ros::Subscriber hokuyo_laser = n.subscribe("/scan",1,laser_subscriber_callback);
//begin code execution
cmvision::Blob b;
wait_for_light();
std::cout << "saw light!" << std::endl;
/*if(scan_for_color(blue,left))
{
std::cout << "found blob!" << std::endl;
if(go_to_color(blue))
{
std::cout << "at blob!" << std::endl;
}
else std::cout << "lost blob or something" << std::endl;
b = center_on_blob("center");
for(int i = 0;i < 3;i++)
{
b = center_on_blob("center");
grab_blob_special(b);
}
std::cout << "grabbed botguy" << std::endl;
}
else std::cout << "no blob" << std::endl;
/*sap(START_POSITION);
set_new_pose(HOME, 1.57);
wait_for_light();
std::cout << "starting node\n";
START_TIME = ros::Time::now();
int count = 0;
count = 0;
ros::Timer timmer = n.createTimer(ros::Duration(177.),timer_callback);
sleep(START_TIME_OFFSET);
set_new_goal(FIRST_FIELD, 3.14);
set_new_state(2);
set_new_vel(1,0);
sleep(2);
set_new_vel(0,0);
set_new_state(0);
sap(REST_POSITION);
while(!at_goal()) ros::spinOnce();
sleep(3);
count = 0;
while(game_time() < PHASE_ONE_TIME)
{
b = center_on_blob("size");
if(grab_blob(b))
{
score_object();
count++;
}
}
set_new_goal(SECOND_FIELD, 3.14);
sap(REST_POSITION);
while(!at_goal()) ros::spinOnce();
sleep(2);
count = 0;
while(game_time() < PHASE_TWO_TIME)
{
b = center_on_blob("size");
if(grab_blob(b))
{
score_object();
}
}
//set_new_goal(MIDDLE,3.14);
//while(!at_goal()) ros::spinOnce();
//sleep(3);
if(scan_for_water())
{
go_to_color(blue);
count = 0;
while(count < 1)
{
b = center_on_blob("center");
if(grab_blob(b))
{
score_object();
count++;
}
}
}
else
{
set_new_goal(SECOND_FIELD, 3.14);
sap(REST_POSITION);
while(!at_goal()) ros::spinOnce();
sleep(2);
while(game_time() < GAME_DURATION)
{
sap(REST_POSITION);
//ssp(WRIST_FLEX,0);
b = center_on_blob("size");
if(grab_blob(b)) score_object();
set_new_goal(SECOND_FIELD, 3.14);
while(!at_goal()) ros::spinOnce();
}
kill_this();
}
//while(!at_goal()) ros::spinOnce();
//sleep(2);
//while(game_time() < PHASE_THREE_TIME) ros::spinOnce();
//set_new_goal(POSTS,3.14);
//ros::spin();*/
return 0;
}
int main()
{
double start = 0.0;
// initialize servo, camera, etc here?.
// if you are using a Create you should uncomment the next line
//create_connect();
enable_servos ();
set_servo_position(3, ARM_UP);
int a1;
int a2;
//goto START_DOWN_CAVE;
//lights
#if 0
wait_for_light(2); // change the port number to match where your robot
shut_down_in(119); // shut off the motors and stop the Create after 119 seconds
// note that shut_down_in should immediately follow wait_for_light!!
printf("My program should score more points than this template!\n");
// replace printf above with your code
#endif
//create_disconnect();
//go forward to line follow
motor(0,700);
motor(2,700);
msleep(1000);
while ((a1 = analog(1)) <500 && (a2 = analog(3)) <500) {
printf("%5d %5d\n",a1,a2);
//msleep(5);
if(seconds()-start>6.5){
printf("time_out\n");
break;
}
}
printf("Found black %d\n", a1);
//
//line follow
start=seconds ();
while(1){
a1 =analog (1);
a2 =analog (3);
printf("cave%5d %5d\n",a1,a2);
if(seconds()-start>4.5){
break;
}
if ((a1<500 && a2<500) || (a1>500 && a2>500)) {
motor(0,90 * factor);
motor(2,56 * factor);
} else if (a1<500) {
motor(0,90 * 1.00 * factor);
motor(2,75 * 0.80 * factor);
}else {
motor(0,90 * 0.80 * factor);
motor(2,75 * 1.00 * factor);
}
}
return 0;
}
int main()
{
wait_for_light(0);
beep();
}
int main()
{
//waiting for light to start the program.
wait_for_light(0);
shut_down_in(120.0);
//making sure the claw and arm are in position.
set_servo_position(1, 0);//ensures that the arm is in the right position
sleep(0.6);//0.6 is how many seconds the motors are turning on their input values
set_servo_position(0, 624);//ensures that the claw is in the right position
sleep(0.6);//0.6 is how many seconds the motors are turning on their input values
//this section allows the robot to come out of the starting box, turn, drive forward and put itself in position to grab the polyps.
mav(0, -1000);//-600 is velocity
mav(2, -975);//-600 is velocity
sleep(5.0);//7.9 is how many seconds the motors are turning on their input values
mav(0, -500);//-500 is velocity
mav(2, 500);//500 is velocity
sleep(1.6);//7.9 is how many seconds the motors are turning on their input values
mav(0, 900);
mav(2, 900);
sleep(1.8);
mav(2, -800);//-800 is the velocity
mav(0, -800);//-800 is the velocity
sleep(2.0);//1.8 is how many seconds the motors are turning on their input values
mav(0, -300);//-300 is the velocity
mav(2, -100);//-100 is the velocity
sleep(1.9);//1.7 is how many seconds the motors are turning on their input values
ao();//turns motors off
//in this ectionthe arm is lowered the claw closes capturing the polyps and the arm is raised to expose the touch sensor.
set_servo_position(0, 800);//this lowers the claw
sleep(0.6);//0.6 is how many seconds it give the servo to spin to the input position
set_servo_position(0, 900);
sleep(1.8);
set_servo_position(0, 1000);
sleep(1.6);
set_servo_position(0, 1300);
sleep(2.6);
set_servo_position(0, 1700);//this lowers the claw all the way to the board
sleep(1.4);//1.4 is how many seconds it give the servo to spin to the input position
set_servo_position(1, 2048);//this closes the claws with the polyps inside
sleep(0.6);//0.6 is how many seconds it give the servo to spin to the input position
set_servo_position(0, 1324);//this raises the arm to expose the touch sensors
sleep(0.6);//0.6 is how many seconds it give the servo to spin to the input position
//in this section the robot turns toward MPA and drives straight until the touch sensor is triggered by hitting one of the walls of the MPA.
mav(0, -100);
mav(2, -300);
sleep(2.0);
mav(0, 1000);
mav(2, 1000);
sleep(2.8);
mav(0, -200);
mav(2, -200);
sleep(2.6);
mav(0, 500);
mav(2, -500);
sleep(1.0);
set_servo_position(0, 1324);
sleep(0.6);
while(digital(8)==0)
{
mav(2, -900);
mav(0, -700);
sleep(0.2);
if(digital(9)==1)
{
mav(2, -700);
mav(0, -975);
sleep(0.7);
}
}
mav(0, 300);
mav(2, 300);
sleep(1.2);
ao();
set_servo_position(0, 1224);
sleep(0.8);
set_servo_position(1, 0);
sleep(1.4);
set_servo_position(0, 1024);
sleep(0.2);
set_servo_position(0, 1124);
sleep(0.2);
set_servo_position(1, 2048);
sleep(1.4);
set_servo_position(1, 0);
sleep(1.4);
set_servo_position(1, 2048);
sleep(1.4);
set_servo_position(0, 524);
sleep(0.6);
set_servo_position(1, 0);
sleep(1.4);
set_servo_position(0, 1424);
sleep(0.6);
set_servo_position(1, 0);
sleep(1.4);
set_servo_position(0, 624);
sleep(0.6);
set_servo_position(0, 1424);
sleep(0.8);
set_servo_position(0, 624);
sleep(0.8);
mav(0, 300);
mav(2, 0);
sleep(0.6);
mav(0, 500);
mav(2, 500);
sleep(0.8);
mav(0, 500);
mav(2, 500);
sleep(0.1);
mav(0, -500);
mav(2, 500);
sleep(1.8);
mav(0, 500);
mav(2, -500);
sleep(1.8);
mav(0, -500);
mav(2, -500);
sleep(0.1);
mav(0, -500);
mav(2, 500);
sleep(1.6);
mav(0, 1000);
mav(2, 1000);
sleep(1.0);
set_servo_position(0, 1324);
sleep(1.0);
mav(0, 1000);
mav(2, 1000);
sleep(1.8);
mav(0, -500);
mav(2, -500);
sleep(4.6);
mav(0, -500);
mav(2, 500);
sleep(1.7);
mav(0, -500);
mav(2, -500);
sleep(2.0);
mav(0, -500);
mav(2, 500);
sleep(1.6);
//mav(0, 500);
//mav(3, 500);
//sleep(0.8);
mav(0, 300);
mav(2, 300);
sleep(0.2);
ao();
set_servo_position(0, 1024);
sleep(1.4);
set_servo_position(1, 700);
sleep(16.0);
set_servo_position(1, 0);
set_servo_position(0, 1324);
sleep(1.4);
sleep(1.0);
mav(0, 500);
mav(2, 500);
sleep(1.2);
mav(0, 500);
mav(2, -500);
sleep(1.6);
mav(0, -1000);
mav(2, -1000);
sleep(2.3);
mav(0, 500);
mav(2, -500);
sleep(1.4);
while(digital(8)==0)
{
mav(2, -1975);
mav(0, -2000);
sleep(0.5);
}
mav(0, 500);
mav(2, -500);
sleep(1.6);
mav(2, -1975);
mav(0, -2000);
sleep(2.3);
}
