void Robot980::CamUpdate()
{
// search variables
bool foundColor = false;
ParticleAnalysisReport par1, par2; // particle analysis reports
memset(&par1, 0, sizeof(ParticleAnalysisReport));
memset(&par2, 0, sizeof(ParticleAnalysisReport));
// calculate gimbal position based on colors found
if (FindTwoColors(m_tdPink, m_tdGreen,
(m_trackColor == DriverStation::kBlue) ? ABOVE : BELOW,
&par1, &par2))
{
foundColor = true;
if (par1.imageTimestamp == m_dSavedImageTimestamp)
{
// This image has been processed already, so don't do anything
return;
}
// The target was recognized - save the timestamp
m_dSavedImageTimestamp = par1.imageTimestamp;
m_trailerInfo.color = m_trackColor;
// get center of target; Average the color two particles to
// get center x & y of combined target
m_trailerInfo.dCenterMassX = (par1.center_mass_x_normalized +
par2.center_mass_x_normalized) / 2;
m_trailerInfo.dCenterMassY = (par1.center_mass_y_normalized +
par2.center_mass_y_normalized) / 2;
m_trailerInfo.particleQuality = (par1.particleQuality +
par2.particleQuality) / 2;
}
else
{ // need to pan
foundColor = false;
}
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.
*/
}
else
{
// new image, but didn't find two colors
} // end if found color
}
/**
* 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.
*/
bool TargetDetector::Detect(double& centerOfMassX, double& centerOfMassY, double& particleToImagePercent)
{
DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ABOVE %s", td2.name, td1.name);
// search variables
bool foundColor = false;
//GetWatchdog().Feed(); // turn watchdog off while debugging
// calculate gimbal position based on colors found
if ( FindTwoColors(td1, td2, ABOVE, &par) )
{
//PrintReport(&par);
foundColor = true;
savedImageTimestamp = par.imageTimestamp;
DPRINTF(LOG_DEBUG,"image timetamp: %lf", savedImageTimestamp);
// report center, size of target
centerOfMassX = par.center_mass_x_normalized;
centerOfMassY = par.center_mass_y_normalized;
particleToImagePercent = par.particleToImagePercent;
} // end while
return foundColor;
} // end autonomous
bool Michael1Camera::TrackTarget(){
if ( FindTwoColors(td1, td2, ABOVE, &par1) ){
ShowActivity("X: %f, Y: %f", par1.center_mass_x_normalized, par1.center_mass_y_normalized);
//horizontalServo->Set(horizontalServo->Get() + (par1.center_mass_x_normalized/2)*0.2);
//verticalServo->Set(verticalServo->Get() - (par1.center_mass_y_normalized/2)*0.2);
return true;
} else {
return false;
}
}
int Michael1Camera::oktoshoot(){
if(FindTwoColors(td1, td2, ABOVE, &par1))
if ((par1.center_mass_x_normalized<=0.2)&&(par1.center_mass_x_normalized>=-.2)){
return 3;
}else if((par1.center_mass_x_normalized<=.5)&&(par1.center_mass_x_normalized>.2)){
return 2;
}else if(par1.center_mass_x_normalized>.5){
return 1;
}else if((par1.center_mass_x_normalized<-.2)&&(par1.center_mass_x_normalized>=-.5)){
return 4;
}else if(par1.center_mass_x_normalized<-.5){
return 5;
}
return 0;
}
int FindTwoColors(TrackingThreshold td1, TrackingThreshold td2,
SecondColorPosition position, ParticleAnalysisReport *trackReport)
{ return FindTwoColors(td1, td2, position, trackReport, 3); }
/**
* 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 Autonomous(void)
{
char funcName[] = "Autonomous";
DPRINTF(LOG_DEBUG, "Autonomous");
DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ABOVE %s",
td2.name, td1.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;
// initialize pan variables
// incremental tasking toward dest (-1.0 to 1.0)
float incrementH, incrementV;
// pan needs a 1-up number for each call
int panIncrement = 0;
// current position range -1.0 to +1.0
horizontalPosition = RangeToNormalized(horizontalServo->Get(), 1);
verticalPosition = RangeToNormalized(verticalServo->Get(), 1);
// set servos to start at center position
setServoPositions(horizontalDestination, verticalDestination);
// for controlling loop execution time
float loopTime = 0.1;
//float loopTime = 0.05;
double currentTime = GetTime();
double lastTime = currentTime;
// search variables
bool foundColor = 0;
double savedImageTimestamp = 0.0;
bool staleImage = false;
while (IsAutonomous())
{
//GetWatchdog().Feed(); // turn watchdog off while debugging
// calculate gimbal position based on colors found
if (FindTwoColors(td1, td2, ABOVE, &par))
{
//PrintReport(&par);
foundColor = true;
// reset pan
panIncrement = 0;
if (par.imageTimestamp == savedImageTimestamp)
{
// This image has been processed already,
// so don't do anything for this loop
staleImage = true;
DPRINTF(LOG_DEBUG, "STALE IMAGE");
}
else
{
// The target was recognized
// save the timestamp
staleImage = false;
savedImageTimestamp = par.imageTimestamp;
DPRINTF(LOG_DEBUG, "image timetamp: %lf",
savedImageTimestamp);
// Here is where your game-specific code goes
// when you recognize the target
// get center of target
// TODO: use par.average_mass_x_normalized and
// par.average_mass_y_normalized after updated WPILib is distributed
horizontalDestination = par.center_mass_x_normalized;
verticalDestination = par.center_mass_y_normalized;
}
}
else
{ // need to pan
foundColor = false;
}
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;
// you may need to reverse this based on your vertical servo installation
//incrementV = verticalPosition - verticalDestination;
incrementV = verticalDestination - verticalPosition;
adjustServoPositions(incrementH, incrementV);
ShowActivity("** %s & %s found: Servo: x: %f y: %f ** ",
td1.name, td2.name, horizontalDestination,
verticalDestination);
}
else
{ //if (!staleImage) { // new image, but didn't find two colors
// adjust sine wave for panning based on last movement direction
if (horizontalDestination > 0.0)
{
sinStart = PI / 2.0;
}
else
{
sinStart = -PI / 2.0;
}
/* pan to find color after a short wait to settle servos
* panning must start directly after panInit or timing will be off */
if (panIncrement == 3)
{
panInit(8.0); // number of seconds for a pan
}
else if (panIncrement > 3)
{
panForTarget(horizontalServo, sinStart);
/* Vertical action: In case the vertical servo is pointed off center,
* center the vertical after several loops searching */
if (panIncrement == 20)
{
verticalServo->Set(0.5);
}
}
panIncrement++;
ShowActivity
("** %s and %s not found ",
td1.name, td2.name);
} // end if found color
// sleep to keep loop at constant rate
// this helps keep pan consistant
// elapsed time can vary significantly due to debug printout
currentTime = GetTime();
lastTime = currentTime;
if (loopTime > ElapsedTime(lastTime))
{
Wait(loopTime - ElapsedTime(lastTime)); // seconds
}
} // end while
DPRINTF(LOG_DEBUG, "end Autonomous");
ShowActivity
("Autonomous end ");
} // end autonomous
