/* BTCamera

* camera-servo hybrid class

* manages camera and servos, including tracking functions

*/

#include "BTCamera.h"

#include "WPILib.h"

#include "TrackAPI.h"

#include "VisionAPI.h"

#include "AxisCamera.h"

#include "BTRobot.h"

BTCamera::BTCamera(void)

{

count = 0;

distance = 0;

// ================================

// ***camera initialization code***

// ================================

int framesPerSecond;

framesPerSecond = 15;

if(StartCameraTask(framesPerSecond, 0, k320x240, ROT_0) == -1)

{

dprintf(LOG_ERROR, "Failed to spawn camera task; exiting. Error code %s",

GetVisionErrorText(GetLastVisionError()) );

}

else

{

dprintf(LOG_INFO, "Camera task started successfully, resolution 160x120");

}

td1 = GetTrackingData(MARIUCCI_GREEN, MARIUCCI_GREEN_LIGHT);

td2 = GetTrackingData(MARIUCCI_PINK, MARIUCCI_PINK_LIGHT);

// search variables

foundColor = false;

savedImageTimestamp = 0.0;

staleImage = false;

// ===============================

// ***servo initialization code***

// ===============================

horizontalServo = new Servo(PORTS::CAMERA_SERVO_H); // create horizontal servo

verticalServo = new Servo(PORTS::CAMERA_SERVO_V); // create vertical servo

servoDeadband = 0.01; // move if > this amount

sinStart = 0.0; // control where to start the sine wave for pan

// 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;

// pan needs a 1-up number for each call

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);

// ===========================

// ***other initializations***

// ===========================

// for controlling loop execution time

loopTime = 0.1;

currentTime = GetTime();

lastTime = currentTime;

// allow writing to vxWorks target

Priv_SetWriteFileAllowed(1);

SetDebugFlag(DEBUG_SCREEN_AND_FILE);

}

//returns distance between camera and target in inches

float BTCamera::GetCameraDistance(ParticleAnalysisReport* par)

{

float distance;

distance = (log((par->boundingRect.width)/46.872)/(-0.1247));

return distance;

}

#if 1

/**

* Set servo positions (0.0 to 1.0) translated from normalized values (-1.0 to 1.0).

*

* @param normalizedHorizontal Pan Position from -1.0 to 1.0.

* @param normalizedVertical Tilt Position from -1.0 to 1.0.

*/

void BTCamera::SetServoPositions(float normalizedHorizontal, float normalizedVertical)

{

float servoH = NormalizeToRange(normalizedHorizontal);

/* narrow vertical range keep vertical servo from going too far */

//float servoV = NormalizeToRange(normalizedVertical, 0.2, 0.8);

float servoV = NormalizeToRange(normalizedVertical);

float currentH = (1 - horizontalServo->Get());

float currentV = (1 - verticalServo->Get());

/* make sure the movement isn't too small */

if ( fabs(servoH - currentH) > servoDeadband ) {

horizontalServo->Set( 1 - servoH );

/* save new normalized horizontal position */

horizontalPosition = RangeToNormalized(servoH, 1);

}

if ( fabs(servoV - currentV) > servoDeadband ) {

verticalServo->Set( 1 - servoV );

verticalPosition = RangeToNormalized(servoV, 1);

}

}

/**

* Adjust servo positions (0.0 to 1.0) translated from normalized values (-1.0 to 1.0).

*

* @param normalizedHorizontal Pan adjustment from -1.0 to 1.0.

* @param normalizedVertical Tilt adjustment from -1.0 to 1.0.

*/

void BTCamera::AdjustServoPositions(float normDeltaHorizontal, float normDeltaVertical)

{

/* adjust for the fact that servo overshoots based on image input */

normDeltaHorizontal /= 8.0;

normDeltaVertical /= 4.0;

/* compute horizontal goal */

float currentH = (1 - horizontalServo->Get());

float normCurrentH = RangeToNormalized(currentH, 1);

float normDestH = normCurrentH + normDeltaHorizontal;

/* narrow range keep servo from going too far */

if (normDestH > 1.0) normDestH = 1.0;

if (normDestH < -1.0) normDestH = -1.0;

/* convert input to servo range */

float servoH = NormalizeToRange(normDestH);

/* compute vertical goal */

float currentV = (1 - verticalServo->Get());

float normCurrentV = RangeToNormalized(currentV, 1);

float normDestV = normCurrentV + normDeltaVertical;

if (normDestV > 1.0) normDestV = 1.0;

if (normDestV < -1.0) normDestV = -1.0;

/* convert input to servo range */

float servoV = NormalizeToRange(normDestV, 0.2, 0.8);

/* make sure the movement isn't too small */

if ( fabs(currentH-servoH) > servoDeadband ) {

horizontalServo->Set( 1 - servoH );

/* save new normalized horizontal position */

horizontalPosition = RangeToNormalized(servoH, 1);

}

if ( fabs(currentV-servoV) > servoDeadband ) {

verticalServo->Set( 1 - servoV );

verticalPosition = RangeToNormalized(servoV, 1);

}

}

#endif

/* scans for target and adjusts servos

* call inside autonomous loop

*/

double BTCamera::Track(ControlBoard* controlBoard)

{

ParticleAnalysisReport par; // particle analysis report

SecondColorPosition secondColorPosition = ABOVE; //default to blue alliance

if (controlBoard->ds->GetAlliance() == DriverStation::kRed)

{

secondColorPosition = BELOW;

dprintf(LOG_DEBUG, "SERVO - looking for COLOR %s BELOW %s", td2.name, td1.name);

}

if (controlBoard->ds->GetAlliance() == DriverStation::kBlue)

{

secondColorPosition = ABOVE;

dprintf(LOG_DEBUG, "SERVO - looking for COLOR %s ABOVE %s", td2.name, td1.name);

}

// calculate gimbal position based on colors found

if ( FindTwoColors(td1, td2, secondColorPosition, &par) ){

foundColor = true;

distance = GetCameraDistance(&par);

panIncrement = 0;   // reset pan

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 timestamp: %lf", savedImageTimestamp);

// Here is where your game-specific code goes

// when you recognize the target

// get center of target

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

//printf("Servo: x: %f\n", horizontalDestination);

} 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(4.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

if (count > 10)

{

printf("Timestamp = %lf\n", par.imageTimestamp);

count = 0;

}

count++;

// 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

}

return par.center_mass_x_normalized;

}

bool BTCamera::IsTargetInRange(void)

{

// if (GetCameraDistance(&par) <= SHOOTER_RANGE_LIMIT)

return foundColor;

// else

// return false;

}

#if 0

float BTCamera::GetServoValue(int whichServo)

{

float servoValue = 0;

if (whichServo == HORIZ)

{

servoValue = horizontalServo->Get();

}

if (whichServo == VERT)

{

servoValue = verticalServo->Get();

}

return servoValue;

}

#endif

#if 1

/** Simple test to see if the color is taking up too much of the image */

bool BTCamera::IsTooClose(ParticleAnalysisReport* par) {

if (par->particleToImagePercent > MAX_PARTICLE_TO_IMAGE_PERCENT)

return true;

return false;

}

/** Simple test to see if the color is large enough */

bool BTCamera::IsBigEnough(ParticleAnalysisReport* par) {

if (par->particleToImagePercent < MIN_PARTICLE_TO_IMAGE_PERCENT)

return false;

return true;

}

#endif