Vision Subsystem v4 for NXT (NXTCam v4)

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