Hur man gör en autonom basket spela robot med en iRobot skapa som bas (5 / 7 steg)

Steg 5: kod

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

#define cam 0 / / kamera servo port
#define arm 3 / / armbåge servo port
#define et (analog(0)) //et port

/ * Skapa kabeln måste också vara inkopplad. Ström-uttaget
3 delar in i port 8 och en märkta U X i JP 28
(bredvid USB-porten) med U mot kameran * /

#define c_down 5 / / kamera servo ner
#define a_down 17 / / armbåge servo ner
#define hålla 50 / / servo håll bollen
#define fångade 27 / / armbåge servo position att hålla från att fastna på bord
#define skjuta 150 / / servo kasta bollen
#define track_c 25 / / kamera servo spår nära position
#define track_f 45 / / kamera servo spår långt position

#define center 120 / / center kamera vision
#define inrange 30 / / track_y samordna när bollen är i klo
#define bollen 0 / / kanal av orange boll
#define ball_x (track_x(ball,0)) //x koordinaten för bollen
#define ball_y (track_y(ball,0)) //y koordinaten för bollen

#define långsam 100 / / hastighet av långsam motor
#define snabb 175 / / hastighet av snabb motor

#define klart 0.2//sleep att backa från hinder
#define tid 0,5 //1.0 är en 90 graders högersväng
#define resten 0.05//time att sova vid spårning av blobbar

#define speeda 175 / / hastighet av undviker slå
#define back_s -200 / / hastighet för att backa från stötte objekt
#define raka 32767 / / enhet i en rak linje

#define backb 2 / / kanal av ryggstöd huvudsakliga färg
#define torget 1 / / kanal av ryggstöd accentfärg
#define track_d 250 / / kamera läge för att spåra målet
#define track_find 70 / / kameraposition för länge tracking
#define omvänd 2.25//sleep tid för en 180
#define back_f -150 / / tillbaka snabb hastighet
#define back_sl-125 / / rygg låg hastighet
#define center_x 178 / / true x center cam
#define center_y 146 / / true y centrera av cam

int pida; //avoid process
int pidb; //track process
int pidc; //score process
int have_ball = 0; //tells vilken funktion som vi är i

void main () {
lång ch;
enable_servos (); //enable servon
init_camera (); //start kamera
Anslut (), //connect att skapa med full controll
start_a (); //start undvika funktion
start_b (); //start ball_tracking funktion
While(1) {
om (r_button () || gc_ldrop|| gc_rdrop) {//if plockade upp eller r axeln knappen
Kill(PIDA);
Kill(pidb);
Kill(pidc);
disable_servos();
Disconnect();
Break;}
create_battery_charge();
display_clear();
printf ("avgift = %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());
medan (gc_battery_charge < 2800l & &! b_button()) {
create_battery_charge();
display_clear();
printf ("avgift = %l\n", gc_battery_charge);
Sleep(1.0);}
cconnect();
Back();
Sleep(2.0);
start_a();
start_b();}
}

}

void avoid() {

While(1) {//repeat forever

create_sensor_update (); //update alla sensor värden
create_drive (speeda, raka);
IF(gc_lbump==1) {//left bula
avoid_right();} vänder rätt att undvika
annat if(gc_rbump==1) {//right bula
avoid_left();} vänder vänster att undvika
annat if(gc_lfcliff==1) {//left främre klippan
avoid_right();}
annat if(gc_rfcliff==1) {//right främre klippan
avoid_left();}
annat if(gc_lcliff==1) {//left klippan
avoid_right();}
annat if(gc_rcliff==1) {//right klippan
avoid_left();}
}
}

void track_ball() {

Kill(pidc);

While(!have_ball) {//repeat till få bollen

track_update();
(långt); //sets kameran
(redo); //sets arm

While(et<255) {//until max värde händer när bollen fångas

track_update (); //update kamera bild

om (ball_x < =(center-5)) {//if bollen är kvar
track_update();
create_drive_direct (långsam, snabb); //turn vänster
Sleep(rest);}

annars om (ball_x > =(center+5)) {//if bollen är rätt
track_update();
create_drive_direct(fast,Slow); / / sväng höger
Sleep(rest);}

annars om (ball_x <(center+5) & & ball_x >(center-5)) {/ / om bollen är centrerad
track_update();
create_drive_straight (snabb); //go rak
Sleep(rest);}
}
Grab (); //grab bollen
(PIP), //make buller
Stop (); //stop körning
have_ball = 1; //make en anteckning att jag har bollen
}
start_c (); //find korgen
sömn (1.0); //sleep så att jag inte gör något när jag bli dödad
}

