Page History

h3. 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}
#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)
  {}
}
{code}

h3.