void GotoBox()
{
while (get_create_lbump(.1) == 0 && get_create_rbump(.1) == 0)//drive backwards until hit pvc w/ bumper
{
create_drive_straight(200); //unless arm is strong enough to hold
}
create_stop();
ClearWall(); //move away from pvc to turn
if(Side == 0) //turn towards outer starting box -- now facing our outer starting box w/ blocks in claw
{
TurnCCW();
}
if(Side == 1)
{
TurnCW();
}
create_stop();
ForwardTouch(); //forward till front touch sensor ==1
create_drive_straight(132); //one cube length (5 inches) + 5 mm safety // drive backwards
sleep(1);
Release(); // realease claw w/ 2 blocks -- block now in starting box
create_drive_straight(-75);
sleep(1);
create_stop(); //make sure cubes are touching pvc
}
int main()
{
create_connect();
init_servo();
//light_it_up(LIGHT_PORT);
lightstart(LIGHT_PORT,120.0);
create_full();
create_drive_segment(HIGH_SPEED, -150);
start_process(set_top);
create_drive_arc(HIGH_SPEED, -200, 155);
create_drive_segment(HIGH_SPEED, -365);
create_drive_arc(HIGH_SPEED, 90, -64);
create_drive_segment(HIGH_SPEED, -85);
create_cease();
set_top();
align_twall();
grab_top_de();
create_spin_angle(400,-165);
create_drive_segment(HIGH_SPEED, -320);
//align_wall();
create_stop();
create_sync();
dump_kelp_de();
create_drive_segment(HIGH_SPEED,30);//go away from starting box
create_spin_angle(HIGH_SPEED, -78);//turn to gate part 1
create_drive_segment(HIGH_SPEED, 938);//go to gate
create_drive_arc(HIGH_SPEED,185,84);//arc to gate
//create_drive_time(HIGH_SPEED,1800);//go through the gate
/*
create_drive_segment(HIGH_SPEED,1470);
create_drive_segment(HIGH_SPEED,-100);
create_spin_angle(HIGH_SPEED,78);
*/
create_cease();
create_drive_straight(HIGH_SPEED);//go onto oppponent's side of board
msleep(1400);
create_stop();
create_arc(350,550);//arc to the opponent's mpa
msleep(2500L);
create_stop();//pause to keep the creat
create_drive_straight(HIGH_SPEED);//go onto oppponent's side of board
msleep(750);
create_stop();
create_drive_straight(-HIGH_SPEED);//go onto oppponent's side of board
msleep(5000);
create_stop();
/*
create_drive_segment(500,-500);//go past the IC
create_drive_arc(320,-250,25);//arc to
create_drive_arc(320,250,-8);
*/
//create_drive_segment(HIGH_SPEED,-750);
//create_drive_arc(HIGH_SPEED,100,84);
//create_drive_segment(HIGH_SPEED,1300);
}
int main()
{
int i;
TurnCCW();//don't know what turn function we're using
for(i=0; i < 5; i++)
{
create_drive_straight(100);
sleep(2);
CenterCamera2();
}
for(create_drive_straight(-200); !digital(TouchSensor);){}
}
void ClearWall()
{
if(get_create_lbump(.1) == 1 || get_create_rbump(.1) == 1)
{
create_drive_straight(-132); //one cube length (5 inches) + 5 mm safety
sleep(1);
create_stop();
}
if(digital(TouchSensor) == 1)
{
create_drive_straight(132);
sleep(1);
create_stop();
}
}
int main()
{
printf("Line Follow."); // announce the program
sleep(1.0); // wait 1 second
create_connect(); // Open the connection between CBC and Create
int ana0 = 0, ana1 = 0; // variables to store the results of the analog sensors
create_full(); // We don't care about safety
while(get_create_lbump(0) == 0 && get_create_rbump(0) == 0){ //while the bumper is not pressed
ana0 = analog10(0); // left sensor
ana1 = analog10(1); // right sensor
printf("analog 0: %d\n", ana0);// print results
printf("analog 1: %d\n", ana1);
if((ana0 < 200) && (ana1 > 200)) // if the left sensor is off and right is on black line
{
create_spin_CW(128); // spin the create Clock Wise
}else if((ana1 < 200) && (ana0 > 200)) // else if reversed
{
create_spin_CCW(128); // spin Counter Clock Wise
}else if((ana0 < 200) && (ana1 < 200)) // else neither is on the line
{
create_drive_straight(180); // drive straight
}
}
create_stop(); // Stop the Create
create_disconnect(); // Disconnect the Create
}
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();
}
//generic move forward function calibrated in inches
void forward (int inches){
create_drive_straight(SPEED_FWD);
int time_conversion = inches * FORWARD_CONSTANT;
msleep(time_conversion);
create_stop();
}
//generic move backward function calibrated in inches
void backward (int inches){
create_drive_straight(SPEED_BWD);
int time_conversion = inches * BACKWARD_CONSTANT;
msleep(time_conversion);
create_stop();
}
/*
* Class: cbccore_low_Create
* Method: create_drive_straight
* Signature: (I)V
*/
JNIEXPORT void JNICALL Java_cbccore_low_Create_create_1drive_1straight(JNIEnv *env, jobject obj, jint speed)
{
#ifdef CBC
create_drive_straight(speed);
#else
printf("Java_cbccore_low_Create_create_1drive_1straight stub\n");
#endif
}
void ForwardTouch ()
{
while (analog(TouchSensor) == 0)
{
create_drive_straight(-500);
}
create_stop();
}
void CenterCamera() //CAMERA MUST BE AT CENTER OF CREATE, POINTING RIGHT RESPECT TO CREATE
{
create_drive_straight(-200);
while (track_x (0,0) > 84) //WHAT TRACK # IS RED? MAKE SURE IT DOESNT DETECT BOTGUY
{
track_update();
}
create_stop();
if (track_x (0,0) < 75)
{
create_drive_straight(200);
while (track_x (0,0) < 75)
{
track_update();
}
create_stop();
}
}
void kill() {
printf("killing in %i.", wait * 1000);
msleep(wait * 1000);
printf("killing program.\n");
disable_servos();
create_drive_straight(0);
motor(0, 0);
motor(1, 0);
motor(2, 0);
motor(3, 0);
exit(1);
}
// script to move dist mm at speed mm/sec
void create_script_move(int dist, int speed) {
int sdist, ddist;
create_drive_straight(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();
}
void testDrive(){
float time1, time2;
set_create_distance(0);
printf("//Press Black Button to start/stop drive\n");
while(!black_button()); time1 = seconds();
create_drive_straight(-50);
printf("drive(50);\t");
sleep(2.0);
while(!black_button()); time2 = seconds();
stop();
printf("sleep(%f);\t//move %dmm",time2-time1,get_create_distance(0));
done = 1;
}
void main()
{
create_connect();
enable_servos();
set_each_analog_state(1,1,0,0,0,0,0,0);
initialize();
armPosBlockStack2();
clawStack2();
clawOpen();
create_drive_straight(300);
run_for(3, clawGrab);
create_stop();
clawGrab();
}
int main()
{
set_each_analog_state(0,0,0,1,0,0,0,0);// This line sets analog ports to be pullup (0) or floating (1)
create_connect();
enable_servos();
set_create_total_angle(0);
set_create_distance(0);
//AUO=start_process(ArmUpOpen);
ArmUp();
set_servo_position(
sleep(3);
CenterCamera();
create_drive_straight(-200);
while(digital(8) != 1)
{}
create_stop();
sleep(1);
ArmDownClose();
sleep(3);
beep();
create_drive_straight(200);
sleep(2);
create_stop();
}
void drivemm(float mmspeed, float seconds)//speed is mm/s
{
#ifdef CREATE
create_drive_straight(mmspeed);
sleep(seconds);
create_stop();
printf("%d mm/s * %d seconds = %d mm.\n", mmspeed, seconds, mmspeed*seconds);
#endif
#ifdef CBC
mav(0,mmspeed);
mav(1,mmspeed);
sleep(seconds);
off(0);
off(1);
#endif
}
int main()
{
create_connect();
float x=.45;
if(black_button() == 1)
{
create_drive_straight(-100);
sleep(10);
create_stop();
}
while(left_button() == 0)
{
while(black_button() == 0)
{
if (a_button() == 1)
{
x=x+.001;
printf("%f\n", x);
sleep(.5);
}
if (b_button() == 1)
{
x=x-.001;
printf("%f\n", x);
sleep(.5);
}
if (up_button() == 1)
{
x=x+.0001;
printf("%f\n", x);
sleep(.5);
}
if (down_button() == 1)
{
x=x-.0001;
printf("%f\n", x);
sleep(.5);
}
}
create_spin_CW(450);
sleep(x);
create_stop();
}
}
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(int argc, char** argv) {
enable_servo(kServoPortSorter);
set_servo_position(kServoPortSorter, kServoPositionSorterCenter);
clear_motor_position_counter(kMotorPortBayLeft);
clear_motor_position_counter(kMotorPortBayRight);
create_connect();
camera_open(LOW_RES);
scanf("%s", NULL);
printf("waiting for light\n");
while (analog(0) > 200) {}
shut_down_in(700000);
motor(kMotorPortBayLeft, 10);
motor(kMotorPortBayRight, 10);
while (get_motor_position_counter(kMotorPortBayLeft) < 230 || get_motor_position_counter(kMotorPortBayRight) < 230) {
printf("pos: %i/%i\n", get_motor_position_counter(kMotorPortBayLeft), get_motor_position_counter(kMotorPortBayRight));
}
motor(kMotorPortBayLeft, 0);
motor(kMotorPortBayLeft, 0);
// wait_for_side_button();
msleep(6000);
create_drive_straight(200);
msleep(1850);
create_spin_CCW(200);
msleep(750);
create_drive_straight(-200);
msleep(1500);
create_spin_CW(200);
msleep(1750);
create_drive_straight(200);
while (!get_create_lbump() && !get_create_rbump()) {}
create_drive_straight(-150);
msleep(900);
create_spin_CCW(200);
msleep(950);
create_drive_straight(150);
while (!get_create_lbump() && !get_create_rbump()) {}
create_stop();
msleep(10000);
thread all_off = thread_create(wait_for_kill);
thread_start(all_off);
raise_bay();
create_spin_CW(100);
msleep(500);
create_drive_straight(-100);
msleep(1000);
create_stop();
thread jiggle_c = thread_create(jiggle_create);
thread_start(jiggle_c);
thread sort_b = thread_create(sort_balls);
thread_start(sort_b);
msleep(33000);
while (true) {}
/*thread_destroy(jiggle_c);
create_stop();
lower_bay();
create_drive_straight(100);
while (!get_create_lbump() && !get_create_rbump()) {}
create_stop();
msleep(10000);
raise_bay();
create_spin_CW(100);
msleep(500);
create_drive_straight(-100);
msleep(1000);
create_stop();
thread_create(jiggle_create);
thread_start(jiggle_c);
thread_create(sort_balls);
while (true) {}
camera_close();*/
return 0;
}
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 ();
}
int main()
{
create_connect();
create_drive_straight(1000);
sleep(2);
}
int main()
{
int m1=0,m2=3,pageno=0;
double createDriveTime;
printf("Start!\n");
extra_buttons_show();
while (1)
{
while (pageno == 0)
{
set_a_button_text("Forward");
set_b_button_text("Backward");
set_c_button_text("All Off");
set_x_button_text("OpenHand");
set_y_button_text("CloseHand");
set_z_button_text("Create Page");
if (a_button_clicked())
{
printf("Moving forward...\n");
motor(m1,100);
motor(m2,100);
while (!c_button_clicked())
{
msleep(1);
}
ao();
}
if (b_button_clicked())
{
printf("Moving backward...\n");
motor(m1,-100);
motor(m2,-100);
while (!c_button_clicked())
{
msleep(1);
}
ao();
}
if (x_button_clicked())
{
enable_servo(3);
set_servo_position(3,0);
msleep(300);
disable_servo(3);
}
if (y_button_clicked())
{
enable_servo(3);
set_servo_position(3,1300);
msleep(300);
disable_servo(3);
}
if (z_button_clicked())
{
pageno=1;
create_connect();
}
}
while (pageno == 1)
{
set_a_button_text("Create Fwd");
set_b_button_text("Create Bwd");
set_c_button_text("Stop");
set_x_button_text("Turn R");
set_y_button_text("Turn L");
set_z_button_text("M S Page");
if (a_button_clicked())
{
createDriveTime = 0;
printf("Create Moving forward w/ spd 100...\n");
create_drive_straight(100);
createDriveTime = seconds();
while (!c_button_clicked())
{
msleep(1);
}
create_stop();
printf("Time: %f seconds\n",seconds() - createDriveTime );
}
if (b_button_clicked())
{
createDriveTime = 0;
printf("Create Moving backward w/ spd 100...\n");
create_drive_straight(-100);
createDriveTime = seconds();
while (!c_button_clicked())
{
msleep(1);
}
create_stop();
printf("Time: %f seconds\n",seconds() - createDriveTime );
}
if (x_button_clicked())
{
create_drive(-50,0);
while (!c_button_clicked())
{
msleep(1);
}
create_stop();
}
if (y_button_clicked())
{
create_drive(50,0);
while (!c_button_clicked())
{
msleep(1);
}
create_stop();
}
if (z_button_clicked())
{
pageno=0;
create_disconnect();
}
}
}
return 0;
}
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 drive(x)
{
create_drive_straight(x);
}
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 Cubes (void)
{
if (Side == 0)
{
//Right of Botguy
//keep offense in box until scorer gets into position
//start with claw facing out
ForwardTouch(); //until it hits luggage DOES BAR TOUCH SENSOR REGISTER LUGGAGE TOUCH?
TurnCCW(); //turn left -- now facing airplanes
CenterCamera(); //move forwards and center the camera on the blocks -- now facing airplanes and in front of blocks
TurnCCW(); //turn left to face blocks with claw -- now in front of and facing blocks
ForwardTouch(); //move forward until it hits blocks
Grab(); //grab blocks
/*create_drive_straight(100);
sleep(2);//to avoid crashing with our luggage carts
create_stop();
TurnCCW();*/
//OFFENSE SHOULD BE OUT OF STARTING BOX BY NOW
//FORWARD BAR SENSOR SHOULD NOT BE OBSTRUCTED
//CREATE SHOULD DRIVE BACKWARDS
GotoBox();
create_drive_straight(200);
sleep(1);
create_stop();
TurnCW();
create_drive_straight(-300);
sleep(1);
create_stop();
TurnCW();
CenterCamera();
TurnCCW();
ForwardTouch();
Grab();
TurnCCW();
create_drive_straight(-500);
sleep(2.2352);
TurnCW();
GotoBox();
Release();
}
if (Side == 1) //Left of Botguy. START WITH CREATE FACING OUT
{
ForwardTouch(); //Switch to distance
TurnCW();
ForwardTouch();
create_drive_straight(50);
sleep(1);
create_stop();
TurnCW();
CenterCamera();
TurnCCW();
ForwardTouch();
Grab();
TurnCCW();
/*create_drive_straight(100);
sleep(1);
create_stop();
TurnCW();*/
while (get_create_lbump(.1) == 0 && get_create_rbump(.1) == 0)
{
create_drive_straight(200); //unless arm is strong enough to hold
}
ClearWall();
TurnCW();
ForwardTouch();
create_drive_straight(132); //one cube length (5 inches) + 5 mm safety
sleep(1);
create_stop();
create_drive_straight(-75);
sleep(1);
create_stop();
Release();
}
}
