/*
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);
}
}
/*
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);
}
}
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);
}
}
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;
}
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;
}