Étape 5: Code
Il peut être salissant, mais ça marche.#use « createlib.ic »
#use « xbccamlib.ic »
#define cam 0 / / port de servo de caméra
#define bras 3 / / bras port servo
#define et (analog(0)) //et port
/ * Le câble de créer doit également être branché. La prise d’alimentation
les 3 volets brancher sur port 8 et un X U étiquetées dans JP 28
(à côté du port USB) avec le U vers la caméra * /
#define c_down 5 / / servo de la caméra vers le bas
#define a_down 17 / / bras servo vers le bas
#define détiennent 50 / / servo tenir la balle
#define pris 27 / / arm position du servo pour empêcher de se coincer les doigts sur la table
#define tirer 150 / / servo throw boule
#define track_c 25 / / piste servo caméra close position
#define track_f 45 / / servo caméra piste position extrême
#define Centre 120 / / Centre de la vision de la caméra
#define inrange 30 / / track_y coordonnée lorsque la balle est dans la griffe
#define ball 0 / / canal de balle orange
#define ball_x (track_x(ball,0)) //x coordonnée du ballon
#define ball_y (track_y(ball,0)) //y coordonnée du ballon
#define lente 100 / / Vitesse du moteur lent
#define rapide 175 / / vitesse du moteur rapide
#define effacer 0.2//sleep vers l’arrière, loin des obstacles
#define temps 0,5 //1.0 est un droit de 90 degrés tournez
#define reste 0.05//time pour dormir tout en suivant les objets BLOB
#define speeda 175 / / vitesse d’eviter de tour
#define back_s -200 / / Vitesse à le pour arrière de l’objet heurté
#define droite 32767 / / drive en ligne droite
#define backb 2 / / canal de couleur principale panneau
#define carré 1 / / canal de couleur d’accentuation de panneau
#define track_d 250 / / position de caméra pour le suivi objectif
#define track_find 70 / / suivi de la position de la caméra pour longtemps
#define inverser le temps de 2.25//sleep pour un 180
#define back_f -150 / / retour rapide
#define back_sl -125/dos/ralenti
#define center_x 178 / / true x Centre de came
#define center_y 146 / / true y centrer de came
pida int ; processus //avoid
int pidb ; processus //track
int pidc ; processus //score
int have_ball = 0; //tells qui fonctionnent, nous sommes en
Sub main() {}
long ch ;
enable_servos (); //enable servos
init_camera (); //start caméra
CConnect (); //connect pour créer avec contrôle complet
start_a (); //start éviter fonction
start_b (); //start ball_tracking fonction
{while(1)}
Si (r_button () || gc_ldrop|| gc_rdrop) {//if ramassé ou touche épaule r
Kill(PIDA) ;
Kill(pidb) ;
Kill(PIDC) ;
disable_servos() ;
Disconnect() ;
Break;}
create_battery_charge() ;
display_clear() ;
printf ("frais = %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()) ;
tandis que (gc_battery_charge < 2800l & &! b_button()) {}
create_battery_charge() ;
display_clear() ;
printf ("frais = %l\n", gc_battery_charge) ;
Sleep(1.0);}
CConnect() ;
Back() ;
Sleep(2.0) ;
start_a() ;
start_b();}
}
}
void avoid() {}
while(1) {//repeat pour toujours
create_sensor_update (); //update toutes les valeurs du capteur
create_drive (speeda, droite) ;
if(gc_lbump==1) {//left bosse
avoid_right();} tourne à droite pour éviter
d’autre if(gc_rbump==1) {//right bosse
avoid_left();} tourne à gauche pour éviter
d’autre if(gc_lfcliff==1) {//left falaise avant
avoid_right();}
d’autre if(gc_rfcliff==1) {//right falaise avant
avoid_left();}
d’autre if(gc_lcliff==1) {//left falaise
avoid_right();}
d’autre if(gc_rcliff==1) {//right falaise
avoid_left();}
}
}
void track_ball() {}
Kill(PIDC) ;
while(!have_ball) {//repeat jusqu'à obtenir ball
track_update() ;
extrême (); //sets l’appareil photo
prêt (); //sets le bras
while(et<255) {//until valeur max arrive quand la balle est attrapée
track_update (); //update image de la caméra
Si (ball_x < =(center-5)) {//if la balle est laissée
track_update() ;
create_drive_direct (lente et rapide); //turn gauche
Sleep(REST);}
ElseIf (ball_x > =(center+5)) {//if la balle est bonne
track_update() ;
create_drive_direct(Fast,Slow); / / tourner à droite
Sleep(REST);}
ElseIf (ball_x <(center+5) & & ball_x >(center-5)) {/ / si le ballon est centré
track_update() ;
create_drive_straight (rapide); //go droite
Sleep(REST);}
}
Grab (); //grab ball
bip (); //make bruit
Stop (); //stop au volant
have_ball = 1; //make une note que j’ai boule
}
start_c (); //find le panier
sommeil (1.0); //sleep afin que je ne fais rien quand je me fais tué
}
void find_basket() {}
tuer (pidb); //kill ball, processus de suivi
trouver (); //put caméra vers le haut
track_set_minarea (1000); //the panneau est grand, ainsi seulement se pencher pour gros objets BLOB
tandis que (have_ball) {//while j’ai la balle
track_update() ;
while(track_x(backb,0) < =(center_x-20) || track_x(backb,0) > =(center_x+20)) {//while non centrée
track_update() ;
if(track_x(backb,0) > =(center_x+20)) {//if le panneau est laissé
track_update() ;
create_spin_CCW(100);} tourner à gauche
d’autre if(track_x(backb,0) < =(center_x-20)) {//if du panneau est juste
track_update() ;
create_spin_CW(300-center_x);} tourner à droite à ralentir à l’approche du centre
}
Stop() ;
while(track_size(backb,0) < =(6000)) {//while l’objectif est moins de 6000 pixels de taille
track_update() ;
if(track_x(backb,0) < =(center_x-5)) {//if reste la cible
track_update() ;
create_drive_direct (lente et rapide); //turn gauche
Sleep(REST);}
d’autre if(track_x(backb,0) > =(center_x+5)) {//if la cible a raison
track_update() ;
create_drive_direct(Fast,Slow); / / tourner à droite
Sleep(REST);}
d’autre if(track_x(backb,0) <(center+5) & & track_x(backb,0) >(center_x-5)) {/ / si la cible est centrée
track_update() ;
create_drive_straight (rapide); //go droite
Sleep(REST);}
}
Stop() ;
create_drive_straight(Fast); / / obtenir un peu plus près
Sleep(1.0) ;
Stop() ;
Sleep(1.0) ;
create_spin_CW (speeda); //spin droit
sommeil (inverse); //sleep long qu'assez pour un 180 tourner
Stop() ;
bas (); //put caméra vers le bas pour panneau de piste
Sleep(1.0) ;
track_set_minarea (200); //use une plus petite taille min, puisque nous sommes pointus dans ce domaine et va se rapprocher
while(track_y(backb,0) > =(center_y-140)) {//while la cible est moins que la coordonnée y
track_update() ;
if(track_x(backb,0) < =(center_x-5)) {//if reste la cible
track_update() ;
back_right (); //turn gauche
Sleep(REST);}
d’autre if(track_x(backb,0) > =(center_x+5)) {//if la cible a raison
track_update() ;
back_left(); / / tourner à droite
Sleep(REST);}
d’autre if(track_x(backb,0) <(center+5) & & track_x(backb,0) >(center_x-5)) {/ / si la cible est centrée
track_update() ;
arrière (); //go droite
Sleep(REST);}
}
Stop() ;
beep() ;
Jet (); //shoot
Sleep(1.0) ;
have_ball = 0; //reminder, j’ai jeté la balle et ne l’ont pas
}
start_b (); //back suivi de boule
sommeil (1.0); //don't quoi que ce soit jusqu'à la mort de ce processus
}
void cconnect() {}
create_connect() ;
create_full (); //for controll complète de capteurs de rebord
create_power_led(0,255);} énergie verte a conduit
disconnect() Sub {}
Stop (); //stop déménagement
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();}
stop() Sub {}
create_drive(0,Straight);}
back() Sub {}
create_drive(back_s,Straight);}
ready() Sub {}
set_servo_position (arm, a_down);}
check() Sub {}
set_servo_position (cam, track_c);}
void far() {}
set_servo_position (cam, track_f);}
void ledge() {}
set_servo_position (bras, pris);}
void throw() {}
int a ;
pour (un = 50; a > = 30 ; un-= 1) {//get prêt
set_servo_position(ARM, a);}
set_servo_position (bras, shoot);}
void grab() {}
int a ;
pour (a = 0; a < = attente ; un += 1) {//raise le bras en douceur
set_servo_position(ARM, a);}}
void down() {}
set_servo_position (cam, track_d);}
find() Sub {}
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 pour le courant, créer la fin du processus et ont priorité
kill_process(PID) ;
CREATE_FREE ; //i'm fait
Stop();}
void avoid_left() {}
tuer (pidb); //stop tout le reste
Kill(PIDC) ;
rebord (); //pick vers le haut de griffe donc il ne se coince pas sur la table
back_away (); //back.
rotate_l (); //rotate de l’obstacle
prêt (); //put griffe vers le bas
if(have_ball) {//if j’ai la balle
start_c();} commencer le suivi de l’objectif
d’autre if(!have_ball) {//if je n’ai pas la boule
start_b();} commencer le suivi de la balle
}
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);}
