void Ahrs_ValueChanged(IHardwareComponent sender)
{
IAnalogSensor a = (IAnalogSensor)sender;
int val = a.AnalogValue; // 0v = 0, 5V = 470 approx
/*
* Yaw PWM val
* --------------------
*
*/
double heading;
if (val > 507)
{
heading = GeneralMath.map(val, 508, 1024, 0, 180);
}
else
{
heading = GeneralMath.map(val, 0, 507, 180, 360);
}
//Debug.WriteLine("Ahrs: Value=" + val + " heading: " + heading);
lock (currentSensorsDataLock)
{
ISensorsData sensorsData = new SensorsDataShorty(this.currentSensorsData);
sensorsData.CompassHeadingDegrees = heading;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
public override bool TryValue(ref string newValue)
{
//Test parsability
if (int.TryParse(newValue, out int result))
{
//Test bounds
if (result >= minVal && result <= maxVal)
{
tmpNewValue = newValue;
if (result == GetInnerValue())
{
tmpNewValue = "";
}
nameDirty = true;
return(true);
}
else
{
//Failed bounds check
result = GeneralMath.Clamp(result, minVal, maxVal);
//Update value string
newValue = result.ToString();
return(false);
}
}
//Failed to parse
return(false);
}
/// <summary>
/// Generates the "<paramref name="tone"/>"th note of a 3-octave C Scale
/// </summary>
public FeedbackChords(
bool major,
int tone,
NoteProgression progression = NoteProgression.Scale,
int octaveOffset = 0,
int octaveSpan = 3)
{
this.major = major;
IReadOnlyList <FrequencySet> scale = GetSequence(progression);
//Note spans 3 octaves of CMajor scale.
int notes = scale.Count * octaveSpan + 1;
int note = GeneralMath.Clamp(tone, 0, notes - 1);
int octaves = octaveOffset + note / scale.Count;
int noteIndex = note % scale.Count;
fundamentalFreq = BaseFreqLB * Math.Pow(2, octaves) * scale[noteIndex].freqRatio;
Duration = major ? 0.2 : 0.4;
//Place sound Forward
angle = 0.0;
BuildStream();
}
public void Power()
{
Assert.AreEqual(0, GeneralMath.Power(5, -1));
Assert.AreEqual(0L, GeneralMath.Power(5L, -1));
Assert.AreEqual(0, GeneralMath.Power(0, 5));
Assert.AreEqual(0, GeneralMath.Power(0, 1));
Assert.Throws <InvalidOperationException>(() => GeneralMath.Power(0, 0));
Assert.AreEqual(0L, GeneralMath.Power(0L, 5));
Assert.AreEqual(0L, GeneralMath.Power(0L, 1));
Assert.Throws <InvalidOperationException>(() => GeneralMath.Power(0L, 0));
for (int b = 1; b < 5; b++)
{
int currentBase = 1;
for (int exponent = 0; exponent < 16; exponent++)
{
Assert.AreEqual(currentBase, GeneralMath.Power(b, exponent));
Assert.AreEqual((long)currentBase, GeneralMath.Power((long)b, exponent));
currentBase *= b;
}
}
}
public void Factorial()
{
Assert.AreEqual(1L, GeneralMath.Factorial(0));
var r = new Random();
for (int i = 0; i < 16; i++)
{
int n = -r.Next();
Assert.Throws <ArgumentException>(() => GeneralMath.Factorial(n));
Assert.Throws <ArgumentException>(() => GeneralMath.Factorial((double)n));
}
long currentResult = 1;
for (int multiplier = 1; multiplier <= 20; multiplier++)
{
currentResult *= multiplier;
Assert.AreEqual(currentResult, GeneralMath.Factorial(multiplier));
Assert.AreEqual((double)currentResult, GeneralMath.Factorial((double)multiplier));
}
double currentDoubleResult = currentResult;
for (int multiplier = 21; ; multiplier++)
{
currentDoubleResult *= multiplier;
if (double.IsPositiveInfinity(currentDoubleResult))
{
break;
}
double delta = Math.Pow(10, (long)Math.Log10(currentDoubleResult) - 14);
Assert.AreEqual(currentDoubleResult, GeneralMath.Factorial((double)multiplier), delta, multiplier.ToString());
}
}
public void QuadraticEquation_Test()
{
List <double[]> coefficients = new List <double[]>();
coefficients.Add(new double[] { 2, 0 });
coefficients.Add(new double[] { -0.2, -4 });
coefficients.Add(new double[] { 2.5, -100 });
coefficients.Add(new double[] { 3, -7 });
foreach (double[] pair in coefficients)
{
double[] zeroes = GeneralMath.QuadraticEquation(pair[0], pair[1]);
double mid = (zeroes[0] + zeroes[1]) / 2;
double actualZero0 = System.Math.Round(zeroes[0] * zeroes[0] +
pair[0] * zeroes[0] + pair[1], 10);
double actualZero1 = System.Math.Round(zeroes[1] * zeroes[1] +
pair[0] * zeroes[1] + pair[1], 10);
double actualmid = System.Math.Round(mid * mid +
pair[0] * mid + pair[1], 10);
Assert.AreEqual(0, actualZero0);
Assert.AreEqual(0, actualZero1);
Assert.AreNotEqual(0, mid);
}
}
public static void SPLToAdjustmentDB(
double dbSPLL,
double dbSPLR,
out double dbAdjustL,
out double dbAdjustR,
Calibration.Source source = Calibration.Source.Custom)
{
dbSPLL = GeneralMath.Clamp(dbSPLL, -60, dbMax);
dbSPLR = GeneralMath.Clamp(dbSPLR, -60, dbMax);
//Start with Left calculation
Calibration.GetLevelOffset(
level: dbSPLL,
levelOffsetL: out double dbOffsetL,
levelOffsetR: out double dbOffsetR,
source: source);
dbAdjustL = dbOffsetL + dbSPLL - dbOffset;
//If they're not the same, then generate a new set of offsets
// (It doesn't matter if we mess up OffsetL, we already finished using it)
if (dbSPLL != dbSPLR)
{
//To right calculation if it's different
Calibration.GetLevelOffset(
level: dbSPLR,
levelOffsetL: out dbOffsetL,
levelOffsetR: out dbOffsetR,
source: source);
}
dbAdjustR = dbOffsetR + dbSPLR - dbOffset;
}
/// <summary>
/// Collapses the tone to the nearest chromatic note
/// </summary>
public FeedbackChords(
bool major,
double lateralization,
double tone,
NoteProgression progression = NoteProgression.Scale,
int octaveOffset = 0,
int octaveSpan = 3)
{
this.major = major;
IReadOnlyList <FrequencySet> scale = GetSequence(progression);
int notes = scale.Count * octaveSpan + 1;
int note = (int)Math.Floor(notes * tone);
note = GeneralMath.Clamp(note, 0, notes - 1);
int octaves = octaveOffset + note / scale.Count;
int noteIndex = note % scale.Count;
fundamentalFreq = BaseFreqLB * Math.Pow(2, octaves) * scale[noteIndex].freqRatio;
Duration = major ? 0.2 : 0.4;
//Angle is between -Range/2 and +Range/2
angle = Range * (lateralization - 0.5);
BuildStream();
}
public override void Seek(int position)
{
position = GeneralMath.Clamp(position, 0, ChannelSamples);
stream.Seek(position);
this.position = position;
if (position < attackUpEndSample)
{
envelopeState = EnvelopeState.AttackUp;
currentEnvelope = ENVELOPE_CUTOFF * Math.Pow(1.0 / ENVELOPE_CUTOFF, position / (double)attackUpSamples);
}
else if (position < attackDownEndSample)
{
envelopeState = EnvelopeState.AttackDown;
currentEnvelope = Math.Pow(sustainAmplitude, (position - attackUpEndSample) / (double)attackDownSamples);
}
else if (position < sustainEndSample)
{
envelopeState = EnvelopeState.Sustain;
currentEnvelope = sustainAmplitude * Math.Pow(sustainDecayRate, (position - attackDownEndSample));
}
else
{
envelopeState = EnvelopeState.Released;
currentEnvelope = sustainAmplitude *
Math.Pow(sustainDecayRate, sustainEndSample - attackDownEndSample) *
Math.Pow(releaseDecayRate, position - sustainEndSample);
}
}
/// <summary>
/// Update color value to be between 0 and 1
/// </summary>
/// <param name="color"></param>
/// <param name="colorType"></param>
private static void updateColorValue(ref float color, string colorType)
{
if (color > 1f || color < 0f)
{
Debug.LogWarning($"{colorType} should be set between 0 and 1");
color = GeneralMath.Clamp(color, 0f, 1f);
}
}
public override void Seek(int position)
{
position = GeneralMath.Clamp(position, 0, ChannelSamples);
this.position = position;
stream.Seek(position);
envelopeStream.Seek(position);
}
public void SetPitch(double pitchFactor)
{
pitchFactor = GeneralMath.Clamp(pitchFactor, 0.1, 10.0);
if (lastPitchShiftEffect is not null)
{
lastPitchShiftEffect.PitchFactor = pitchFactor;
}
}
int ISimpleIntStepTemplate.GetValue(int stepNumber)
{
if (DecreaseParameter)
{
stepNumber *= -1;
}
return(GeneralMath.Clamp(BaseValue + BaseStepSize * stepNumber, Minimum, Maximum));
}
public static Color JetMap(float value, float min, float max)
{
float z = 4f * (value - min) / (max - min);
return(new Color(
r: GeneralMath.Clamp(1.5f - Math.Abs(z - 3f), 0f, 1f),
g: GeneralMath.Clamp(1.5f - Math.Abs(z - 2f), 0f, 1f),
b: GeneralMath.Clamp(1.5f - Math.Abs(z - 1f), 0f, 1f)));
}
double ISimpleDoubleStepTemplate.GetPartialValue(double stepNumber)
{
if (DecreaseParameter)
{
stepNumber *= -1;
}
return(GeneralMath.Clamp(BaseValue + BaseStepSize * stepNumber, Minimum, Maximum));
}
public void RadToDeg_Test()
{
foreach (double i in inputDouble)
{
double expected = i / System.Math.PI * 180.0;
double actual = GeneralMath.RadToDeg(i);
Assert.AreEqual(expected, actual);
}
}
double ISimpleDoubleStepTemplate.GetPartialValue(double stepNumber)
{
if (flipSign)
{
stepNumber *= -1;
}
return(GeneralMath.Clamp(ConvergenceValue + delta * Math.Pow(BaseMajorFactor, stepNumber / StepsPerMajorFactor), Minimum, Maximum));
}
public void DegToRad_Test()
{
foreach (double i in inputDouble)
{
double expected = i / 180.0 * System.Math.PI;
double actual = GeneralMath.DegToRad(i);
Assert.AreEqual(expected, actual);
}
}
public static Color JetMap(double value, double min, double max)
{
double z = 4.0 * (value - min) / (max - min);
return(new Color(
r: (float)GeneralMath.Clamp(1.5 - Math.Abs(z - 3.0), 0.0, 1.0),
g: (float)GeneralMath.Clamp(1.5 - Math.Abs(z - 2.0), 0.0, 1.0),
b: (float)GeneralMath.Clamp(1.5 - Math.Abs(z - 1.0), 0.0, 1.0)));
}
public override void Seek(int position)
{
position = GeneralMath.Clamp(position, 0, Samples);
stream.Seek(position);
cycles = position / shifterSamples.Length;
partial = Complex64.FromPolarCoordinates(
magnitude: 1.0,
phase: cycles * cyclePartial);
this.position = position % shifterSamples.Length;
}
public override float ReadNextSample()
{
if (HasMoreSamples())
{
double effectivePos = position++ / (0.5f * Samples) - 1.0f;
return((float)GeneralMath.Tanh(effectivePos * tanhLimit));
}
return(0f);
}
public static int NearestValidOffset(double offset)
{
if (offset < -90.0 || offset > 90.0)
{
Debug.LogError($"Spatialization offset ({offset})is outside of bounds [-90.0,90.0]. Clamping.");
offset = GeneralMath.Clamp(offset, -90.0, 90.0);
}
return((int)Math.Round(10.0 * offset));
}
public override void Seek(int position)
{
currentSample = GeneralMath.Clamp(position, 0, ChannelSamples);
if (position + sampleOffset >= ChannelSamples)
{
stream.Seek(ChannelSamples - (position + sampleOffset));
}
else
{
stream.Seek(position + sampleOffset);
}
}
public override void Seek(int position)
{
position = GeneralMath.Clamp(position, 0, ChannelSamples);
if (position + sampleOffset > 0)
{
stream.Seek(position + sampleOffset);
}
else
{
stream.Reset();
}
}
/// <summary>
/// Run solution for problem 12.
/// </summary>
/// <param name="divisorCountThreshold">Divisor count threshold.</param>
/// <returns>
/// First triangle number to have equal or greater than
/// <paramref name="divisorCountThreshold"/>.
/// </returns>
public static int Run(int divisorCountThreshold)
{
var answer = 0;
var divisors = Array.Empty <int>();
for (var i = 1; divisors.Length + 1 < divisorCountThreshold; i += 1)
{
answer += i;
divisors = GeneralMath.GetProperDivisors(answer).ToArray();
}
return(answer);
}
public override void Seek(int position)
{
position = GeneralMath.Clamp(position, 0, ChannelSamples);
adjIndexA = 0;
adjIndexB = adjustments.Length / 2;
bufferIndex = 0;
bufferCount = 0;
samplesRemaining = -1;
stream.Seek(position);
Array.Clear(oldBuffer, 0, bufferSize);
Array.Clear(newBuffer, 0, bufferSize);
}
public override int Read(float[] data, int offset, int count)
{
int samplesRead = stream.Read(data, offset, count);
for (int i = 0; i < samplesRead; i++)
{
if (data[offset + i] > 1f || data[offset + i] < -1f)
{
data[offset + i] = GeneralMath.Clamp(data[offset + i], -1f, 1f);
}
}
return(samplesRead);
}
public override int Read(float[] data, int offset, int count)
{
int samplesToReturn = Math.Min(count, Samples - position);
for (int i = 0; i < samplesToReturn; i++)
{
double effectivePos = (position + i) / (0.5 * Samples) - 1.0;
data[offset + i] = (float)GeneralMath.Tanh(effectivePos * tanhLimit);
}
position += samplesToReturn;
return(samplesToReturn);
}
private int ReadBody(float[] buffer, int offset, int count)
{
int samplesWritten = GeneralMath.Clamp(count, 0, bufferCount - bufferIndex);
Array.Copy(
sourceArray: outputAccumulation,
sourceIndex: bufferIndex,
destinationArray: buffer,
destinationIndex: offset,
length: samplesWritten);
bufferIndex += samplesWritten;
return(samplesWritten);
}
private void RPiCameraSensor_TargetsChanged(object sender, TargetingCameraEventArgs args)
{
// Raspberry Pi based camera sensor works under Wheezy and uses OpenCV and Python to process
// 240x320 frames and select areas with yellow color. Bearing, inclination and size of blobs is then
// reported over HTTP to RPiCamera (derived from HttpServerBase). Frequency is around 10 FPS.
//Debug.WriteLine("RPi Camera Event: " + args);
// On Raspberry Pi:
// pixy.blocks[i].signature The signature number of the detected object (1-7)
// pixy.blocks[i].x The x location of the center of the detected object (0 to 319)
// pixy.blocks[i].y The y location of the center of the detected object (0 to 199)
// pixy.blocks[i].width The width of the detected object (1 to 320)
// pixy.blocks[i].height The height of the detected object (1 to 200)
// Field of view:
// goal 45 degrees left x=10
// middle x=160
// goal 45 degrees right x=310
//
// goal 30 degrees up y=10
// middle y=90
// goal 30 degrees down y=190
//
if (args.width * args.height > 500) // only large objects count
{
int bearing = GeneralMath.map(args.x, 0, 320, 45, -45);
int inclination = GeneralMath.map(args.y, 0, 200, 30, -30);
//Debug.WriteLine("RPi: bearing=" + bearing + " inclination: " + inclination);
lock (currentSensorsDataLock)
{
ISensorsData sensorsData = new SensorsDataPlucky(this.currentSensorsData);
sensorsData.TargetingCameraBearingDegrees = bearing;
sensorsData.TargetingCameraInclinationDegrees = inclination;
sensorsData.TargetingCameraTimestamp = args.timestamp;
//Debug.WriteLine(sensorsData.ToString());
this.currentSensorsData = sensorsData;
}
}
}
