/*
 Main task
 */
task main () {
  blob_array _blobs;
  // combine all colliding blobs into one
  bool _condensed = true;

  //blob_array _blobs;
  int _l, _t, _r, _b;
  int _nblobs;
  eraseDisplay();

  // Initialise the camera
  NXTCAMinit(cam);
  while(true) {
    eraseDisplay();
    // Fetch all the blobs, have the driver combine all
    // the colliding blobs.
    _nblobs = NXTCAMgetBlobs(cam, _blobs, _condensed);
    for (int i = 0; i < _nblobs; i++) {
      // Draw the scaled blobs
      _l = xscale(_blobs[i].x1);
      _t = yscale(_blobs[i].y1);
      _r = xscale(_blobs[i].x2);
      _b = yscale(_blobs[i].y2);
      nxtFillRect(_l, _t, _r, _b);
    }

    nxtDisplayTextLine(1, "%d", _nblobs);
    wait1Msec(100);
  }
}
Ejemplo n.º 2
0
/*
 Main task
 */
task main () {
  blob_array _blobs;
  memset(_blobs, 0, sizeof(blob_array));

  // combine all colliding blobs into one
  bool _condensed = true;

  //blob_array _blobs;
  int _l, _t, _r, _b;
  int _x, _y;
  int _nblobs;
  eraseDisplay();

  // Initialise the camera
  NXTCAMinit(cam);
  while(true) {
    eraseDisplay();
    // Fetch all the blobs, have the driver combine all
    // the colliding blobs.
    _nblobs = NXTCAMgetBlobs(cam, _blobs, _condensed);
    for (int i = 0; i < _nblobs; i++) {
      // Draw the scaled blobs
      _l = xscale(_blobs[i].x1);
      _t = yscale(_blobs[i].y1);
      _r = xscale(_blobs[i].x2);
      _b = yscale(_blobs[i].y2);
      nxtDrawRect(_l, _t, _r, _b);

    }
    NXTCAMgetAverageCenter(_blobs, 3, 0, _x, _y);
    _x = xscale(_x);
    _y = yscale(_y);
    nxtEraseRect(_x-4, _y-4, _x+4, _y+4);
    nxtDrawLine(_x, _y+2, _x, _y-2);
    nxtDrawLine(_x+2, _y, _x-2, _y);
    nxtDisplayTextLine(1, "%d", _nblobs);
    wait1Msec(100);
  }
}
Ejemplo n.º 3
0
void
xscale_transpose(struct image in, struct image out)
{
	uint16_t *in_row, *out_smp;
	uint32_t i, j, taps;

	taps = out.height < in.width ? 2 * in.width / out.height : 2;
	taps += taps & 1;

	for (i=0; i<out.width; i++) {
		in_row = in.data + i * in.width * 4;
		for (j=0; j<out.height; j++) {
			out_smp = out.data + (j * out.width + i) * 4;
			xscale(in_row, out_smp, in.width, out.height, taps, j);
		}
	}
}
/*
 Main task
 */
