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