void Target::SaveImages()
{
printf("Target::SaveImages\n");
long then = (long) GetFPGATime();
if (!imaqWriteFile(m_cameraImage.GetImaqImage(), "/tmp/0000-camera.bmp", NULL)) {
printf("%s: imaqWriteFile(\"/tmp/0000-camera.bmp\") FAILED\n", __FUNCTION__);
// ignore the error
}
if (!imaqWriteFile(m_monoImage.GetImaqImage(), "/tmp/0001-monoImage.bmp", NULL)) {
printf("%s: imaqWriteFile(\"/tmp/0001-monoImage.bmp\") FAILED\n", __FUNCTION__);
// ignore the error
}
if (!imaqWriteFile(m_threshold.GetImaqImage(), "/tmp/0002-threshold.bmp", m_falseColor)) {
printf("%s: imaqWriteFile(\"/tmp/0002-threshold.bmp\") FAILED\n", __FUNCTION__);
// ignore the error
}
if (!imaqWriteFile(m_convexHull.GetImaqImage(), "/tmp/0003-convexHull.bmp", m_falseColor)) {
printf("%s: imaqWriteFile(\"/tmp/0003-convexHull.bmp\") FAILED\n", __FUNCTION__);
// ignore the error
}
if (!imaqWriteFile(m_filtered.GetImaqImage(), "/tmp/0004-filtered.bmp", m_falseColor)) {
printf("%s: imaqWriteFile(\"/tmp/0004-filtered.bmp\") FAILED\n", __FUNCTION__);
// ignore the error
}
imaqMergeOverlay(m_overlay.GetImaqImage(), m_filtered.GetImaqImage(), m_falseColor, 256, NULL);
OverlayParticle(&m_overlay, m_pTop);
OverlayParticle(&m_overlay, m_pBottom);
OverlayParticle(&m_overlay, m_pLeft);
OverlayParticle(&m_overlay, m_pRight);
OverlayCenter(&m_overlay, m_centerX, m_centerY);
OverlayLimits(&m_overlay, m_leftX, m_centerX, m_rightX);
imaqMergeOverlay(m_overlay.GetImaqImage(), m_overlay.GetImaqImage(), NULL, 0, NULL);
if (!imaqWriteFile(m_overlay.GetImaqImage(), "/tmp/0005-overlay.bmp", FALSE)) {
printf("%s: imaqWriteFile(\"/tmp/0005-overlay.bmp\") FAILED\n", __FUNCTION__);
// ignore the error
}
long now = (long) GetFPGATime();
printf("%s: image save took %ld microseconds\n", __FUNCTION__, (now - then));
}
// used only once per periodict routine and on only one timer to set the ticks
// for all timers used in that periodict routine
void CxTimer::Update(void)
{
long sf=10000; // take it from microsec to millisec
long maxsf=0x7fffffff; // mask off any sign bit because of actual unsigned count
long curt=GetFPGATime(); // gets number of microsecond ticks in the current counter
curt=curt&maxsf; // fix for signed vs unsigned
if(curt>=m_myLastTm)
m_timeTicks=curt-m_myLastTm; // not roll over just normal
else
m_timeTicks=maxsf-(m_myLastTm-curt)+1; // handle roll over case
// the roll over case is designed to keep with math limits and use normal oprands
// it would be simpler to do a two complement but with the uncertainty about the
// compilers actual methods a pure operator methods was used.
// because of the scaling a simple integer division will always leave some
// number of ticks out and overtime will cause the timer to lose accuracy
// keeping the sum of ticks from the remainder allows for long term accuracy
//
m_timeTicksRm+=(m_timeTicks%sf); // add any remainder to remainder
// Now get the whole number part of the ticks
m_timeTicks=m_timeTicks/sf; // get millisec from microsec clock
// if enough fractional ticks have accumilated increase the tick count by 1
while (m_timeTicksRm>sf) {m_timeTicks++;m_timeTicksRm-=sf;} // add odd time
// keep this count to use as the last count in then next call
m_myLastTm=curt;
}
RobotTelemetry::RobotTelemetry() {
isEnabled = false;
robotDiagnostics = new RobotTelemetryData();
mRobotTelemetry = semBCreate(SEM_Q_FIFO,SEM_FULL);
startTime = GetFPGATime();
initTelemetryData();
}
/**
* @brief Logs a message to the logfile.
* @param message The message to log.
*
* @author Arthur Lockman
*/
void LogSystem::PrintToFile(const char* message)
{
//@TODO Log message to the logfile.
FILE* logFile = fopen("logfile.txt","a");
fprintf(logFile, "%i: %s\n",GetFPGATime(),message);
fclose(logFile);
}
RobotNav::RobotNav(float CurrentX, float CurrentY, float Theta) : Subsystem("RobotNav")
{
m_Time = GetFPGATime();
m_CurrentX = CurrentX;
m_CurrentY = CurrentY;
m_Theta = Theta;
NavTab = NetworkTable::GetTable("Nav");
}
bool Target::GetImage()
{
printf("Target::GetImage\n");
long then = (long) GetFPGATime();
if (!m_camera.IsFreshImage()) {
return false;
}
printf("Target::GetImage - got fresh image\n");
if (!m_camera.GetImage(&m_cameraImage)) {
printf("%s: image acquisition FAILED\n", __FUNCTION__);
return false;
}
long now = (long) GetFPGATime();
printf("%s: image acquisition took %ld microseconds\n", __FUNCTION__, (now - then));
return true;
}
RobotNav::RobotNav() : Subsystem("RobotNav")
{
m_Time = GetFPGATime();
//TODO: getx from smart-dashboard
m_CurrentX = 0;
//TODO: get y from smart-dashboard
m_CurrentY = 0;
//TODO: get theta from smart-dashboard
m_Theta = 0;
NavTab = NetworkTable::GetTable("Nav");
}
void RobotTelemetry::updateTelemetryData() {
unsigned int buttonIndex = 0;
unsigned int index = 0;
DigitalModule* dio = DigitalModule::GetInstance(1);
AnalogModule* aio = AnalogModule::GetInstance(1);
//micro seconds to milliseconds
robotDiagnostics->time = (long)((GetFPGATime() - startTime) * 1.0e-3);
//robotDiagnostics->batteryVoltage = DriverStation::GetInstance()->GetBatteryVoltage();
for(index = 0; index < sizeof(robotDiagnostics->pwmOutputs)/sizeof(*robotDiagnostics->pwmOutputs); index++) {
robotDiagnostics->pwmOutputs[index] = dio->GetPWM(index+1);
}
for(index = 0; index < sizeof(robotDiagnostics->relayOutputs)/sizeof(*robotDiagnostics->relayOutputs); index++) {
robotDiagnostics->relayOutputs[index] = (dio->GetRelayForward(index+1) ? 1 : (dio->GetRelayReverse(index+1) ? 1 : 0));
}
for(index = 0; index < sizeof(robotDiagnostics->digitalInputs)/sizeof(*robotDiagnostics->digitalInputs); index++) {
robotDiagnostics->digitalInputs[index] = dio->GetDIO(index+1);
}
for(index = 0; index < sizeof(robotDiagnostics->analogInputs)/sizeof(*robotDiagnostics->analogInputs); index++) {
robotDiagnostics->analogInputs[index] = aio->GetAverageVoltage(index+1);
}
for(index = 0; index < sizeof(robotDiagnostics->pneumatics)/sizeof(*robotDiagnostics->pneumatics); index++) {
robotDiagnostics->pneumatics[index] = 0;
}
for(index = 0; index < sizeof(robotDiagnostics->joysticks)/sizeof(*robotDiagnostics->joysticks); index++) {
robotDiagnostics->joysticks[index].x = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kXAxis);
robotDiagnostics->joysticks[index].y = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kYAxis);
robotDiagnostics->joysticks[index].z = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kZAxis);
for(buttonIndex = 0; buttonIndex < 8; buttonIndex++) {
robotDiagnostics->joysticks[index].buttons[buttonIndex] = Joystick::GetStickForPort(index+1)->GetRawButton(buttonIndex+1);
}
}
for(index = 0; index < sizeof(robotDiagnostics->gamepads)/sizeof(*robotDiagnostics->gamepads); index++) {
robotDiagnostics->gamepads[index].x = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kXAxis);
robotDiagnostics->gamepads[index].y = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kXAxis);
robotDiagnostics->gamepads[index].z = Joystick::GetStickForPort(index+1)->GetAxis(Joystick::kXAxis);
for(buttonIndex = 0; buttonIndex < 8; buttonIndex++) {
robotDiagnostics->gamepads[index].buttons[buttonIndex] = Joystick::GetStickForPort(index+1)->GetRawButton(buttonIndex+1);
}
}
}
// Called repeatedly when this Command is scheduled to run
void CatapultSetCommand::Execute()
{
printf("catapult set execute\n");
catapultsubsystem->Set(CatapultSubsystem::little,Relay::kForward);
if (SENSOR_GET(digitalin,catapult))
{
catapultsubsystem->Set(CatapultSubsystem::big,Relay::kForward);
catapultsubsystem->Set(CatapultSubsystem::little,Relay::kForward);
} else
{
catapultsubsystem->Set(CatapultSubsystem::big,Relay::kReverse);
if ((GetFPGATime()%(switchtime*2))>=(switchtime))
{catapultsubsystem->Set(CatapultSubsystem::little,Relay::kReverse);}
else
{catapultsubsystem->Set(CatapultSubsystem::little,Relay::kForward);}
}
}
uint32_t
Tachometer::GetInterval()
{
NTSynchronized LOCK(tachSem);
if (intervalValid) {
// check if interval is _still_ valid
uint32_t now = GetFPGATime();
uint32_t interval = now - lastTime;
if (interval < 200000) { // 200mS
return lastInterval;
} else {
intervalValid = false;
}
}
return 0;
}
JankyTask::JankyTask(const char* taskName, UINT32 priority) {
std::string name = taskName;
char tmp[30];
if (!taskName)
{
sprintf(tmp, "%d", GetFPGATime());
name = "jankyTask-";
name += tmp;
}
enabled_ = false;
running_ = true;
isDead_ = false;
task_ = new Task(name.c_str(), (FUNCPTR)JankyTask::JankyPrivateStarterTask, priority);
task_->Start((UINT32)this);
}
/**
* Return the FPGA system clock time in seconds.
*
* Return the time from the FPGA hardware clock in seconds since the FPGA
* started.
* Rolls over after 71 minutes.
* @returns Robot running time in seconds.
*/
double Timer::GetFPGATimestamp() {
// FPGA returns the timestamp in microseconds
// Call the helper GetFPGATime() in Utility.cpp
return GetFPGATime() * 1.0e-6;
}
bool Target::AnalyzeParticles()
{
printf("Target::AnalyzeParticles\n");
long then = (long) GetFPGATime();
if (m_numParticles < 3) {
printf("ERROR: m_numParticles < 3, no analysis possible\n");
return false;
}
// image quality tests:
//
// 1) The four particles should be approximately the same size. If not, one or more
// of the particles is partially hidden or we didn't identify the particles correctly.
//
// 2) The particles should be laid out without overlapping. For example, the left
// and right bounds of the top particle should not overlap the right bound of the
// left particle or the left bound of the right particle. This relationship can
// be used to determine which particle is missing when only 3 are in view.
// (If only 2 particles are in view, we can't use this to distinguish between e.g.
// the top+left particles and the right+bottom particles. TBD: see if we can use
// e.g. absolute height information to sort this out when we're really close to
// the targets.)
//
// 3) There should be some space between the outer edge of each particle and the edge
// of the image. If not, one of the particles is partially outside the field of view.
//
// 4) The aspect ratio (height to width ratio) of each (unclipped) particle should be
// approximately that of the target rectangles.
//
// 5) The image center position calculated from the inside edges of the particles,
// the center of mass of the particles, and the outside edges of the particles should
// be approximately the same.
double size; // median size
if (m_numParticles == 4) {
size = (m_particles[1].size + m_particles[2].size) / 2.;
} else {
// only 3 particles
size = m_particles[1].size;
}
// These limits are very large in order to accomodate
// clipping and image distortions from the hoops and nets at close range.
double min_size = size * 0.20;
double max_size = size * 3.00;
if (m_particles[0].size > max_size) {
printf("ERROR: particle 0 is unreasonably large, bad image\n");
return false;
}
if (m_numParticles == 4 && m_particles[3].size < min_size) {
printf("WARNING: particle 3 is unreasonably small, dropping it\n");
m_numParticles = 3;
}
if (m_numParticles == 3 && m_particles[2].size < min_size) {
printf("ERROR: particle 2 is unreasonably small\n");
return false;
}
// Sort the particles by position, based on their inside edges.
// These are in image coordinates, so (0,0) is top-left.
m_pTop = m_pBottom = m_pLeft = m_pRight = &m_particles[0];
for (int i = 1; i < m_numParticles; i++) {
Particle *p = &m_particles[i];
if (p->bottomBound < m_pTop->bottomBound) {
m_pTop = p;
}
if (p->topBound > m_pBottom->topBound) {
m_pBottom = p;
}
if (p->rightBound < m_pLeft->rightBound) {
m_pLeft = p;
}
if (p->leftBound > m_pRight->leftBound) {
m_pRight = p;
}
}
printf("sorted: top %d bottom %d left %d right %d\n",
m_pTop->index, m_pBottom->index, m_pLeft->index, m_pRight->index);
if (m_numParticles < 4) {
// TBD: The if-then-elseif structure here assumes that only one
// particle is missing. Rework this to deal with two, or even three
// missing particles.
if (m_pTop->bottomBound > m_pLeft->topBound ||
m_pTop->bottomBound > m_pRight->topBound)
{
printf("top overlaps left/right, top removed\n");
m_pTop = NULL;
}
else if (m_pBottom->topBound < m_pLeft->bottomBound ||
m_pBottom->topBound < m_pRight->bottomBound)
{
printf("bottom overlaps left/right, bottom removed\n");
m_pBottom = NULL;
}
else if ((m_pTop && m_pLeft->rightBound > m_pTop->leftBound) ||
(m_pBottom && m_pLeft->rightBound > m_pBottom->leftBound))
{
printf("left overlaps top/bottom, left removed\n");
m_pLeft = NULL;
}
else if ((m_pTop && m_pRight->leftBound < m_pTop->rightBound) ||
(m_pBottom && m_pRight->leftBound < m_pBottom->rightBound))
{
printf("right overlaps top/bottom, right removed\n");
m_pRight = NULL;
}
else
{
printf("ERROR: particle overlap can't be resolved\n");
return false;
}
}
// uniqueness check
if (m_pTop == m_pLeft || m_pTop == m_pRight || m_pTop == m_pBottom ||
m_pLeft == m_pRight || m_pLeft == m_pBottom || m_pRight == m_pBottom) {
printf("ERROR: particles aren't unique.\n");
return false;
}
// check outside boundaries and particle sizes for clipping and image artifacts
m_topClipped = m_bottomClipped = m_leftClipped = m_rightClipped = false;
if (!m_pTop || m_pTop->topBound < BORDER) {
printf("WARNING: top particle is clipped\n");
m_topClipped = true;
}
if (!m_pBottom || m_pBottom->bottomBound > (HEIGHT - BORDER)) {
printf("WARNING: bottom particle is clipped\n");
m_bottomClipped = true;
}
if (!m_pLeft || m_pLeft->leftBound < BORDER) {
printf("WARNING: left particle is clipped\n");
m_leftClipped = true;
}
if (!m_pRight || m_pRight->rightBound > (WIDTH - BORDER)) {
printf("WARNING: right particle is clipped\n");
m_rightClipped = true;
}
//////////////////////////////////////////////////////////////////////////
//
// Calculate the angle and distance to the center of the target(s).
//
// Project data from the image plane to the real-world plane of the
// backboards using what we know about the real-world dimensions of
// the features we're seeing. From that, we can calculate the angle
// and distance from the camera centerline to each feature. In order
// to aim the robot, we may also need to deal with a rotation and/or
// translation between the camera centerline and the robot centerline,
// but we'll ignore that for now.
//
// If we're shooting from either side of the field, we may need to
// adjust our aiming point to hit the center of the hoops rather than
// aiming directly for the center of the reflective targets. Again,
// we'll ignore that for now.
//
// For a given field-of-view angle and image plane width (in pixels),
// the distance (in pixel-width units) to the image plane is given by:
// d' = (w'/2) / tan(h/2)
// where w is the width of the image plane in pixels (640) and h is
// the horizontal angle of view (49 degrees for this camera/lens.)
// This constant has been pre-calculated (IMAGE_PLANE).
//
// The view angle for an object at x' pixels from the center can be
// solved from:
// d' = x' / tan(a)
// tan(a) = x' / d'
// a = atan(x' / d')
//
// Given a desired target and the real-world distances to visible targets
// on either side of it, we can calculate the distance to the target.
// Once we have the distance to the center target, we can calculate
// the distances to the targets on either side.
//
//////////////////////////////////////////////////////////////////////////
if (m_pTop) {
m_topAngle = atan((m_pTop->xCenter-(WIDTH/2))/IMAGE_PLANE)*DEGREES;
printf("top angle %g degrees\n", m_topAngle);
}
if (m_pBottom) {
m_bottomAngle = atan((m_pBottom->xCenter-(WIDTH/2))/IMAGE_PLANE)*DEGREES;
printf("bottom angle %g degrees\n", m_bottomAngle);
}
if (m_pTop && m_pBottom) {
m_centerX = (m_pTop->xCenter + m_pBottom->xCenter) / 2.;
m_centerAngle = atan((m_centerX - (WIDTH/2)) / IMAGE_PLANE) * DEGREES;
printf("center angle %g degrees\n", m_centerAngle);
} else if (m_pTop) {
m_centerX = m_pTop->xCenter;
m_centerAngle = m_topAngle;
m_bottomAngle = m_topAngle; // best available estimate
} else if (m_pBottom) {
m_centerX = m_pBottom->xCenter;
m_centerAngle = m_bottomAngle;
m_topAngle = m_bottomAngle; // best available estimate
} else {
printf("ERROR: m_pTop and m_pBottom are both NULL (can't happen!)\n");
return false;
}
if (m_pLeft && m_pRight) {
m_centerY = (m_pLeft->yCenter + m_pRight->yCenter) / 2.;
} else if (m_pLeft) {
m_centerY = m_pLeft->yCenter;
} else if (m_pRight) {
m_centerY = m_pRight->yCenter;
} else {
printf("ERROR: m_pLeft and m_pRight are both NULL (can't happen!)\n");
return false;
}
double leftWidthReal;
double leftWidthImage;
if (m_pLeft) {
if (m_leftClipped) {
m_leftX = m_pLeft->rightBound;
leftWidthImage = m_centerX - m_leftX;
leftWidthReal = FRC_INSIDE;
} else {
m_leftX = m_pLeft->leftBound;
leftWidthImage = m_centerX - m_leftX;
leftWidthReal = FRC_OUTSIDE;
}
} else if (m_pTop) {
m_leftX = m_pTop->leftBound;
leftWidthImage = m_centerX - m_leftX;
leftWidthReal = FRC_WIDTH;
} else if (m_pBottom) {
m_leftX = m_pBottom->leftBound;
leftWidthImage = m_centerX - m_leftX;
leftWidthReal = FRC_WIDTH;
} else {
printf("ERROR: m_pTop and m_pBottom are both NULL (can't happen!)\n");
return false;
}
double theta1 = atan( leftWidthImage / IMAGE_PLANE );
m_leftAngle = m_centerAngle - theta1 * DEGREES; // relative to robot
printf("theta1 %g, left angle %g degrees\n", theta1*DEGREES, m_leftAngle);
double rightWidthReal;
double rightWidthImage;
if (m_pRight) {
if (m_rightClipped) {
m_rightX = m_pRight->leftBound;
rightWidthImage = m_rightX - m_centerX;
rightWidthReal = FRC_INSIDE;
} else {
m_rightX = m_pRight->rightBound;
rightWidthImage = m_rightX - m_centerX;
rightWidthReal = FRC_OUTSIDE;
}
} else if (m_pTop) {
m_rightX = m_pTop->rightBound;
rightWidthImage = m_rightX - m_centerX;
rightWidthReal = FRC_WIDTH;
} else if (m_pBottom) {
m_rightX = m_pBottom->rightBound;
rightWidthReal = FRC_WIDTH;
rightWidthImage = m_rightX - m_centerX;
} else {
printf("ERROR: m_pTop and m_pBottom are both NULL (can't happen!)\n");
return false;
}
double theta2 = atan( rightWidthImage / IMAGE_PLANE );
m_rightAngle = m_centerAngle + theta2 * DEGREES; // relative to robot
printf("theta2 %g, right angle %g degrees\n", theta2*DEGREES, m_rightAngle);
// these quantities appear repeatedly in the calculations
double k1 = sin(theta1) / leftWidthReal;
double k2 = sin(theta2) / rightWidthReal;
double k3 = M_PI - (theta1 + theta2);
double sink3 = sin(k3);
double cosk3 = cos(k3);
printf("k1 %g k2 %g k3 %g sin k3 %g cos k3 %g\n",
k1, k2, k3*DEGREES, sink3, cosk3);
// Calculate the distance to the center (c) using either
// the triangle on the left or the right.
double alpha = atan((k1*sink3)/(k2+k1*cosk3));
if (alpha < 0) { alpha += M_PI; }
printf("alpha %g degrees\n", alpha * DEGREES);
double c1 = sin(alpha) / k1;
double beta = atan((k2*sink3)/(k1+k2*cosk3));
if (beta < 0) { beta += M_PI; }
printf("beta %g degrees\n", beta * DEGREES);
double c2 = sin(beta) / k2;
// Check: the sum of alpha+beta should be equal to k3.
printf("alpha+beta %g k3 %g\n", (alpha+beta)*DEGREES, k3*DEGREES);
// Check: these two distance calculations should agree.
printf("distance1 %g, distance2 %g\n", c1, c2);
m_centerDistance = c1;
// angle "gamma1" and side "b" in Jeff's diagram
m_leftDistance = sin(M_PI - (theta1 + alpha)) / k1;
// angle "gamma2" and side "a" in Jeff's diagram
m_rightDistance = sin(M_PI - (theta2 + beta)) / k2;
printf("distance left %g, center %g, right %g\n",
m_leftDistance, m_centerDistance, m_rightDistance);
long now = (long) GetFPGATime();
printf("%s: particle analysis took %ld microseconds\n", __FUNCTION__, (now - then));
return true;
}
bool Target::FindParticles()
{
printf("Target::FindParticles\n");
long then = (long) GetFPGATime();
// extract the blue plane
if (!imaqExtractColorPlanes(m_cameraImage.GetImaqImage(), IMAQ_RGB,
NULL, NULL, m_monoImage.GetImaqImage()))
{
printf("%s: imaqExtractColorPlanes FAILED\n", __FUNCTION__);
return false;
}
// select interesting particles
if (!imaqThreshold(m_threshold.GetImaqImage(), m_monoImage.GetImaqImage(),
/*rangeMin*/ THRESHOLD, /*rangeMax*/ 255, /*useNewValue*/ 1, /*newValue */ 1))
{
printf("%s: imaqThreshold FAILED\n", __FUNCTION__);
return false;
}
if (!imaqConvexHull(m_convexHull.GetImaqImage(), m_threshold.GetImaqImage(),
/*connectivity8*/ 1))
{
printf("%s: imaqConvexHull FAILED\n", __FUNCTION__);
return false;
}
if (!imaqSizeFilter(m_filtered.GetImaqImage(), m_convexHull.GetImaqImage(),
/*connectivity8*/ 1, /*erosions*/ 2, /*keepSize*/ IMAQ_KEEP_LARGE,
/*structuringElement*/ NULL))
{
printf("%s: imaqSizeFilter FAILED\n", __FUNCTION__);
return false;
}
int particleCount = 0;
if (!imaqCountParticles(m_filtered.GetImaqImage(), 1, &particleCount))
{
printf("%s: imaqCountParticles FAILED\n", __FUNCTION__);
return false;
}
// select the four largest particles (insertion sort)
// for now, keep track of only the particle number (index) and size
memset((void *)m_particles, 0, sizeof m_particles);
for (int i = 0; i < particleCount; i++) {
double size;
if (!imaqMeasureParticle(m_filtered.GetImaqImage(), i, FALSE,
IMAQ_MT_PARTICLE_AND_HOLES_AREA, &size))
{
printf("%s: imaqMeasureParticle %d FAILED\n", __FUNCTION__, i);
break;
}
for (int j = 0; j < 4; j++) {
if (size > m_particles[j].size) {
for (int k = 3; k > j; k--) {
m_particles[k].index = m_particles[k-1].index;
m_particles[k].size = m_particles[k-1].size;
}
m_particles[j].index = i;
m_particles[j].size = size;
break;
}
}
}
// fill in the rest of the measured data
for (m_numParticles = 0;
m_numParticles < 4 && m_particles[m_numParticles].size > 0;
m_numParticles++)
{
Particle* p = &m_particles[m_numParticles];
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_CENTER_OF_MASS_X, &(p->xCenter));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_CENTER_OF_MASS_Y, &(p->yCenter));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_LEFT, &(p->leftBound));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_RIGHT, &(p->rightBound));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_TOP, &(p->topBound));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_BOTTOM, &(p->bottomBound));
// calculate height/width from bounding box
p->height = p->bottomBound - p->topBound;
p->width = p->rightBound - p->leftBound;
}
long now = (long) GetFPGATime();
printf("%s: particle detection took %ld microseconds\n", __FUNCTION__, (now - then));
printf("%s: found %d particles\n", __FUNCTION__, particleCount);
printf("%s: returning %d particles\n", __FUNCTION__, m_numParticles);
for (int i = 0; i < m_numParticles; i++) {
Particle *p = &m_particles[i];
printf(" particle %d index %d top %g bottom %g left %g right %g size %g x %g y %g\n",
i, p->index, p->topBound, p->bottomBound, p->leftBound, p->rightBound,
p->size, p->xCenter, p->yCenter);
}
return true;
}
/*
* Return the FPGA system clock time in seconds.
*
* Return the time from the FPGA hardware clock in seconds since the FPGA
* started.
* @returns Robot running time in seconds.
*/
double GetClock(void)
{
return GetFPGATime() * 1e-6;
}
/**
* @brief Logs a message to the console.
* @param message The message to log.
*
* @author Arthur Lockman
*/
void LogSystem::Print(const char* message)
{
printf("%i: %s\n",GetFPGATime(),message);
}