Cómo hacer un baloncesto autónoma jugar robot utilizando un iRobot Create como base (5 / 7 paso)

Paso 5: código

#use "createlib.ic"
#use "xbccamlib.ic"

#define cam 0 / / puerto de servo de cámara
#define brazo 3 / / Puerto servo del brazo
#define et (analog(0)) Puerto //et

/ * Crear cable también tiene que estar enchufada. La toma de corriente,
los 3 frentes enchufe en el puerto de 8 y la una etiqueta U X en JP 28
(al lado del puerto USB) con la U hacia la cámara * /

#define c_down 5 / / servos de la cámara hacia abajo
#define a_down 17 / / servo del brazo hacia abajo
#define mantenga 50 / / servo retención bola
#define atrapados 27 / / posición del servo para evitar quedar atrapado en la tabla del brazo
#define disparar 150 / / servo lanzar bola
#define track_c 25 / / pista de servo cámara cerrar posición
#define track_f 45 / / servo cámara pista posición lejana

#define centro 120 / / centro de la visión de la cámara
#define inrange 30 / / track_y coordinar cuando es bola en garra
#define bola 0 / / canal de bola naranja
#define ball_x (track_x(ball,0)) //x coordenadas de bola
#define ball_y (track_y(ball,0)) //y coordenadas de bola

#define lento 100 / velocidad del motor lento
#define rápida 175 / velocidad del motor rápido

#define clara 0.2//sleep hacia atrás sin obstáculos
#define tiempo 0.5 //1.0 es girar la derecha 90 grados
#define resto 0.05//time a dormir mientras gotas de seguimiento

#define speeda 175 / velocidad de evitar girar
#define back_s -200 / velocidad a la parte posterior del objeto beta
#define 32767 recta / / conducir en línea recta

#define backb 2 / canal de color principal tablero
#define cuadrado 1 / canal de color de acento del tablero
#define track_d 250 / posición de la cámara para el seguimiento de objetivo
#define track_find 70 / / seguimiento de la posición de la cámara por mucho tiempo
#define 2.25//sleep inversa tiempo para un 180
#define back_f -150 / / espalda alta velocidad
#define back_sl -125/espalda despacio
#define center_x 178 / / true x centro de cam
#define center_y 146 / / true y centro de la leva

int pida; //avoid proceso
pidb int; //track proceso
int pidc; //score proceso
int have_ball = 0; //tells función que estamos en

void main() {}
ch largo;
enable_servos (); //enable servos
init_camera (); //start cámara
Conecte (); //connect con control completo
start_a (); //start evitar la función
start_b (); //start ball_tracking función
{while(1)}
Si ((r_button) || gc_ldrop|| gc_rdrop) {//if recogido o botón del hombro de r
Kill(pida);
Kill(pidb);
Kill(PIDC);
disable_servos();
Disconnect();
Break;}
create_battery_charge();
display_clear();
printf ("costo = %l\n", gc_battery_charge);
if(gc_battery_charge<1200l|| {b_button())}
Kill(pida);
Kill(pidb);
Kill(PIDC);
Throw();
have_ball = 0;
create_demo(1);
while(b_button());
mientras que (gc_battery_charge < 2800l & &! b_button()) {}
create_battery_charge();
display_clear();
printf ("costo = %l\n", gc_battery_charge);
Sleep(1.0);}
Cconnect();
Back();
Sleep(2.0);
start_a();
start_b();}
}

}

void avoid() {}

while(1) {//repeat para siempre

create_sensor_update (); //update todos los valores del sensor
create_drive (speeda, recta);
if(gc_lbump==1) {//left de golpe
avoid_right();} gira a la derecha para evitar
if(gc_rbump==1) else {//right bump
avoid_left();} gira a la izquierda para evitar
if(gc_lfcliff==1) else {//left frente acantilado
avoid_right();}
if(gc_rfcliff==1) else {//right frente acantilado
avoid_left();}
if(gc_lcliff==1) else {//left acantilado
avoid_right();}
if(gc_rcliff==1) else {//right acantilado
avoid_left();}
}
}

void track_ball() {}

Kill(PIDC);

while(!have_ball) {//repeat hasta obtener bola

track_update();
lejos (); //sets la cámara
listo (); //sets el brazo

while(et<255) {//until valor máximo ocurre cuando la bola es atrapada

track_update (); imagen de cámara //update

Si (ball_x < =(center-5)) {//if la bola queda
track_update();
create_drive_direct (lento, rápido); //turn izquierda
Sleep(REST);}

else if (ball_x > =(center+5)) {//if la bola es
track_update();
create_drive_direct(Fast,Slow); / / girar a la derecha
Sleep(REST);}

else if (ball_x <(center+5) & & ball_x >(center-5)) {/ / si la bola es centrada
track_update();
create_drive_straight (rápida), //go recta
Sleep(REST);}
}
Grab (); //grab bola
BEEP (); //make ruido
STOP (); //stop de conducción
have_ball = 1; //make una nota que tengo bola
}
start_c (); //find la cesta
sueño (1.0); //sleep por lo que no estoy haciendo nada cuando sale maté
}

void find_basket() {}

