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();
}
int main()
{
X=0;
create_connect();
set_create_total_angle(0);
printf("X\tAngle");
while(black_button()==0)
{
if (a_button()==1)
{
turnHalfCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (b_button()==1)
{
turnHalfCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (left_button()==1)
{
TurnCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (right_button() == 1)
{
TurnCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
}
}
void TurnCW()
{
while(get_create_total_angle(0.1) > -90)
{
create_spin_CW(500);
}
create_stop();
}
void TurnCCW()
{
while(get_create_total_angle(0.1) > 90)
{
create_spin_CCW(500);
}
create_stop(); //TEST THESE. IS IT TOO FAST FOR LAG TIME?
//IS IT BETTER TO GO MANUAL? (219.44 for 1 second)
}
/*
* Class: cbccore_low_Create
* Method: get_create_total_angle
* Signature: ()I
*/
JNIEXPORT jint JNICALL Java_cbccore_low_Create_get_1create_1total_1angle(JNIEnv *env, jobject obj, jfloat lag)
{
#ifdef CBC
return get_create_total_angle(lag);
#else
printf("Java_cbccore_low_Create_get_1create_1total_1angle stub\n");
return 0;
#endif
}
int main()
{
create_connect();
go(457.2,250);
resetd();
while (get_create_total_angle(10) < 90)
{turnl(250);}
reseta();
go(457.2,250);
resetd();
while (get_create_total_angle(10) > -90)
{turnr(250);}
reseta();
go(250,250);
while (get_create_total_angle(10) > -90)//Riddhi..your job is to write a
//function like mine, go, that makes an easy command to turn the robot.
{turnr(250);}
reseta();
go(400,250);
resetd();
motor(3,50);
sleep(1.5);//this is to lower the cage thing for the green
//and pink containers. might have to go forward or backward.
go(-400,250);//to go backwards
resetd();
while (get_create_total_angle(10) < 90)
{turnl(250);}
reseta();
go(-457.2,250);
resetd();
//need help with steps 13-21 on the pseudocode.
while (analog(2) < 100)
{create_drive_straight(250);} //to raise the cage, and leave the filled containers of luggage.
motor(3,50);
sleep(1.5);
enable_servos();
get_servo_position(2);
if (get_servo_position(2) > 1023)
{set_servo_position(2,get_servo_position(2)-1023);}
if (get_servo_position(2) < 1023)
{set_servo_position(2,get_servo_position(2)+1023);}
}
int main()
{
X=0;
create_connect();
set_create_total_angle(0);
printf("X\tAngle");
cbc_display_clear();
printf("A to turn CCW 90deg.\tB to turn CW 90deg.\n");
printf("Left to turn CCW 45deg.\tRight to turn CW 45deg.\n");
printf("Down to force angle readjustment.\n");
while(black_button()==0)
{
if (a_button()==1)
{
turn(tspeed,45);
//turnHalfCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (b_button()==1)
{
turn(tspeed,-45);
//turnHalfCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (left_button()==1)
{
turn(tspeed,90);
//turnCCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (right_button() == 1)
{
turn(tspeed,-90);
//turnCW();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
if (down_button() == 1)
{
turnAdjust();
printf("%d\t%d\n",X,get_create_total_angle(0.1));
}
}
}
/**
* 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);
}
}
void turn_for(int angle, int speed) {
set_create_total_angle(0);
if (speed > 0) {
while (get_create_total_angle() >-angle) {
create_spin_CW(speed);
}
create_spin_CW(0);
}
else if (speed < 0) {
while (get_create_total_angle() < angle) {
create_spin_CW(speed);
}
create_spin_CW(0);
}
}
int main(int argc, char ** argv)
{
int i;
printf("Connecting...\n");
create_connect();
create_start();
printf("Spinning CW...n");
create_spin_CW(40);
// spin a while CW
for (i = 0; i < 10; ++i)
{
printf("Distance: %d Angle: %d\n", get_create_distance(), get_create_total_angle());
msleep(1000);
}
printf("Spinning CCW...n");
create_spin_CCW(40);
// spin a while CCW
for (i = 0; i < 10; ++i)
{
printf("Distance: %d Angle: %d\n", get_create_distance(), get_create_total_angle());
msleep(1000);
}
printf("Stopping...\n");
create_stop();
printf("Disconnecting...\n");
create_disconnect();
printf("Done.\n");
return 0;
}
void TurnCW()
{
#ifdef CREATE
while(get_create_total_angle(0.1) > -90)
{
create_spin_CW(500);
}
create_stop();
#endif
#ifdef CBC
mav(1,500);
mav(0,-500);
sleep(2.5900127);
off(0);
off(1);
#endif
}
//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;
}
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 ();
}
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 CenterCamera() //CAMERA MUST BE AT CENTER OF CREATE, POINTING Right RESPECT TO CREATE
{
track_update();
if (track_x (0,0) > 81)
{
create_drive_straight(-200);
while (track_x (0,0) > 81) //WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY
{
track_update();
}
create_stop();
drivemm(inchestomm(4.9), 1);
set_create_total_angle(0);
create_drive(101.3,1); //uses int. does it round?
sleep(2.057252314);
create_stop();
get_create_total_angle(.1);
printf("asdb");
if (get_create_total_angle(.1) != 90)
{
set_create_total_angle(get_create_total_angle(.1) - 90);
printf("%f\n",get_create_total_angle(.1));
if (get_create_total_angle(.1) > 0) //CCW ADDS TO TOTAL ANGLE
{
create_spin_CW(100);
while (get_create_total_angle(.1) < 0);
{
printf("aeqweqw");
}
create_stop();
}
if (get_create_total_angle(.1) < 0)
{
create_spin_CCW(100);
while (get_create_total_angle(.1) > 0);
{
printf("BBBB");
}
create_stop();
}
}
}
/* (track_x (0,0) < 79)
{
create_drive_straight(200);
while (track_x (0,0) < 79)
{
track_update();
}
create_stop();
beep();
set_create_distance(0);
drivemm(-inchestomm(5), 1);
create_drive(206.972051,1);
sleep(1);
create_stop();
create_drive_straight(-200);
while(digital(8 == 0))
{
}
create_stop();
}*/
}
int main()
{
int stupid_int = 0;
int value = 0;
int i = 0;
int angle_of_pole = 90;
camera_open(LOW_RES);
camera_update();
create_connect();
set_create_total_angle(0);
value = get_create_total_angle();
printf("Angle is %d\n", value);
//------------------------
// part 1: spin until past all booster sections
while(get_create_total_angle() > -85){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(1,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
enable_servos();
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of_pole = 60;
//end part 3
//part 4: repeat everything
/*while(get_create_total_angle() > -80){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 50;
//end part 3
while(get_create_total_angle() > -70){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 50;
//end part 3
while(get_create_total_angle() > -100){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 40;
//end part 3
while(get_create_total_angle() > -60){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 60;
//end part 3*/
//end part 4
}
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 ();
}
