void drive_random()
{
srand ( time(NULL) );
int direction=rand()%361-180; //-180 to +180
if(direction<0)
{
set_create_total_angle(0);
create_drive_direct(-200,200);
while(get_create_total_angle(0.1)>direction);
}
else
{
set_create_total_angle(0);
create_drive_direct(200,-200);
while(get_create_total_angle(0.1)<direction);
}
int i=0;
while(i<1000)
{
if(get_create_lbump(0.1)||get_create_rbump(0.1)) //if bumper pressed
{
i=2000;
}
else
{
create_drive_direct(300,300);
}
i++;
msleep(1);
}
create_stop();
}
void line_follow()
{
int ldrive = 100;
int hdrive = 300;
int mdrive = 200;
if (LFCLIFF >= T && RFCLIFF <= T)
{
create_drive_direct(hdrive , ldrive);
}
else if (LFCLIFF <= T && RFCLIFF >= T)
{
create_drive_direct(ldrive , hdrive);
}
else if (LFCLIFF >= T && RFCLIFF >= T)
{
create_drive_direct(hdrive , hdrive);
}
else if (LFCLIFF <= T && RFCLIFF <= T)
{
create_drive_direct(mdrive , mdrive);
}
else if (LCLIFF >= T)
{
create_stop();
turn_while(250 , 77 , 1);
}
else if (RCLIFF >= T)
{
create_stop();
turn_while(250 , 77 , -1);
}
}
/* This program will move Robot A accordig to its assignment. */
int main()
{
printf("Hello, World!\n");
create_connect();
int rport=1; // identify ports used, might need to change
int threshold=500; // light threshold in the room
int high=100, low=-10; //motor power for arc. Will need to change to perfect drive.
while (b_button()==0) {} // start after pushing B
/* To be replaced with wait_for_light(port#); so that light starts the robot. Right now it is started with B*/
while (a_button()==0){ // stop after pushing A
/* to be relplaced with a button sensor. Such as digital(Port #); so that the claw grabs the transport when the sensor(s) are pushed.*/
while (analog10(rport) > threshold) { // continue until not dark
create_drive_direct(-10,100); // arc left
if (a_button()!=0) break; } // or button pressed
// to be replaced with a sensor
while (analog10(rport) <= threshold){ // continue until dark
create_drive_direct(100,-10); // arc right
if (a_button()!=0) break; } // or button pressed
// to be replaced with a sensor
create_stop();
//all stop because button was pushed.
}
create_disconnect();
return 0;
}
//This function blocks and does a pretty accurate spin. Note that the function will
//not return until the spin is complete
//CAUTION, requesting the robot to spin more than about 3600 degrees may never terminate
//the 9/07 firmware update is required for this works. Returns -1 if error
int create_spin_block(int speed, int angle)
{
char buffer[7];
char *bptr = buffer;
int r[7]={18,7,9,24,12,15,21},v[7],i,b;
long lenc=0L, renc=0L,slenc,srenc,flenc,frenc;
float offset, ticsPerMM=7.8324283, rad=129.0, pi=3.1415926;
CREATE_BUSY;
create_write_byte(8);//version
CREATE_WAITFORBUFFER(buffer,7,-99)
for(i=0;i<7;i++){
v[i]=(*(bptr++));
}
CREATE_FREE;
if(!(v[1]==r[1] && v[2]==r[2] && v[3]==r[3])){
printf("create_spin_block\n requires Create Firmware dated\n 9/24/2007\n");
printf("Yours is:%d/%d/%d\nFunction aborted\n", v[2], v[3], v[1]+2000);
return(-1);
}
_create_get_raw_encoders(&slenc, &srenc);
offset=(float)angle*pi*rad*ticsPerMM/180.0;
if(angle>0){create_drive_direct(speed,-speed);}
else {offset=-offset; create_drive_direct(-speed,speed);}
flenc=slenc+(long)offset;
frenc=srenc+(long)offset;
if(frenc > 65535L && flenc > 65000L){
while(lenc >= slenc)_create_get_raw_encoders(&lenc, &renc);
while(lenc < flenc-65536L)_create_get_raw_encoders(&lenc, &renc);
create_stop();}
else{
while(lenc < flenc && renc < frenc)_create_get_raw_encoders(&lenc, &renc);
create_stop();
}
return(0);
}
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 jiggle_create(void) {
while (true) {
create_drive_direct(180, -180);
msleep(5);
create_drive_direct(-180, 180);
msleep(5);
}
}
int main()
{
create_connect();
create_drive_direct(-500 , -500);
msleep(2000);
create_drive_direct(300 , -300);
msleep(500);
create_disconnect();
}
int main()
{
create_connect();
create_drive_direct(100,100);
wait_for_milliseconds(2000);
create_drive_direct(-200, 200);
wait_for_milliseconds(5000);
create_stop();
create_disconnect();
return 0;
}
/*goes forward for distance*/
void forward_for(int distance, int speed) {
set_create_distance(0);
while (get_create_distance() < distance ) {
create_drive_direct(speed,speed);
printf("value is %d", get_create_distance());
msleep(100);
}
create_drive_direct(0,0);
}
void track(int color)
{
track_update();
if(track_size(color,0) > TRACKSIZETHRESHOLD) //if object with color is visible
{
create_drive_direct(MAXSPEED+(track_x(color,0)-80)*TURNSPEED,MAXSPEED-(track_x(color,0)-80)*TURNSPEED);
}
else //if object with color is not visible
{
create_drive_direct(200,-200);
}
}
int main()
{
create_connect();
while(1)
{
create_drive_direct(200, 300);
if(get_create_rbump(0.01) ==1)
{
create_drive_direct(300, 200);
msleep(800);
}
}
create_stop();
create_disconnect();
}
void create_lineup(){//lines up the create on a black line
int done = 0;
float tstart = seconds();
int retry = 1;//don't retry ever
while(done < 4){//WORK ON THIS
msleep(5);
int lspd,rspd;
lspd = rspd = 0;
if (lcliff > 800) lspd = 20;
if (lcliff < 500) lspd = -20;
if (rcliff > 800) rspd = 20;
if (rcliff < 500) rspd = -20;
if (seconds()-tstart > 4){lspd/=2;rspd/=2;}
if (seconds()-tstart > 6){
create_stop();
return;//failure, timeout
}
//printf("\n%6d%6d",lcliff,rcliff);
create_drive_direct(lspd,rspd);
if (lspd == rspd && lspd == 0)
done++;
else
done=0;
}
create_stop();
}
int main()
{
create_connect();
create_drive_straight(500);
sleep(2);
create_drive_direct(500, -500);
msleep(440);
create_drive_straight(500);
msleep(4200);
create_drive_direct(-500, 500);
msleep(375);
create_drive_straight(500);
msleep(1500);
create_stop();
create_disconnect();
}
/*
* Class: cbccore_low_Create
* Method: create_drive_direct
* Signature: (II)V
*/
JNIEXPORT void JNICALL Java_cbccore_low_Create_create_1drive_1direct(JNIEnv *env, jobject obj, jint r_speed, jint l_speed)
{
#ifdef CBC
create_drive_direct(r_speed, l_speed);
#else
printf("Java_cbccore_low_Create_create_1drive_1direct stub\n");
#endif
}
int main()
{
create_connect();
create_full();
while(digital(15) == 0);
create_drive_direct(-1500, -1500);
while(!(a_button_clicked()));
return 0;
}
//This function blocks and does a pretty accurate spin. Note that the function will
//not return until the spin is complete
//CAUTION, requesting the robot to spin more than about 3600 degrees may never terminate
//the 9/07 firmware update is required for this works.
int create_spin_block(int speed, int angle)
{
int i,b;
long lenc=0L, renc=0L,slenc,srenc,flenc,frenc;
float offset, ticsPerMM=7.8324283, rad=129.0, pi=3.1415926;
slenc=robot.lenc;
srenc=robot.renc;
offset=(float)angle*pi*rad/180.0;
if(angle>0){create_drive_direct(speed,-speed);}
else {offset=-offset; create_drive_direct(-speed,speed);}
flenc=slenc+(long)offset;
frenc=srenc+(long)offset;
while(lenc < flenc && renc < frenc){
lenc=robot.lenc;
renc=robot.renc;
}
create_stop();
}
int main()
{
printf("starting connor\n");
enable_servo(1);
set_servo_position(1,100);
//disable_servo(1)
create_connect();
while (! digital(11))
{
printf("%d\n", digital(11));
create_drive_direct(-150,-150);
}
printf("starting connor\n");
set_servo_position(1,1400);
while (get_create_lbump() == 0 && get_create_rbump() == 0)
{
create_drive_direct(150, 150); // drive straight at 150mm/s
}
disable_servos();
}
// script to move with individual motor control
// dist mm with r_speed mm/sec on right wheel and l_speed on left
void create_script_move_direct(int dist, int r_speed, int l_speed) {
int sdist, ddist;
create_drive_direct(r_speed, l_speed);
create_distance();
sdist=gc_distance;
ddist=sdist+dist;
if(dist>0)
while(gc_distance<ddist)create_distance();
else
while(gc_distance>ddist)create_distance();
create_stop();
}
int main()
{
printf("Tryying to connect to the Create\nBattery: %d...\n", get_create_battery_capacity());
create_connect();
printf("Connected...");
while (get_create_lbump() == 0 && get_create_rbump() == 0)
{
create_drive_direct(150,150);
}
create_disconnect();
printf("All done");
return 0;
}
void wait_for_kill(void) {
while (!side_button()) {}
alloff();
motor(0, 0);
motor(1, 0);
motor(2, 0);
motor(3, 0);
alloff();
disable_servos();
create_drive_direct(0, 0);
create_stop();
create_disconnect();
exit(0);
}
int main()
{
printf("Hello Create\n");
printf("press R button to start\n");
while(digital(13) ==0) {
msleep(20);
}
printf("Connecting\n");
create_connect(); // this engages the create
while(get_create_lbump()==0 && get_create_rbump()==0) {
create_drive_direct(100,-100);
}
create_stop();
create_disconnect();
msleep(500);
return 0;
}
/**
* move the iCreate, stop at a certain distance moved.
*/
int move(int left, int right, int distance)
{
int distance0 = 0;
int loop_done = 0;
distance0 = get_create_distance (.05);
create_drive_direct (left, right);
while (! loop_done)
{
printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
if (abs (distance0 - get_create_distance (.05)) > distance)
{
loop_done = 1;
}
msleep(50);
}
}
/**
* move the iCreate, stop at a certain angle
*/
int spin (int left, int right,int angle)
{
int angle0 = 0;
int loop_done = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (left, right);
while (! loop_done)
{
printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
if (abs ( angle0 - get_create_total_angle (.05)) > angle)
{
loop_done = 1;
create_stop ();
}
msleep(50);
}
}
/*Turning to the right*/
void turn_right(int time) {
int i = 0;
while(i++<=time) {
create_drive_direct(-100, -80);
}
}
/*Turning to the left*/
void turn_left(int time) {
int i = 0;
while(i++<=time) {
create_drive_direct(-80,-100);
}
}
void main ()
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle0 = 0;
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
create_connect ();
clear_motor_position_counter (0);
angle0 = get_create_total_angle (.1);
create_drive_direct (150,-150);
move_to_position (0, -900, -1550);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.1), task_A_done);
if (angle0 - get_create_total_angle (.1) < 90)
{
create_drive_direct (150,-150);
}
else
{
task_A_done = 1;
}
if (get_motor_done (0))
{
}
else
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(0.1);
}
create_stop ();
create_disconnect ();
//put specific commands to control robot in here
// loop_done = 0;
// task_A_done = 0;
// task_B_done = 0;
// while(! loop_done)
// {
// if (/*task A is not done*/)
// {
// //continue doing task A
// }
// else
// {
// task_A_done = 1;
// }
// if (/*task B is not done*/)
// {
// //continue doing task B
// }
// else
// {
// task_B_done = 1;
// }
// if (task_A_done && task_B_done)
// {
// loop_done = 1;
// }
// }
}
void main (int argc, char ** argv)
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle = 0;
int dangle = 0;
int angle0 = 0;
int distance0 = 0;
int distance = 0 ;
int ddistance = 0;
// float lf_args[20];
// read inputs
// printf("Args: %d\n", argc);
// for (i = 2; i < argc; i++)
// {
// scanf("%lf%, argv[i]
// }
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
//to start by light, do:
//while(analog(1) > 200)
if (0)
{
//we have seen the light
msleep(100);
if(analog(1) < 800){
shut_down_in(119);
}
}
create_connect ();
clear_motor_position_counter (0);
enable_servo (0);
set_servo_position (0 ,1300);
//set_servo_position (1800);
// ------------------------------------------------------------------------
//step:1 aim the arm to knock over the box
//raise arm
printf("-----Step 1-----\n");
move_motor (0, -900, -1200);
// swing arm to hit the box, then get back to position
spin (50, -50, 20);
spin (-50, 50, 20);
create_stop ();
// ------------------------------------------------------------------------
//step:2 knock over box and turn
//lift arm
//turn 90
printf("-----Step 2-----\n");
move_to_position (0, -900, -2600);
msleep (900);
//turn create in a circular position and raise arm so it doesn't hit anything
angle0 = get_create_total_angle (.05);
angle = angle0;
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
create_drive_direct (-178,-648);
while(! loop_done)
{
if (dangle >= 155)
{
task_A_done = 1;
create_stop ();
}
else if (dangle >= 125)
{
create_drive_direct (-120, -181);
}
if (get_motor_done (0))
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(100);
angle = get_create_total_angle (.05);
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
}
create_stop ();
// ------------------------------------------------------------------------
//step:4 aim claw to point of botguy
//turn 20
//lower arm
printf("-----Step 4-----\n");
//--tweaking position so it gets in just the right position
move_to_position (0, 400, -900);
move (20, 20, 50);
set_servo_position (0 ,400);
create_drive_straight (-30);
move_motor (0, 600, -300);
create_drive_straight (-38);// at this point, botguy is in his palm
//can't remember function to program
msleep (3000);
loop_done = 0;
if (! loop_done)
{
create_stop ();
if (! digital (10))
{
move_motor (0, 400, -900);
spin (20, -20, 5);
move_motor (0, 600, -300);
create_stop ();
loop_done = 1;
}
else
{
loop_done = 1;
}
}
// ------------------------------------------------------------------------
//step:5 grab botguy and lift him up (not complete)
printf("-----Step 5-----\n");
move (-10, -10, 10);
msleep (1300);
//spin (60, -60, 5);
//close arm and grab botguy
set_servo_position (0, 1510);
msleep (400);
move_to_position (0, -1000, -3000);
//wait for him to finish
msleep (5000);
// ------------------------------------------------------------------------
//step:6 move backwards till it's in position
printf("-----Step 6-----\n");
//back up in an arc
move (130, 280, 780);
//move towards the center and lower drop height
move_to_position (0, -900, -500);
create_drive_straight (-20);
msleep (3000);
//drop botguy in transport
set_servo_position (0, 200);
create_stop ();
//start moving towards the pipe
move_motor (0, 500, -1200);
spin (50, -50, 70);
move (50, 50, 100);
spin (-50, 50, 70);
/*
if (! loop_done)
{
if (get_object_count(2) == 1)
{
create_stop ();
spin(-10, 10, 5);
create_stop ();
move(20, 20, 20);
create_stop ();
set_servo_position (0, 1400);
loop_done = 1;
}
}
loop_done = 0;
spin (10, 10, 5);
set_servo_position (0, 200);
*/
/*
spin (50, -50, 85);
move_motor (0, 400, -400);
move (-100, -100, 50);
loop_done = 0;
//close arm on pipe
set_servo_position (0, 1510);
//turn arm to face the pole
spin (-50, 50, 50);
//up, forward, then drop pipe
move_motor (0, 400, -2300);
move (-100, -100, 100);
set_servo_position (0, 200);
ao ();
*/
create_disconnect ();
}
//main stuff haha
int main()
{
int claw = 1; //claw port
int arm = 0; //arm port
create_connect();
printf("create connected");
set_create_total_angle(0);
//create setup
turn_for(10, -200); //Turns the create around
//does some important stuff
create_drive_direct(-100, -100);
set_servo_position(claw, 1561);
enable_servos();
msleep(300);
set_servo_position(arm, 1850); //arm down
msleep(500);
set_servo_position(claw, 1000);
msleep(500);
set_servo_position(arm, 1643); //grabs ball
msleep(500);
turn_for(36, 200); //spins onto first black path to line follow later
msleep(500);
forward_for(50, -100); //moves forward onto the black line
line_follow(blackThresh , 212); //starts following the black line
set_servo_position(claw, 1561); //drops ball
//dropped ball already
msleep(400);
forward_for(500, 100); //back it up back it up
set_servo_position(arm, 245); //holds the arm up
msleep(100);
set_create_distance(0);
set_create_total_angle(0);
while (get_create_total_angle() > -10) { //spins itself onto the angle of other black line
create_spin_CW(200);
}
create_spin_CW(0); //stops spinning
msleep(1500);
create_drive_direct(-200, -200); //goes onto the black line to line follow later
msleep(1000);
create_drive_direct(0,0); //stops itself
msleep(500);
set_create_total_angle(0);
while (get_create_total_angle() > -3) { //angles itself a little bit
create_spin_CW(200);
}
create_spin_CW(0); //stops spinning
msleep(400);
create_drive_direct(-200, -200); //Drives forward a little bit to the next black line
msleep(1500);
create_drive_direct(0,0);
set_create_total_angle(0);
while (get_create_total_angle() > -7) { //Angles itself so it can drive in the middle
create_spin_CW(200);
}
create_spin_CW(0); //stops spinning
create_drive_direct(-230,-230); //Drives itself into the middle lane
msleep(4000);
create_drive_direct(0,0); //Reset stuff
set_create_total_angle(0);
while(analog(0) < blackThresh) { //Align itself using line following in the middle lane
create_spin_CW(50);
printf("Value is %d \n", analog(0));
if(digital(0) >= blackThresh) {
printf("I have found the middle black line!");
create_spin_CW(1); //Angle itself so it is exactly on the black line
}
}
create_spin_CW(0); //resets something for some reason why is this here?
set_servo_position(claw, 1561); //grabs the red cube
msleep(200);
set_servo_position(arm, 1984);
msleep(500);
set_servo_position(claw, 1144);
msleep(500);
return 0;
}
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();
}
void main (int argc, char ** argv)
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle = 0;
int dangle = 0;
int angle0 = 0;
int distance0 = 0;
int distance = 0 ;
int ddistance = 0;
// float lf_args[20];
// read inputs
// printf("Args: %d\n", argc);
// for (i = 2; i < argc; i++)
// {
// scanf("%lf%, argv[i]
// }
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
create_connect ();
clear_motor_position_counter (0);
enable_servo (0);
set_servo_position (0 ,1300);
//set_servo_position (1800);
// ------------------------------------------------------------------------
//step:1 aim the arm to knock over the box
//raise arm
printf("-----Step 1-----\n");
move_to_position (0, -900, -1200);
while(! loop_done)
{
if (get_motor_done (0))
{
loop_done = 1;
}
msleep(100);
}
create_stop ();
loop_done = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (50,-50);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done);
if (angle0 - get_create_total_angle (.05) >= 20)
{
loop_done = 1;
create_stop ();
}
msleep(100);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (-50,50);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done);
if (angle0 - get_create_total_angle (.05) <= -20)
{
loop_done = 1;
create_stop ();
}
msleep(100);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
// ------------------------------------------------------------------------
//step:2 knock over box and turn
//lift arm
//turn 90
printf("-----Step 2-----\n");
move_to_position (0, -900, -3100);
msleep (900);
angle0 = get_create_total_angle (.05);
angle = angle0;
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
create_drive_direct (-178,-648);
while(! loop_done)
{
if (dangle >= 125)
{
create_drive_direct (-120, -181);
}
if (dangle >= 155)
{
task_A_done = 1;
create_stop ();
}
if (get_motor_done (0))
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(100);
angle = get_create_total_angle (.05);
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
// ------------------------------------------------------------------------
//step:4 aim claw to point of botguy
//turn 20
//lower arm
printf("-----Step 4-----\n");
angle0 = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (70, -70);
printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
//while(angle0 - get_create_total_angle (.05) < 4)
if (0)
{
printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
msleep(50);
}
move_to_position (0, 400, -900);
distance0 = get_create_distance (.05);
create_drive_direct (20, 20);
while (! loop_done)
{
if (get_create_distance (.05) - distance0 > 50)
{
loop_done = 1;
create_stop ();
}
}
set_servo_position (0 ,400);
loop_done = 0;
task_A_done = 0;
task_B_done = 0;
create_drive_straight (-20);
move_to_position (0, 600, -300);
while (! get_motor_done (0))
{
msleep (50);
}
create_drive_straight (-50);
msleep (2000);
//while (! loop_done)
if (0)
{
if (digital (10))
{
create_stop ();
loop_done = 1;
}
if (get_create_distance (.05) - distance0 < -100)
{
create_stop ();
loop_done = 1;
}
}
loop_done = 0;
// ------------------------------------------------------------------------
//step:5 grab botguy and lift him up (not complete)
printf("-----Step 5-----\n");
create_drive_direct (60, -60);
while(angle0 - get_create_total_angle (.05) < 3)
{
msleep(50);
}
create_stop ();
set_servo_position (0, 1510);
move_to_position (0, -1000, -3000);
msleep (5000);
// ------------------------------------------------------------------------
//step:6 move backwards untill the bumper hits the transport (not completed)
printf("-----Step 6-----\n");
create_drive_direct (130, 280);
distance0 = get_create_distance (.05);
while (! loop_done)
{
printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
if (get_create_distance (.05) - distance0 > 700)
{
loop_done = 1;
}
msleep(50);
}
loop_done = 0;
move_to_position (0, -900, -500);
create_drive_straight (-100);
msleep (1000);
set_servo_position (0, 200);
create_disconnect ();
printf("-----Step 6-B-----\n");
create_drive_direct (-400, -150);
distance0 = get_create_distance (.05);
while(! loop_done)
{
printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
if (get_create_distance (.05) - distance0 < -100)
{
loop_done = 1;
create_stop ();
}
msleep(50);
}
loop_done = 0;
// ------------------------------------------------------------------------
//step:7 drop botguy.
printf("-----Step 7-----\n");
move_to_position (0, -900, -300);
msleep (6000);
set_servo_position (0, 200);
create_disconnect ();
}