Kill (pidb); //kill bola de seguimiento
buscar (); //put cámara para arriba
track_set_minarea (1000); //the tablero es grande, tan sólo buscan grandes blobs

mientras (have_ball) {//while tengo la bola

track_update();
while(track_x(backb,0) < =(center_x-20) || track_x(backb,0) > =(center_x+20)) {//while no centrada
track_update();

if(track_x(backb,0) > =(center_x+20)) {//if el tablero quedo
track_update();
create_spin_CCW(100);} Gire a la izquierda

otra cosa if(track_x(backb,0) < =(center_x-20)) {//if del tablero es correcta
track_update();
create_spin_CW(300-center_x);} Gire a la derecha de frenar como los enfoques del centro
}
STOP();

while(track_size(backb,0) < =(6000)) {//while el objetivo es de menos de 6000 píxeles de tamaño
track_update();
if(track_x(backb,0) < =(center_x-5)) {//if el objetivo quedo
track_update();
create_drive_direct (lento, rápido); //turn izquierda
Sleep(REST);}

if(track_x(backb,0) otro > =(center_x+5)) {//if el destino es justo
track_update();
create_drive_direct(Fast,Slow); / / girar a la derecha
Sleep(REST);}

if(track_x(backb,0) otra cosa <(center+5) & & track_x(backb,0) >(center_x-5)) {/ / si el objetivo se centra
track_update();
create_drive_straight (rápida), //go recta
Sleep(REST);}
}
STOP();
create_drive_straight(Fast); / / obtener un poco más cerca
Sleep(1.0);
STOP();
Sleep(1.0);
create_spin_CW (speeda); derecho de //spin
sueño (inversa); //sleep tiempo que suficiente para un 180 vuelta
STOP();
hacia abajo (); //put cámara hasta tablero de pista
Sleep(1.0);
track_set_minarea (200); //use un tamaño más pequeño de min, ya que somos acentuados en él y va a estar más cerca de

while(track_y(backb,0) > =(center_y-140)) {//while el objetivo es menor que la coordenada y
track_update();
if(track_x(backb,0) < =(center_x-5)) {//if el objetivo quedo
track_update();
back_right (); //turn izquierda
Sleep(REST);}

if(track_x(backb,0) otro > =(center_x+5)) {//if el destino es justo
track_update();
back_left(); / / girar a la derecha
Sleep(REST);}

if(track_x(backb,0) otra cosa <(center+5) & & track_x(backb,0) >(center_x-5)) {/ / si el objetivo se centra
track_update();
espalda (); //go recta
Sleep(REST);}

}
STOP();
BEEP();
tiro (); //shoot
Sleep(1.0);
have_ball = 0; //reminder I lanzó la bola y no lo tiene
}
start_b (); //back para seguimiento de la bola
sueño (1.0); //don't hacer nada hasta que este proceso muera
}

void cconnect() {}
create_connect();
create_full (); //for control completo de sensores de borde
create_power_led(0,255);} led verde de alimentación

{} void disconnect()
STOP (); //stop en movimiento
create_disconnect();}

void back_away() {}
Back();
Sleep(Clear);
STOP();}

void rotate_l() {}
create_spin_CCW(speeda);
Sleep(Time);
STOP();}

void rotate_r() {}
create_spin_CW(speeda);
Sleep(Time);
STOP();}

void stop() {}
create_drive(0,straight);}

{} void back()
create_drive(back_s,straight);}

{} void ready()
set_servo_position (brazo, a_down);}

{} void check()
set_servo_position (cam, track_c);}

void far() {}
set_servo_position (cam, track_f);}

void ledge() {}
set_servo_position (brazo, cogido);}

{} void throw()
int a;
para (a = 50; a > = 30;-= 1) {//get listo
set_servo_position(ARM, a);}
set_servo_position (brazo, disparar);}

void grab() {}
int a;
para (a = 0; a < = espera; una += 1) {//raise el brazo suavemente
set_servo_position(ARM, a) ;}}

{} void down()
set_servo_position (cam, track_d);}

find() vacío {}
set_servo_position (cam, track_find);}

void start_a() {}
Pida = start_process(avoid());}

void start_b() {}
pidb = start_process(track_ball());}

void start_c() {}
PIDC = start_process(find_basket());}

void kill(int pid) {}
CREATE_BUSY; //wait para corriente crear proceso, terminar y tener prioridad
kill_process(PID);
CREATE_FREE; //i'm hecho
STOP();}

void avoid_left() {}
Kill (pidb); //stop todo lo demás
Kill(PIDC);
cornisa (); //pick a garra para no quedar atrapado en la tabla
back_away (); //back a
rotate_l (); //rotate del obstáculo
listo (); //put la garra hacia abajo
if(have_ball) {//if tengo la bola
start_c();} iniciar seguimiento objetivo
if(!have_ball) else {//if no tengo la bola
start_b();} iniciar el seguimiento de la bola
}

void avoid_right() {}
Kill(pidb);
Kill(PIDC);
ledge();
back_away();
rotate_r();
Ready();
{if(have_ball)}
start_c();}
else {} if(!have_ball)
start_b();}
}

void back_left() {}
create_drive_direct(back_f,back_sl);}

void back_right() {}
create_drive_direct(back_sl,back_f);}