예제 #1
0
//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);
		}
	}
		
}
예제 #2
0
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();
        }

    }
}