/** * @brief Move the horizontal servo back and forth. * @param panServo The servo object to move * @param sinStart The position on the sine wave to begin the pan */ void panForTarget(Servo *panServo) { panForTarget(panServo, 0.0); }
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