int ColorBox::colorIndex(const QColor& c)
{
if (!isValidColor(c))
return 0;
return colorList().indexOf(c);
}
// Main loop that executes program for robot
task main() {
initialize();
#ifdef DEBUG
StartTask(displaySONAR); // acquire and display sensor readings from a different thread
#endif
int dist = detectBox();
bool firstSpinUp = true;
int col = getColor();
while (isValidColor(col)) {
int power = calcWheelPower(dist, col);
motor[WHEELS] = -power;
if (firstSpinUp) {
wait1Msec(5000);
firstSpinUp = false;
} else
wait1Msec(3000);
feed();
wait1Msec(1000); // Pause so next ball can get aligned
col = getColor();
}
#ifdef DEBUG
nxtDisplayString(5, "TERMINATED");
#endif
terminate();
}
int ColorBox::colorIndex(const QColor& c)
{
if (!isValidColor(c))
return 0;
const QColor *ite = std::find(colors, colors + sizeof(colors), c);
return (ite - colors);
}
// This function receives the image, row/col index and color symbol
// it will return true if the element is to be dilated
bool isToDilate(const std::vector<std::string> & image,
int row,
int col,
char color,
bool square) {
if (image[row][col] == color) {
return false;
}
std::vector<int> dx; // the inrement for row index
std::vector<int> dy; // the increment for col index
getStructureElementsModel(square, dx, dy);
// We iteratively check it's neighboring directions and check
// if the element has color around it.
for (unsigned int i = 0; i < dx.size() ; i++) {
int nrow = row + dx[i];
int ncol = col + dy[i];
if (isValidColor(image, nrow, ncol, color)) {
return true;
}
}
return false;
}