void find_basket() {

döda (pidb), //kill ball spårning process
hitta (); //put kamera upp
track_set_minarea (1000); //the ryggstöd är stor, så att bara leta efter stora blobbar

samtidigt (have_ball) {//while jag har bollen

track_update();
While(track_x(backb,0) < =(center_x-20) || track_x(backb,0) > =(center_x+20)) {//while inte centrerad
track_update();

IF(track_x(backb,0) > =(center_x+20)) {//if ryggstöd är kvar
track_update();
create_spin_CCW(100);} Sväng vänster

annat if(track_x(backb,0) < =(center_x-20)) {//if ryggstöd är rätt
track_update();
create_spin_CW(300-center_x);} sväng höger sakta som center närmar sig
}
Stop();

While(track_size(backb,0) < =(6000)) {//while målet är mindre än 6000 pixlar i storlek
track_update();
IF(track_x(backb,0) < =(center_x-5)) {//if målet är kvar
track_update();
create_drive_direct (långsam, snabb); //turn vänster
Sleep(rest);}

annat if(track_x(backb,0) > =(center_x+5)) {//if målet är rätt
track_update();
create_drive_direct(fast,Slow); / / sväng höger
Sleep(rest);}

annat if(track_x(backb,0) <(center+5) & & track_x(backb,0) >(center_x-5)) {/ / om målet är centrerad
track_update();
create_drive_straight (snabb); //go rak
Sleep(rest);}
}
Stop();
create_drive_straight(fast); / / få lite närmare
Sleep(1.0);
Stop();
Sleep(1.0);
create_spin_CW (speeda); //spin höger
sömn (bakåt), //sleep lång nog för en 180 sväng
Stop();
ner (); //put kamera ner till spår ryggstöd
Sleep(1.0);
track_set_minarea (200); //use en mindre min storlek, eftersom vi pekade på det och ska komma närmare

While(track_y(backb,0) > =(center_y-140)) {//while målet är mindre än y-koordinaten
track_update();
IF(track_x(backb,0) < =(center_x-5)) {//if målet är kvar
track_update();
back_right (); //turn vänster
Sleep(rest);}

annat if(track_x(backb,0) > =(center_x+5)) {//if målet är rätt
track_update();
back_left(); / / sväng höger
Sleep(rest);}

annat if(track_x(backb,0) <(center+5) & & track_x(backb,0) >(center_x-5)) {/ / om målet är centrerad
track_update();
(tillbaka); //go rak
Sleep(rest);}

}
Stop();
beep();
kasta (), //shoot
Sleep(1.0);
have_ball = 0; //reminder jag kastade bollen och inte har det
}
start_b (); //back på bollen uppföljning
sömn (1.0); //don't göra något tills denna process dör
}

void cconnect() {
create_connect();
create_full (); //for full controll avsats sensorer
create_power_led(0,255);} Green power leds

void disconnect() {
Stop (); //stop flytta
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 (arm, a_down);}

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

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

void ledge() {
set_servo_position (arm, fångade);}

void throw() {
int a;
för (en = 50; en > = 30; en-= 1) {//get redo
set_servo_position(arm, a);}
set_servo_position (arm, skjuta);}

void grab() {
int a;
för (en = 0; en < = håll; en += 1) {//raise armen smidigt
set_servo_position(arm, a) ;}}

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

void find() {
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 för aktuella skapa processen till slut och prioriteras
kill_process(PID);
CREATE_FREE; //i'm gjort
Stop();}

void avoid_left() {
döda (pidb), //stop allt annat
Kill(pidc);
Ledge (); //pick upp klo så det inte är fastklämd på bordet
back_away (); //back bort
rotate_l (); //rotate från hinder
(redo); //put klo tillbaka ner
IF(have_ball) {//if jag har bollen
start_c();} börja målet följa
annat if(!have_ball) {//if jag inte har bollen
start_b();} Starta bollen spårning
}

void avoid_right() {
Kill(pidb);
Kill(pidc);
Ledge();
back_away();
rotate_r();
Ready();
IF(have_ball) {
start_c();}
annat 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);}