/**
* 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 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 = horizontalServo->Get();
float currentV = verticalServo->Get();
/* make sure the movement isn't too small */
if (fabs(servoH - currentH) > servoDeadband)
{
horizontalServo->Set(servoH);
/* save new normalized horizontal position */
horizontalPosition = RangeToNormalized(servoH, 1);
}
if (fabs(servoV - currentV) > servoDeadband)
{
verticalServo->Set(servoV);
// save new normalized vertical position
verticalPosition = RangeToNormalized(servoV, 1);
}
ShowActivity("Servo set to: x: %f y: %f", servoH, servoV);
}
/**
* @brief Main program body
* @return int
*/
int main(void)
{
int tmp = 0;
int activityIndex = 0;
int writeVal = 0;
SystemCoreClockUpdate();
Board_Init();
i2c_app_init(I2C0, SPEED_100KHZ);
/* Loop forever */
while (1) {
/* Toggle LED to show activity. */
tmp = ShowActivity(tmp);
/* Test for activity time */
if ((tmp & ACTIVITY_MASK) == 0) {
/* Toggle between writes and reads */
switch (activityIndex++ & 1) {
case 0:
/* Perform target board I2CM write */
WriteBoard_I2CM(writeVal++ & 1);
break;
case 1:
default:
/* Perform target board I2CM read */
ReadBoard_I2CM();
break;
}
}
}
return 0;
}
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;
}
}
/**
* 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 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 = 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 inputs to servo range */
float servoH = NormalizeToRange(normDestH);
/* compute vertical goal */
float currentV = 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;
float servoV = NormalizeToRange(normDestV, 0.2, 0.8);
/* make sure the movement isn't too small */
if (fabs(currentH - servoH) > servoDeadband)
{
horizontalServo->Set(servoH);
/* save new normalized horizontal position */
horizontalPosition = RangeToNormalized(servoH, 1);
}
if (fabs(currentV - servoV) > servoDeadband)
{
verticalServo->Set(servoV);
// save new normalized vertical position
verticalPosition = RangeToNormalized(servoV, 1);
}
ShowActivity("Servo set to: x: %f y: %f", servoH, servoV);
}
/**
* 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
/**
* 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
