/**
* @brief Search for a color. Supports IMAQ_IMAGE_HSL and IMAQ_IMAGE_RGB.
* @param mode Color mode, either IMAQ_HSL or IMAQ_RGB
* @param plane1Range The range for the first plane (hue or red)
* @param plane2Range The range for the second plane (saturation or green)
* @param plane3Range The range for the third plane (luminance or blue)
* @param trackReport Values for tracking: center of particle, particle size, etc
* @param colorReport Color charactaristics of the particle
* @return 0 = error
*/
int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range,
const Range* plane3Range, ParticleAnalysisReport *trackReport,
ColorReport *colorReport)
{
return FindColor(mode, plane1Range, plane2Range, plane3Range, trackReport,
NULL, IMAQ_NO_RECT);
}
UINT32 cgfx_SetBackgroundColor(CoreGfxBase *CoreGfxBase, CRastPort *rp, UINT32 bg)
{
UINT32 oldbg = rp->crp_Background;
rp->crp_Background = FindColor(rp, bg);
rp->crp_BackgroundRGB = bg;
return oldbg;
}
UINT32 cgfx_SetForegroundColor(CoreGfxBase *CoreGfxBase, CRastPort *rp, UINT32 fg)
{
UINT32 oldfg = rp->crp_Foreground;
rp->crp_Foreground = FindColor(rp, fg);
rp->crp_ForegroundRGB = fg;
// DPrintF("Findcolor: Old: %x, Found: %x\n", fg, rp->crp_Foreground);
return oldfg;
}
int CXTPSyntaxEditColorComboBox::DeleteColor(COLORREF crColor)
{
int iIndex = FindColor(crColor);
if (iIndex != CB_ERR)
{
return DeleteString(iIndex);
}
return CB_ERR;
}
/**
* @brief Search for a color. Supports IMAQ_IMAGE_HSL.
* @param hueRange The range for the first plane
* @param minSaturation The lower range saturation
* @param trackReport Values for tracking: center of particle, particle size, color
* @return 0 = error
*/
int FindColor(const Range* hueRange, int minSaturation, ParticleAnalysisReport *trackReport)
{
Range satRange;
satRange.minValue = minSaturation;
satRange.maxValue = 255;
Range lumRange;
lumRange.minValue = 0;
lumRange.maxValue = 255;
ColorMode cmode = IMAQ_HSL;
return FindColor(cmode, hueRange, &satRange, &lumRange, trackReport);
}
/*
=================
Cmd_Environment
=================
*/
void Cmd_Environment( void ){
char name[1024];
int i, x, y;
byte image[256 * 256];
byte *tga;
GetToken( qfalse );
if ( g_release ) {
for ( i = 0 ; i < 6 ; i++ )
{
sprintf( name, "env/%s%s.pcx", token, suf[i] );
ReleaseFile( name );
sprintf( name, "env/%s%s.tga", token, suf[i] );
ReleaseFile( name );
}
return;
}
// get the palette
BuildPalmap();
sprintf( name, "%senv/", gamedir );
CreatePath( name );
// convert the images
for ( i = 0 ; i < 6 ; i++ )
{
sprintf( name, "%senv/%s%s.tga", gamedir, token, suf[i] );
printf( "loading %s...\n", name );
LoadTGA( name, &tga, NULL, NULL );
for ( y = 0 ; y < 256 ; y++ )
{
for ( x = 0 ; x < 256 ; x++ )
{
image[y * 256 + x] = FindColor( tga[( y * 256 + x ) * 4 + 0],tga[( y * 256 + x ) * 4 + 1],tga[( y * 256 + x ) * 4 + 2] );
}
}
free( tga );
sprintf( name, "%senv/%s%s.pcx", writedir, token, suf[i] );
if ( FileTime( name ) != -1 ) {
printf( "%s already exists, not overwriting.\n", name );
}
else{
WritePCXfile( name, image, 256, 256, colormap_palette );
}
}
}
int CXTPSyntaxEditColorComboBox::SetSelColor(COLORREF crColor)
{
int iIndex = FindColor(crColor);
if (iIndex == CB_ERR)
{
iIndex = SetUserColor(crColor);
if (iIndex == CB_ERR)
{
return CB_ERR;
}
}
return SetCurSel(iIndex);
}
/*
=============
AveragePixels
=============
*/
byte AveragePixels (int count)
{
int r,g,b;
int i;
int vis;
int pix;
int bestcolor;
byte *pal;
int fullbright;
vis = 0;
r = g = b = 0;
fullbright = 0;
for (i=0 ; i<count ; i++)
{
pix = pixdata[i];
r += lbmpalette[pix*3];
g += lbmpalette[pix*3+1];
b += lbmpalette[pix*3+2];
vis++;
}
r /= vis;
g /= vis;
b /= vis;
// error diffusion
r += d_red;
g += d_green;
b += d_blue;
//
// find the best color
//
bestcolor = FindColor (r, g, b);
// error diffusion
pal = colormap_palette + bestcolor*3;
d_red = r - (int)pal[0];
d_green = g - (int)pal[1];
d_blue = b - (int)pal[2];
return bestcolor;
}
int CXTPSyntaxEditColorComboBox::SelectUserColor()
{
CColorDialog dlg(GetSelColor(), CC_RGBINIT, this);
if (dlg.DoModal() == IDOK)
{
COLORREF crUser = dlg.GetColor();
if (FindColor(crUser) == CB_ERR)
{
SetUserColor(crUser);
}
else
{
SetUserColor(COLORREF_NULL);
}
return SetSelColor(crUser);
}
return CB_ERR;
}
void CColorCombo::SelectColor(COLORREF Color, bool bCustomColorsOnly, bool bSetCurSel)
{
if (Color == CLR_NONE)
{
SetCurSel(-1);
return;
}
m_Color = Color;
if (!::IsWindow(m_hWnd))
return;
// Find a matching color in the combobox
int nItem = FindColor(Color, bCustomColorsOnly);
// If the color is not found, add it to the shared entries
if (nItem < 0)
nItem = AddColorUserEntry(Color);
// If necessary, select the found or added item
if (bSetCurSel && nItem >= 0)
SetCurSel(nItem);
}
/**
* @brief Search for a color. Supports IMAQ_IMAGE_HSL.
* @param color Definition for the hue range
* @param trackReport Values for tracking: center of particle, particle size, color
* @return 0 = error
*/
int FindColor(FrcHue color, ParticleAnalysisReport* trackReport)
{
int success = 0; // return: 0 = error
/* track color */
// use ACTIVE_LIGHT or WHITE_LIGHT for brightly lit objects
TrackingThreshold td = GetTrackingData(color, PASSIVE_LIGHT);
success = FindColor(IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, trackReport);
if ( !success ) {
DPRINTF (LOG_INFO, "did not find color - errorCode= %i",GetLastVisionError());
return success;
}
//PrintReport(par);
/* set an image quality restriction */
if (trackReport->particleToImagePercent < PARTICLE_TO_IMAGE_PERCENT) {
imaqSetError(ERR_PARTICLE_TOO_SMALL, __FUNCTION__);
success = 0;
}
return success;
}
Status
XParseColor(
Display *display,
Colormap map,
const char *spec,
XColor *colorPtr)
{
if (spec[0] == '#') {
char fmt[16];
int i, red, green, blue;
if ((i = (int) strlen(spec+1))%3) {
return 0;
}
i /= 3;
sprintf(fmt, "%%%dx%%%dx%%%dx", i, i, i);
if (sscanf(spec+1, fmt, &red, &green, &blue) != 3) {
return 0;
}
colorPtr->red = (((unsigned short) red) << (4 * (4 - i)))
| ((unsigned short) red);
colorPtr->green = (((unsigned short) green) << (4 * (4 - i)))
| ((unsigned short) green);
colorPtr->blue = (((unsigned short) blue) << (4 * (4 - i)))
| ((unsigned short) blue);
} else {
if (!FindColor(spec, colorPtr)) {
return 0;
}
}
colorPtr->pixel = TkpGetPixel(colorPtr);
colorPtr->flags = DoRed|DoGreen|DoBlue;
colorPtr->pad = 0;
return 1;
}
/**
* unchanged from SimpleDemo:
*
* Runs the motors under driver control with either tank or arcade
* steering selected by a jumper in DS Digin 0.
*
* added for vision:
*
* Adjusts the servo gimbal based on the color tracked. Driving the
* robot or operating an arm based on color input from gimbal-mounted
* camera is currently left as an exercise for the teams.
*/
void OperatorControl(void)
{
char funcName[] = "OperatorControl";
DPRINTF(LOG_DEBUG, "OperatorControl");
//GetWatchdog().Feed();
TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT);
/* for controlling loop execution time */
float loopTime = 0.05;
double currentTime = GetTime();
double lastTime = currentTime;
double savedImageTimestamp = 0.0;
bool foundColor = false;
bool staleImage = false;
while (IsOperatorControl())
{
setServoPositions(rightStick->GetX(), rightStick->GetY());
/* calculate gimbal position based on color found */
if (FindColor
(IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, &par,
&cReport))
{
foundColor = true;
if (par.imageTimestamp == savedImageTimestamp)
{
// This image has been processed already,
// so don't do anything for this loop
staleImage = true;
}
else
{
staleImage = false;
savedImageTimestamp = par.imageTimestamp;
// compute final H & V destination
horizontalDestination = par.center_mass_x_normalized;
verticalDestination = par.center_mass_y_normalized;
}
}
else
{
foundColor = false;
}
PrintReport(&cReport);
if (!staleImage)
{
if (foundColor)
{
/* Move the servo a bit each loop toward the
* destination. Alternative ways to task servos are
* to move immediately vs. incrementally toward the
* final destination. Incremental method reduces the
* need for calibration of the servo movement while
* moving toward the target.
*/
ShowActivity
("** %s found: Servo: x: %f y: %f",
td.name, horizontalDestination, verticalDestination);
}
else
{
ShowActivity("** %s not found", td.name);
}
}
dashboardData.UpdateAndSend();
// sleep to keep loop at constant rate
// elapsed time can vary significantly due to debug printout
currentTime = GetTime();
lastTime = currentTime;
if (loopTime > ElapsedTime(lastTime))
{
Wait(loopTime - ElapsedTime(lastTime));
}
}
while (IsOperatorControl())
{
// determine if tank or arcade mode; default with no jumper is
// for tank drive
if (ds->GetDigitalIn(ARCADE_MODE) == 0)
{
// drive with tank style
myRobot->TankDrive(leftStick, rightStick);
}
else
{
// drive with arcade style (use right stick)
myRobot->ArcadeDrive(rightStick);
}
}
} // end operator control
void Autonomous(void)
{
char funcName[] = "Autonomous";
DPRINTF(LOG_DEBUG, "start VisionDemo autonomous");
//GetWatchdog().Feed();
// image data for tracking
ColorMode mode = IMAQ_HSL; // RGB or HSL
// TrackingThreshold td = GetTrackingData(RED, FLUORESCENT);
TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT);
int panIncrement = 0; // pan needs a 1-up number for each call
DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ", td.name);
/* initialize position and destination variables
* position settings range from -1 to 1
* setServoPositions is a wrapper that handles the conversion to
* range for servo
*/
horizontalDestination = 0.0; // final destination range -1.0 to +1.0
verticalDestination = 0.0;
// current position range -1.0 to +1.0
horizontalPosition = RangeToNormalized(horizontalServo->Get(), 1);
verticalPosition = RangeToNormalized(verticalServo->Get(), 1);
// incremental tasking toward dest (-1.0 to 1.0)
float incrementH, incrementV;
// set servos to start at center position
setServoPositions(horizontalDestination, verticalDestination);
/* for controlling loop execution time */
float loopTime = 0.05;
double currentTime = GetTime();
double lastTime = currentTime;
double savedImageTimestamp = 0.0;
bool foundColor = false;
bool staleImage = false;
while (IsAutonomous())
{
/* calculate gimbal position based on color found */
if (FindColor
(mode, &td.hue, &td.saturation, &td.luminance, &par,
&cReport))
{
foundColor = true;
panIncrement = 0; // reset pan
if (par.imageTimestamp == savedImageTimestamp)
{
// This image has been processed already,
// so don't do anything for this loop
staleImage = true;
}
else
{
staleImage = false;
savedImageTimestamp = par.imageTimestamp;
// compute final H & V destination
horizontalDestination = par.center_mass_x_normalized;
verticalDestination = par.center_mass_y_normalized;
}
// ShowActivity("Found color ");
}
else
{ // need to pan
foundColor = false;
// ShowActivity("No color found");
}
PrintReport(&cReport);
if (foundColor && !staleImage)
{
/* Move the servo a bit each loop toward the destination.
* Alternative ways to task servos are to move immediately
* vs. incrementally toward the final
* destination. Incremental method reduces the need for
* calibration of the servo movement while moving toward
* the target.
*/
incrementH = horizontalDestination - horizontalPosition;
incrementV = verticalPosition - verticalDestination;
adjustServoPositions(incrementH, -incrementV);
ShowActivity
("** %s found: Servo: x: %f y: %f increment: %f y: %f ",
td.name, horizontalDestination, verticalDestination,
incrementH, incrementV);
}
else if (!staleImage)
{
/* pan to find color after a short wait to settle servos
* panning must start directly after panInit or timing
* will be off
*/
// adjust sine wave for panning based on last movement
// direction
if (horizontalDestination > 0.0)
{
sinStart = PI / 2.0;
}
else
{
sinStart = -PI / 2.0;
}
if (panIncrement == 3)
{
panInit();
}
else if (panIncrement > 3)
{
panForTarget(horizontalServo, sinStart);
/* Vertical action: center the vertical after several
* loops searching */
if (panIncrement == 20)
{
verticalServo->Set(0.5);
}
}
panIncrement++;
} // end if found color
dashboardData.UpdateAndSend();
// sleep to keep loop at constant rate
// elapsed time can vary significantly due to debug printout
currentTime = GetTime();
lastTime = currentTime;
if (loopTime > ElapsedTime(lastTime))
{
Wait(loopTime - ElapsedTime(lastTime));
}
} // end while
myRobot->Drive(0.0, 0.0); // stop robot
DPRINTF(LOG_DEBUG, "end autonomous");
ShowActivity
("Autonomous end ");
} // end autonomous
LONG Blank( PrefObject *Prefs )
{
struct Screen *LScr;
struct Window *Wnd;
LONG RetVal = OK,i,goatFlag = 0, ToFrontCount = 0;
if (Prefs[SCREEN].po_Active )
LScr = cloneTopScreen( FALSE, TRUE );
else
LScr = OpenScreenTags( 0l, SA_DisplayID, Prefs[MODE].po_ModeID,
SA_Depth, 2, SA_Overscan, OSCAN_STANDARD,
SA_ShowTitle, FALSE, SA_Title, "Garshnescreen",
SA_Quiet, TRUE, SA_Behind, TRUE, TAG_DONE );
if( LScr )
{
if( GarshnelibBase->lib_Version < 39 || !Prefs[SCREEN].po_Active )
{
SetRGB4(&(LScr->ViewPort),0,0x0,0x0,0x0);
SetRGB4(&(LScr->ViewPort),1,0x8,0x8,0x8);
SetRGB4(&(LScr->ViewPort),2,0x7,0x4,0x2);
SetRGB4(&(LScr->ViewPort),3,0x0,0xa,0x0);
backgroundClr = 0;
grassClr = GRASS;
herderClr = HERDER;
goatClr = GOAT;
}
else
{
backgroundClr = FindColor( LScr->ViewPort.ColorMap, 0, 0, 0, -1 );
grassClr = FindColor( LScr->ViewPort.ColorMap, 0, 0x9L<<28, 0,
-1 );
goatClr = FindColor( LScr->ViewPort.ColorMap, 0xAL<<28, 0xAL<<28,
0xAL<<28, -1 );
herderClr = FindColor( LScr->ViewPort.ColorMap, 0xBL<<28, 0x2L<<28,
0x4L<<28, -1 );
}
numGoats = Prefs[GOATS].po_Level;
numHerders = Prefs[HERDERS].po_Level;
reproduction = Prefs[REPRO].po_Level;
Width = LScr->Width;
Height = LScr->Height;
for (i=0;i<numHerders;++i)
herderQ[i] = 0;
herderQ[0] = 1;
herders[0].x = Width/2;
herders[0].y = Height/2;
for (i=0;i<numGoats;++i)
goatQ[i] = 0;
Wnd = BlankMousePointer( LScr );
ScreenToFront( LScr );
i = 0;
while( RetVal == OK )
{
WaitTOF();
if(!( ++ToFrontCount % 60 ))
ScreenToFront( LScr );
if( !Prefs[DELAY].po_Level ||
!( ToFrontCount % Prefs[DELAY].po_Level ))
{
myBlank( LScr, LScr->Width, LScr->Height );
if (!goatFlag && i == 20)
{
goatQ[0] = 1;
goats[0].x = Width/2;
goats[0].y = Height/2;
grassEaten[0] = 0;
goatFlag = 1;
}
else
++i;
}
RetVal = ContinueBlanking();
}
UnblankMousePointer( Wnd );
CloseScreen( LScr );
}
else
RetVal = FAILED;
return RetVal;
}
void CViewColorsDlg::OnColorBtn(UINT nID)
{
int iColor = FindColor(nID);
ASSERT(iColor >= 0); // else logic error
EditColor(iColor);
}
RBitmapImage& RWinColorPalette::GetPaletteBitmapImage( )
{
static BOOLEAN m_fPaletteInitialized = FALSE;
static RBitmapImage m_biPalette;
if ( !m_fPaletteInitialized )
{
// find the resource in the resource file
HRSRC hRsrc = FindResource( AfxGetResourceHandle(),
MAKEINTRESOURCE( m_uPaletteBitmapID ), RT_BITMAP );
if ( hRsrc != NULL )
{
// get a handle to the resource data
HGLOBAL hTemp = LoadResource( AfxGetResourceHandle(), hRsrc );
if ( hTemp != NULL )
{
// Initlize the palette bitmap with the resource data
m_biPalette.Initialize( LockResource( hTemp ) );
// unlock and free the resource
UnlockResource( hTemp );
FreeResource( hTemp );
}
else
AfxThrowResourceException( );
}
else
AfxThrowResourceException( );
m_fPaletteInitialized = TRUE;
COLORMAP crColorMap[] =
{
{ RGB( 255, 255, 255 ), GetSysColor( COLOR_BTNHIGHLIGHT ) },
{ RGB( 192, 192, 192 ), GetSysColor( COLOR_BTNFACE ) },
{ RGB( 128, 128, 128 ), GetSysColor( COLOR_BTNSHADOW ) }
};
RIntPoint ptCells[] =
{
FindColor( crColorMap[0].from ),
FindColor( crColorMap[1].from ),
FindColor( crColorMap[2].from )
};
void* pRawData = m_biPalette.GetRawData();
RGBQUAD* pColorData = (RGBQUAD *) RBitmapImage::GetColorData( pRawData );
LPBYTE pImageData = (LPBYTE) RBitmapImage::GetImageData( pRawData );
for (int j = 0; (LPVOID) pColorData < (LPVOID) pImageData; j++, pColorData++)
{
for (int i = 0; i < NumElements( crColorMap ); i++)
{
if (crColorMap[i].from == RGB( pColorData->rgbRed, pColorData->rgbGreen, pColorData->rgbBlue ))
{
pColorData->rgbBlue = GetBValue( crColorMap[i].to );
pColorData->rgbRed = GetRValue( crColorMap[i].to );
pColorData->rgbGreen = GetGValue( crColorMap[i].to );
pColorData->rgbReserved = 0;
}
}
if (j == 9)
{
// We only need to look at the system colors, so
// jump to the last 10 entries in the palette.
pColorData += 235;
}
}
m_biPalette.UpdatePalette();
//
// Relace the cells that got remapped
//
ROffscreenDrawingSurface dsMem;
dsMem.SetImage( &m_biPalette );
RSolidColor rSolid;
for (int i = 0; i < NumElements( ptCells ); i++)
{
if (ptCells[i].m_x >= 0 && ptCells[i].m_y >= 0)
{
RIntRect rcCell( RIntSize( kCellSize.m_dx - 2, kCellSize.m_dy - 2 ) );
rcCell.Offset( RIntSize( ptCells[i].m_x + 1, ptCells[i].m_y + 1 ) );
rSolid = crColorMap[i].from;
RColor rColor( rSolid );
dsMem.SetFillColor( rColor );
dsMem.FillRectangle( rcCell );
}
}
dsMem.ReleaseImage();
}
return m_biPalette;
}
/**
* @brief Search for a color. Supports IMAQ_IMAGE_HSL.
* @param hueRange The range for the first plane
* @param trackReport Values for tracking: center of particle, particle size, color
* @return 0 = error
*/
int FindColor(const Range* hueRange, ParticleAnalysisReport *trackReport)
{ return FindColor(hueRange, DEFAULT_SATURATION_THRESHOLD, trackReport); }
