Stap 5: Code
Het kan worden rommelig, maar het werkt.#use "createlib.ic"
#use "xbccamlib.ic"
#define cam 0 / / camera servo poort
#define arm 3 / / arm servo poort
#define et (analog(0)) //et poort
/ * De kabel maken ook dient te worden aangesloten. De macht jack,
de 3 fronten sluit aan op de poort 8 en de een gelabelde U X in JP 28
(naast de USB-poort) met de U richting de camera * /
#define c_down 5 / / camera servo naar beneden
#define a_down 17 / / servo arm omlaag
#define houden 50 / / servo hold bal
#define gevangen 27 / / arm servo positie om te voorkomen betrapt op tafel
#define schieten 150 / / servo Gooi bal
#define track_c 25 / / camera servo track positie sluiten
#define track_f 45 / / camera servo track ver positie
#define center 120 / / center van camera visie
#define inrange 30 / / track_y coördineren wanneer de bal is in de klauw
#define bal 0 / / kanaal van oranje bal
#define ball_x (track_x(ball,0)) //x-coördinaat van de bal
#define ball_y (track_y(ball,0)) //y-coördinaat van de bal
#define langzaam 100 / / snelheid van langzame motor
#define snel 175 / / snelheid van snelle motor
#define duidelijk 0.2//sleep back uit de buurt van obstakels
#define tijd 0,5 //1.0 is een recht 90 graden draaien
#define rest 0.05//time om te slapen terwijl het bijhouden van de blobs
#define speeda 175 / / snelheid van vermijden om
#define back_s-200 / / speed om terug uit de buurt van bumped object
#define rechte 32767 / / drive in een rechte lijn
#define backb 2 / / kanaal van Rugplank hoofdkleur
#define square 1 / / kanaal van Rugplank accentkleur
#define track_d 250 / / camera positie voor het bijhouden van doel
#define track_find 70 / / camerapositie lang bijhouden
#define omgekeerde 2.25//sleep tijd voor een 180
#define back_f-150 / / weer hoge snelheid
#define back_sl-125 / / rug trage snelheid
#define center_x 178 / / waar x centrum voor cam
#define center_y 146 / / waar y center voor cam
int pida; //avoid proces
int pidb; //track-proces
int pidc; //score-proces
int have_ball = 0; //tells welke functie wij zijn in
void main() {}
lange ch;
enable_servos (); //enable servo 's
init_camera (); //start camera
cconnect (); //connect maken met volledige controle
start_a (); //start vermijden functie
start_b (); //start ball_tracking functie
while(1) {}
Als (r_button () || gc_ldrop|| gc_rdrop) {//if pakte of r schouder knop
Kill(pida);
Kill(pidb);
Kill(pidc);
disable_servos();
Disconnect();
pauze;}
create_battery_charge();
display_clear();
printf ("gratis %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());
terwijl (gc_battery_charge < 2800l & &! b_button()) {}
create_battery_charge();
display_clear();
printf ("gratis %l\n =", gc_battery_charge);
Sleep(1.0);}
cconnect();
Back();
Sleep(2.0);
start_a();
start_b();}
}
}
ongeldig avoid() {}
while(1) {//repeat forever
create_sensor_update (); //update alle sensor-waardes
create_drive (speeda, rechte);
if(gc_lbump==1) {//left hobbel
avoid_right();} draait rechts om te voorkomen dat
anders if(gc_rbump==1) {//right hobbel
avoid_left();} draait links om te voorkomen dat
anders if(gc_lfcliff==1) {//left front cliff
avoid_right();}
anders if(gc_rfcliff==1) {//right front cliff
avoid_left();}
anders if(gc_lcliff==1) {//left klif
avoid_right();}
anders if(gc_rcliff==1) {//right klif
avoid_left();}
}
}
ongeldig track_ball() {}
Kill(pidc);
while(!have_ball) {//repeat tot get bal
track_update();
ver (); //sets de camera
klaar (); //sets de arm
while(et<255) {//until max waarde gebeurt er wanneer de bal wordt gevangen
track_update (); //update camerabeeld
Als (ball_x < =(center-5)) {//if de bal wordt overgelaten
track_update();
create_drive_direct (langzame, snel); //turn links
Sleep(rest);}
anders als (ball_x > =(center+5)) {//if de bal klopt
track_update();
create_drive_direct(Fast,Slow); / / sla rechtsaf
Sleep(rest);}
anders als (ball_x <(center+5) & & ball_x >(center-5)) {/ / als de bal is gecentreerd
track_update();
create_drive_straight (snelle); //go rechte
Sleep(rest);}
}
Grab (); //grab bal
pieptoon (); //make lawaai
Stop (); //stop rijden
have_ball = 1; //make een opmerking dat ik heb bal
}
start_c (); //find de mand
slaap (1.0); //sleep zodat ik niets doen wanneer ik gedood
}
ongeldig find_basket() {}
Kill (pidb), //kill bal proces voor het bijhouden
zoeken (); //put camera omhoog
track_set_minarea (1000); //the Rugplank is groot, dus alleen zoekt naar grote blobs
terwijl (have_ball) {//while ik heb de bal
track_update();
while(track_x(backb,0) < =(center_x-20) || track_x(backb,0) > =(center_x+20)) {//while niet gecentreerd
track_update();
if(track_x(backb,0) > =(center_x+20)) {//if het bord blijft
track_update();
create_spin_CCW(100);} Sla linksaf
anders if(track_x(backb,0) < =(center_x-20)) {//if het bord klopt
track_update();
create_spin_CW(300-center_x);} Sla rechtsaf vertragen het naderen van de center
}
Stop();
while(track_size(backb,0) < =(6000)) {//while het doel is van minder dan 6000 pixels in grootte
track_update();
if(track_x(backb,0) < =(center_x-5)) {//if het doel blijft
track_update();
create_drive_direct (langzame, snel); //turn links
Sleep(rest);}
anders if(track_x(backb,0) > =(center_x+5)) {//if de doelstelling klopt
track_update();
create_drive_direct(Fast,Slow); / / sla rechtsaf
Sleep(rest);}
anders if(track_x(backb,0) <(center+5) & & track_x(backb,0) >(center_x-5)) {/ / als het doel is gecentreerd
track_update();
create_drive_straight (snelle); //go rechte
Sleep(rest);}
}
Stop();
create_drive_straight(Fast); / / een beetje dichter
Sleep(1.0);
Stop();
Sleep(1.0);
create_spin_CW (speeda), //spin recht
slaap (achteruit); //sleep lang die genoeg voor een 180 draaien
Stop();
omlaag (); //put camera tot track Rugplank
Sleep(1.0);
track_set_minarea (200); //use min kleiner, aangezien we puntige en gonna dichter
while(track_y(backb,0) > =(center_y-140)) {//while het doel is minder dan de y-coördinaat
track_update();
if(track_x(backb,0) < =(center_x-5)) {//if het doel blijft
track_update();
back_right (); //turn links
Sleep(rest);}
anders if(track_x(backb,0) > =(center_x+5)) {//if de doelstelling klopt
track_update();
back_left(); / / sla rechtsaf
Sleep(rest);}
anders if(track_x(backb,0) <(center+5) & & track_x(backb,0) >(center_x-5)) {/ / als het doel is gecentreerd
track_update();
terug (); //go rechte
Sleep(rest);}
}
Stop();
Beep();
gooien (); //shoot
Sleep(1.0);
have_ball = 0; //reminder ik gooide de bal en niet hebt
}
start_b (); //back bal bijhouden
slaap (1.0); //don't iets totdat dit proces sterft
}
ongeldig cconnect() {}
create_connect();
create_full (); de volledige controle van de //for van richel sensoren
create_power_led(0,255);} groene stroom geleid
ongeldig disconnect() {}
Stop (); //stop verplaatsen
create_disconnect();}
ongeldig back_away() {}
Back();
Sleep(Clear);
Stop();}
ongeldig rotate_l() {}
create_spin_CCW(speeda);
Sleep(time);
Stop();}
ongeldig rotate_r() {}
create_spin_CW(speeda);
Sleep(time);
Stop();}
VOID stop() {}
create_drive(0,Straight);}
VOID back() {}
create_drive(back_s,Straight);}
ongeldig ready() {}
set_servo_position (arm, a_down);}
ongeldig check() {}
set_servo_position (cam, track_c);}
ongeldig far() {}
set_servo_position (cam, track_f);}
ongeldig ledge() {}
set_servo_position (arm, gevangen);}
ongeldig throw() {}
int;
voor (een = 50; een > = 30; een-= 1) {//get klaar
set_servo_position(arm, a);}
set_servo_position (arm, schieten);}
ongeldig grab() {}
int;
voor (een = 0; een < = houden; een += 1) {//raise de arm soepel
set_servo_position(arm, a) ;}}
ongeldig down() {}
set_servo_position (cam, track_d);}
ongeldig find() {}
set_servo_position (cam, track_find);}
ongeldig start_a() {}
pida = start_process(avoid());}
ongeldig start_b() {}
pidb = start_process(track_ball());}
ongeldig start_c() {}
pidc = start_process(find_basket());}
VOID kill(int pid) {}
CREATE_BUSY; //wait voor huidige maken proces te voltooien, en voorrang
kill_process(PID);
CREATE_FREE; //i'm gedaan
Stop();}
ongeldig avoid_left() {}
Kill (pidb), //stop alles
Kill(pidc);
richel (); //pick omhoog klauw dus het niet op tafel gepakt doet
back_away (); //back weg
rotate_l (); //rotate uit de buurt van obstakel
klaar (); //put klauw terug naar beneden
if(have_ball) {//if heb ik de bal
start_c();} Start doel tracking
anders if(!have_ball) {//if ik heb niet de bal
start_b();} beginnen met het bijhouden van de bal
}
ongeldig avoid_right() {}
Kill(pidb);
Kill(pidc);
Ledge();
back_away();
rotate_r();
Ready();
if(have_ball) {}
start_c();}
anders if(!have_ball) {}
start_b();}
}
ongeldig back_left() {}
create_drive_direct(back_f,back_sl);}
ongeldig back_right() {}
create_drive_direct(back_sl,back_f);}
