//The interrupt response for our phototransistor
void PhotoTransistor_InterruptResponse(void)
{
// Clear Interrupt
clearCaptureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
// Determine Capture time
uint32_t ThisCapture = captureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
// Calculate period
uint32_t Period = ((ThisCapture - LastCapture) * MICROSECONDS_DIVISOR ) / TICKS_PER_MS;
//Store the Last Cpature
LastCapture = ThisCapture;
//Iterate Through the different frequency options
for (int i = 0; i < NUMBER_BEACON_FREQUENCIES; i++)
{
// If the period matches a beacon period
if (ToleranceCheck(Period, beacons[i].period, PERIOD_MEASURING_ERROR_TOLERANCE))
{
// If we're searching for a beacon
if (Bucketing)
{
buckets[i]++;
}
// If we're supposed to align to the bucket, and the number of pulses we've seen for
// this beacon is greater than our threshold
if ((AligningToBucket) && (buckets[i] >= NUMBER_PULSES_FOR_BUCKET))
{
// If this is the beacon corresponding to our target bucket
if (((MyColor() == COLOR_BLUE) && (i == BEACON_INDEX_NW)) || ((MyColor() == COLOR_RED) && (i == BEACON_INDEX_SE)))
{
// stop our drive
clearDriveAligningToBucket();
// Post an aligned event
ES_Event AlignedEvent;
AlignedEvent.EventType = ES_ALIGNED_TO_BUCKET;
PostMasterSM(AlignedEvent);
// stop aligning
AligningToBucket = false;
// reset all buckets
for (int j = 0; j < NUMBER_BEACON_FREQUENCIES; j++)
{
buckets[j] = 0;
}
// reset average information
ResetAverage();
// return to searching for beacons
Bucketing = true;
LastBeacon = NULL_BEACON;
ES_Timer_StopTimer(AVERAGE_BEACONS_TIMER);
return;
}
}
// If we're not aligning to a bucket, and the number of pulses we've seen for this
// beacon is greater than our threshold
else if (buckets[i] >= NUMBER_PULSES_TO_BE_ALIGNED && LastBeacon == NULL_BEACON && !AligningToBucket)
{
// store the beacon to be recorded
LastBeacon = i;
// reset all buckets
for (int j = 0; j < NUMBER_BEACON_FREQUENCIES; j++)
{
buckets[j] = 0;
}
// restart the timer to evaluate the beacon
ES_Timer_InitTimer(AVERAGE_BEACONS_TIMER, AVERAGE_BEACONS_T);
Bucketing = false;
}
// increment the sum of all encoder angles for this beacon
sums[i] += GetPeriscopeAngle();
// increment the number of pulses seen for this beacon
numSamples[i]++;
// If the evaluation timer is already running, restart it
ES_Timer_SetTimer(AVERAGE_BEACONS_TIMER, AVERAGE_BEACONS_T);
}
}
}
void SimulatedAnnealingOptimizer::SimAnnealDoOpt()
{
opMessage = "SimAnneal - optimizing ...";
while (true)
{
SimAnnealCalcHiLo();
CalcTemperature();
optIterCount++;
// we have a best fit by this point
okForOutput = true;
if (ToleranceCheck(simplexFits[hiIndex], simplexFits[loIndex],
simplexParEst[hiIndex], simplexParEst[loIndex]))
return;
double ystar = SimAnnealReflect();
double ystarFlu = ystar - ThermalFluct();
// between current lo and next hi?
if ((ystarFlu > ylo) && (ystarFlu < ynhi))
{
yhi = ystarFlu;
// replace Ph with P*
simplexParEst[hiIndex] = simplexParStar;
simplexFits[hiIndex] = ystar;
opMessage = "SimAnneal - reflection in range ...";
continue;
}
// better than old best ?
if (ystarFlu <= ylo)
{
double ystarstar = SimAnnealExpand();
double ystarstarFlu = ystarstar - ThermalFluct();
if (ystarstarFlu < ylo)
{
simplexParEst[hiIndex] = simplexParStarStar;
simplexFits[hiIndex] = ystarstar;
yhi = ystarstarFlu;
opMessage = "SimAnneal - expansion passed ...";
}
else
{
simplexParEst[hiIndex] = simplexParStar;
simplexFits[hiIndex] = ystar;
yhi = ystarFlu;
opMessage = "SimAnneal - reflection passed ...";
}
continue;
}
// original reflection was a failure
if (ystarFlu < yhi)
{
// better than old worst
simplexParEst[hiIndex] = simplexParStar;
simplexFits[hiIndex] = ystar;
yhi = ystarFlu;
}
// and contract
double ystarstar = SimAnnealContract();
double ystarstarFlu = ystarstar - ThermalFluct();
if (ystarstarFlu < yhi)
{
// contraction was an improvement
simplexParEst[hiIndex] = simplexParStarStar;
simplexFits[hiIndex] = ystarstar;
yhi = ystarstarFlu;
opMessage = "SimAnneal - contraction passed ...";
}
else
{
opMessage = "SimAnneal - reset all ...";
SimAnnealReset();
}
}
}
