Ampujarobotti
Alla oleva esimerkkikoodi hyödyntää Vision Subsystem v4 for NXT- kameraa ja sitä varten rakennettiin varta vasten koulun perusroboteista poikkeava robotti. Robottissa ei käytetä muita sensoreita ja se sisältää pallonampujan.
...
Toiminta: Robotti pyörii aluksi ympäri, kunnes se havaitsee halutun värisen kohteen, tässä tapauksessa punaisen kohteen, ja alkaa tämän jälkeen lähestymään kohdetta. Kun robotti havaitsee kohteen määritellyn arvon korkuisena, se kääntyy hetken aikaa vasemmalle, tähdäten ampujan kohti kohdetta, ja ampuu. Tämän jälkeen robotti alkaa jälleen pyörimään ja etsimään uutta kohdetta ja toistaa samat operaatiot kuin edellä. Kohteiden määrä on määritelty kolmeen kappaleeseen tässä esimerkissä.
HUOM! Kamera saattaa sammua, mikäli ohjelma pysäytetään kesken ajon (=kameran punainen valo sammuu), jolloin ohjelmaa uudelleen ajettaessa robotti vain pyörii ympyrää tunnistamatta mitään. Esim. kun robotti on ampunut kerran ja se sammutetaan sen jälkeen. Mikäli näin tapahtuu, kytke kameran piuha irti ja takaisin, jolloin punainen valo syttyy, jonka jälkeen voit kokeilla ohjelmaa uudelleen.
Koodinpätkän alapuolelta löytyy ohjetta NXTCamView-ohjelman käyttöönotosta ja linkki, josta saa ohjeita sen käytöstä. Ohjelmalla voidaan määritellä kameran seuraamat väriskaalat.
| Code Block |
|---|
#pragma config(Sensor, S4, Kamera, sensorI2CCustomFastSkipStates)
#pragma config(Motor, motorA, Oikea, tmotorNormal, PIDControl, encoder)
#pragma config(Motor, motorB, Vasen, tmotorNormal, PIDControl, encoder)
#pragma config(Motor, motorC, Ampuja, tmotorNormal, PIDControl, encoder)
#define KohteidenMaara 3 // Kohteiden lukumaara
#define NOPEUS 20 // Etenemisnopeus
#define NOPEUS2 100 // Tykin nopeus
#define ResX 88 // NXTcamin resoluutio (Loydettavissa kameran dokumentaatiosta)
#define ResY 144
#define KpX 0.3
#define KpH 0.8
#define KohteenKorkeus 130 // Kohteen haluttu korkeus
#define Odotusaika 75
#include "rdpartyrobotcdr-v2.6/drivers/common.h" // Sisallyttaa ajuripaketista ajurit koodiin
#include "rdpartyrobotcdr-v2.6/drivers/NXTCAM-driver.h"
void Pysayta(); // Pysayta kaikki moottorit
void Etsi(); // Etsi kohde
void Ammu(); // Ammu pallo
task main()
{
blob_array blobs;
memset(blobs, 0, sizeof(blob_array));
int nblobs = 0, errX = 0, errH = 0, BlobN = 0, TargetNum = 0;
eraseDisplay(); //Tyhjentaa robotin ruudun
Pysayta();
if(NXTCAMinit(Kamera))
{
PlaySound(soundUpwardTones); //Mikali kamera kaynnistyy oikein, tulee soundUpwardTones aanimerkki
while(bSoundActive);
}
else
{
PlaySound(soundDownwardTones); //Mikali kamera ei kaynnisty oikein, tulee soundDownwardTones aanimerkki
while(bSoundActive);
StopAllTasks(); //Pysayttaa kaikki toiminnot
}
wait1Msec(100);
while(true)
{
eraseDisplay(); //Tyhjentaa robotin ruudun
nblobs = NXTCAMgetBlobs(Kamera, blobs, true);
nxtDisplayCenteredTextLine(1, "blobs: %d", nblobs);
nxtDisplayCenteredTextLine(3, "Targets Down: %d", TargetNum);
if(nblobs > 0)
{
BlobN = -1;
for(int i = 0; i < nblobs; i++) // Seuraa vain kohteita, joiden Korkeus > Leveys (ei "jahtaa" kaatuneita kohteita)
{
if((blobs[i].x2 - blobs[i].x1) < (blobs[i].y2 - blobs[i].y1))
{
BlobN = i;
break;
}
}
if(BlobN != -1)
{
errX = (blobs[BlobN].x1 + blobs[BlobN].x2) / 2 - ResX/2; //X-suunnassa blobsin vas. reunan arvo + oik. reunan
//arvo jaettuna 2 - X:n resoluutio jaettuna 2
errH = KohteenKorkeus - (blobs[BlobN].y2 - blobs[BlobN].y1); //Kohteen korkeus - (y-suunnassa blobsin maksimi - minimi)
if(abs(errX) > 4) //Pyorimisnopeus etsiessa
{
motor[Vasen] = errX * KpX;
motor[Oikea] = -errX * KpX;
}
else if(abs(errH) > 6)
{
motor[Vasen] = errH * KpH;
motor[Oikea] = errH * KpH;
}
else
{
Pysayta();
Ammu();
Pysayta();
TargetNum++;
if(TargetNum == KohteidenMaara) //Jos kohteiden maara on yhta kuin asetettu kohteiden tavoitemaara -> Pysayta kaikki
{
PlaySound(soundUpwardTones);
while(bSoundActive);
StopAllTasks();
}
else
Etsi();
}
}
else
{
Etsi();
}
}
else
{
Etsi();
}
wait1Msec(Odotusaika);
}
}
void Pysayta() //Pysayttaa kaikki liikkeet
{
motor[Vasen] = 0;
motor[Oikea] = 0;
motor[Ampuja] = 0;
}
void Etsi() //Etsintatila
{
blob_array blobs;
memset(blobs, 0, sizeof(blob_array));
int nblobs = 0, BlobN = -1;
for(int i = 0; i < 1750 * 4.5 / Odotusaika; i++)
{
BlobN = -1;
motor[Vasen] = NOPEUS;
motor[Oikea] = -NOPEUS;
nblobs = NXTCAMgetBlobs(Kamera, blobs, true);
nxtDisplayCenteredTextLine(1, "blobs: %d", nblobs);
if(nblobs > 0)
{
for(int j = 0; j < nblobs; j++)
{
if((blobs[j].x2 - blobs[j].x1) < (blobs[j].y2 - blobs[j].y1))
{
BlobN = j;
break;
}
}
if(BlobN != -1)
break;
}
wait1Msec(Odotusaika);
}
Pysayta();
if(BlobN == -1)
{
motor[Vasen] = NOPEUS;
motor[Oikea] = NOPEUS;
wait1Msec(2000);
}
}
void Ammu()
{
Pysayta();
motor[Vasen] = -20; //"Tahtays" eli kaantyy vasemmalle 200ms ajan
motor[Oikea] = 20;
wait1Msec(200);
Pysayta();
for(int i = 0; i < 3; i++)
{
PlayImmediateTone(440, 10);
while(bSoundActive);
wait1Msec(100);
}
Pysayta();
nMotorEncoder[Ampuja] = 0;
nMotorEncoderTarget[Ampuja] = 360;
motor[Ampuja] = NOPEUS2;
while(nMotorRunState[Ampuja] != runStateIdle);
Pysayta();
while(bSoundActive)
{}
} |
Tämän esimerkin..
| Code Block |
|---|
koodi
|
NXTCamView:n ja ajureiden asennus sekä käyttöönotto
Lego Mindstorms robotteihin liitettävän mindsensors:n Vision Subsystem v4 for NXT (NXTCam-v4)– -- kameran asetuksia ja väriskaaloja määritellään PC:lle asennettavalla erillisellä NXTCamView- ohjelmalla. Jos haluat muokata kameran kohdevärejä, lataa ohjelma koneellesi täältä (versio 0.21).
HUOM! Jos koneelle on jo asennettu NXTCamView, niin on todennäköistä, että myös ajurit on asennettu. Tällöin voit katsoa ohjeita ohjelman käytöstä suoraan kohdasta 4.
Asennusohje (Windows 7):
...