task main () {
  blob_array _blobs;
  memset(_blobs, 0, sizeof(blob_array));

  //blob_array _blobs;
  int _x, _y, _x1, _y1;
  int _nblobs;
  eraseDisplay();

  // pid variables
  float pwr_x = 0.0;
  float err_x = 0.0;
  float perr_x = 0.0;
  float aerr_x = 0.0;
  float sp_x = 0.0;
  float p_x = 0.0;
  float i_x = 0.0;
  float d_x = 0.0;

  float pwr_y = 0.0;
  float err_y = 0.0;
  float perr_y = 0.0;
  float aerr_y = 0.0;
  float sp_y = 0.0;
  float p_y = 0.0;
  float i_y = 0.0;
  float d_y = 0.0;

  sp_y = sp_x = 87;
  p_y = p_x = 0.5;
  d_y = d_x = 1.8;
  i_y = i_x = 0.05;

  nMotorEncoder[motorA] = 0;
  nMotorEncoder[motorB] = 0;

  // Initialise the camera
  NXTCAMinit(cam);
  while(true) {
    eraseDisplay();
    // Fetch all the blobs, have the driver combine all
    // the colliding blobs.
    _nblobs = NXTCAMgetBlobs(cam, _blobs);
    nxtDisplayTextLine(1, "%d, %f", _nblobs, perr_x);

    if (_nblobs == 0) {
      if ((perr_x > 0) && (nMotorEncoder[motorB] > 660)) {
	      motor[motorA] = 0;
	      motor[motorB] = 0;
	    } else if ((perr_x < 0) && (nMotorEncoder[motorB] < -660)) {
				motor[motorA] = 0;
				motor[motorB] = 0;
			}
	  } else {
	    _x = (_blobs[0].x2 + _blobs[0].x1)/2;
	    _y = (_blobs[0].y2 + _blobs[0].y1)/2;


	    err_x = _x - sp_x;
	    aerr_x += err_x;
	    pwr_x = (err_x * p_x) + ((err_x - perr_x) * d_x) + (aerr_x * i_x);


	    err_y = _y - sp_y;
	    aerr_y += err_y;
	    pwr_y = (err_y * p_y) + ((err_y - perr_y) * d_y) + (aerr_y * i_y);


	    motor[motorA] = round(pwr_y);
      motor[motorB] = round(pwr_x);

	    _x1 = xscale(_x);
	    _y1 = yscale(_y);
	    nxtEraseRect(_x1-6, _y1-6, _x1+6, _y1+6);

	    nxtDrawLine(_x1, _y1+3, _x1, _y1-3);
	    nxtDrawLine(_x1+3, _y1, _x1-3, _y1);

	    perr_y = err_y;
	    perr_x = err_x;
	  }
	  wait1Msec(50);
  }
}
Ejemplo n.º 5
0
void readMultipleDataMsgs()
{
  TFileIOResult nBTCmdRdErrorStatus;
  int nSizeOfMessage;
  ubyte nRcvBuffer[kMaxSizeOfMessage];

  while (true)
  {
    // Check to see if a message is available

    nSizeOfMessage = cCmdMessageGetSize(kQueueID);
    if (nSizeOfMessage <= 0)
    {
      wait1Msec(1);    // Give other tasks a chance to run
      break;           // No more message this time
    }

    if (nSizeOfMessage > kMaxSizeOfMessage)
      nSizeOfMessage = kMaxSizeOfMessage;
    nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, kQueueID);





     cmps = 180+(nRcvBuffer[0]-nRcvBuffer[1]);
    int oxmps;
   if(nRcvBuffer[1] == 0){oxmps = nRcvBuffer[0];}
   if(nRcvBuffer[0] == 0){oxmps = -nRcvBuffer[1];}

     nxtDisplayTextLine(3, "button:   %4d", nRcvBuffer[2]);
     nxtDisplayTextLine(4, "Abs:   %4d", oxmps);
     nxtDisplayTextLine(5, "curr:   %4d", HTMCreadRelativeHeading(HTMC));
     if(nRcvBuffer[2]==1){
       int mspeed = 0;
       int mangle = 0;
       //stopMotors();
         blob_array _blobs;
         // combine all colliding blobs into one
         bool _condensed = true;

          //blob_array _blobs;
         int _l, _t, _r, _b;
         int _nblobs;
         eraseDisplay();

            // Initialise the camera
           NXTCAMinit(cam);
           servo[servo1] = 35;
             while(true) {
              eraseDisplay();

              _nblobs = NXTCAMgetBlobs(cam, _blobs, _condensed);
              // Fetch all the blobs, have the driver combine all
              // the colliding blobs.

              if(_nblobs>0){

                int cx = 176-(SIDE_CENTER(_blobs[0].x1, _blobs[0].x2));
                int cy = SIDE_CENTER(_blobs[0].y1, _blobs[0].y2);
                float dx = cx-88;
            /*    if(cy < C_STOP){
                mspeed = (cy/C_STOP)*25;
              }
                  else{
                    mspeed = 0;
                  }*/

                  mspeed=50;
                //  if((cx-44)>0){
                  float rathura = (dx*90)/88;
                mangle = rathura;
            //  }
            //  else{
                 //mangle = (dx/44)*-90;
              //}


                      // Draw the scaled blobs
                          _l = xscale(_blobs[0].x1);
                          _t = yscale(_blobs[0].y1);
                         _r = xscale(_blobs[0].x2);
                          _b = yscale(_blobs[0].y2);
                        nxtFillRect(_l, _t, _r, _b);


                  nxtDisplayTextLine(1, "%d", _nblobs);
                  nxtDisplayTextLine(2, "%d", mangle);
                  nxtDisplayTextLine(3, "%d", (int)dx);
                  runMotorSpeeds(mSD, mSE, mSF, mSG, mangle, mspeed);
                  _nblobs = 0;
                   wait1Msec(100);

          }
        }


       flagged = true;
     }
    // else{
    // stopMotors();
     else{
      if (abs(oxmps-HTMCreadRelativeHeading(HTMC)) > 5)
     {
     flagged = false;
    }
  }





     if(flagged == true){

      stopMotors();
    }

    else{
      turntocmps(cmps, oxmps);
    }

  // }






     //}
  }
  return;
}
Ejemplo n.º 6
0
int gfxengine_t::show()
{
	log_printf(DLOG, "Opening screen...\n");
	int flags = 0;

	if(!is_open)
		return -1;

	if(is_showing)
		return 0;

//	if(_centered && !_fullscreen)
//		SDL_putenv((char *)"SDL_VIDEO_CENTERED=1");

	if(!SDL_WasInit(SDL_INIT_VIDEO))
		if(SDL_InitSubSystem(SDL_INIT_VIDEO) == -1)
		{
			log_printf(ELOG, "Failed to initialize SDL!\n");
			return -2;
		}

	switch(_driver)
	{
	  case GFX_DRIVER_SDL2D:
		break;
	  case GFX_DRIVER_GLSDL:
		if(!_doublebuf)
		{
			log_printf(WLOG, "Only double buffering is supported"
					" with OpenGL drivers!\n");
			doublebuffer(1);
		}
		if(_shadow)
		{
			log_printf(WLOG, "Shadow buffer not supported"
					" with OpenGL drivers!\n");
			shadow(0);
		}
		break;
	}

	switch(_driver)
	{
	  case GFX_DRIVER_SDL2D:
		/* Nothing extra */
		break;
	  case GFX_DRIVER_GLSDL:
	  	flags |= SDL_GLSDL;
		break;
	}

	if(_doublebuf)
		flags |= SDL_DOUBLEBUF | SDL_HWSURFACE;
	else
	{
		if(!_shadow)
		  	flags |= SDL_HWSURFACE;
	}

	if(_fullscreen){
		flags |= SDL_WINDOW_FULLSCREEN;
	}

	glSDL_VSync(_vsync);
	flags |= xflags;
//	SDL_WM_SetCaption(_title, _icontitle);
//	screen_surface = SDL_SetVideoMode(_width, _height, _depth, flags);
    if(SDL_CreateWindowAndRenderer(0, 0, 0, &myscreen, &renderer) < 0)
        exit(2);
    sdlTexture = SDL_CreateTexture(renderer,
                                                SDL_PIXELFORMAT_ARGB8888,
                                                SDL_TEXTUREACCESS_STREAMING,
                                                _width, _height);
    screen_surface = SDL_CreateRGBSurface(0, _width, _height, 32,
            0x00FF0000,
            0x0000FF00,
            0x000000FF,
            0xFF000000);
//	myscreen = SDL_CreateWindow("SDL2 Example", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, _width, _height, flags);
//	screen_surface = SDL_GetWindowSurface(myscreen);
	if(!screen_surface)
	{
		log_printf(ELOG, "Failed to open display! %i, %i, %i\n", _width, _height, flags);
		return -3;
	}

	if(_driver != GFX_DRIVER_GLSDL)
	{
		if((screen_surface->flags & SDL_DOUBLEBUF) == SDL_DOUBLEBUF)
		{
			if(!_doublebuf)
			{
				log_printf(WLOG, "Could not get"
						" single buffered display.\n");
				doublebuffer(1);
			}
		}
		else
		{
			if(_doublebuf)
			{
				log_printf(WLOG, "Could not get"
						" double buffered display.\n");
				doublebuffer(0);
			}
		}
	}

	if((screen_surface->flags & SDL_HWSURFACE) == SDL_HWSURFACE)
	{
		if(_shadow)
		{
			softbuf = SDL_CreateRGBSurface(SDL_SWSURFACE,
					_width, _height,
					screen_surface->format->BitsPerPixel,
					screen_surface->format->Rmask,
					screen_surface->format->Gmask,
					screen_surface->format->Bmask,
					screen_surface->format->Amask);
			if(!softbuf)
			{
				log_printf(WLOG, "Failed to create shadow buffer! "
						"Trying direct rendering.\n");
				shadow(0);
			}
		}
	}
	else
	{
		if(_shadow)
			log_printf(WLOG, "Shadow buffer requested; "
					"relying on SDL's shadow buffer.\n");
		else
		{
			log_printf(WLOG, "Could not get h/w display surface.\n");
			shadow(0);	//...which means we're using SDL's shadow.
		}
	}

//	SDL_WM_SetCaption(_title, _icontitle);
	SDL_ShowCursor(_cursor);
	cs_engine_set_size(csengine, _width, _height);
	csengine->filter = use_interpolation;

	dither(_dither);
	noalpha(alpha_threshold);
	is_showing = 1;

	fullwin = new window_t;
	fullwin->init(this);
	fullwin->place(0, 0, (int)(_width / xscale()),
			(int)(_height / yscale()));

	clear();

	return 0;
}