// Audio
#include <Audio.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <SerialFlash.h>
// EMD645 Sound Spectral Data
#include "specdata.h"
typedef float T;
// GUItool: begin automatically generated code
AudioPlayQueue queue1; //xy=917,329
AudioOutputI2S i2s2; //xy=1121,330
AudioConnection patchCord1(queue1, 0, i2s2, 0);
AudioConnection patchCord2(queue1, 0, i2s2, 1);
AudioControlSGTL5000 sgtl5000_1; //xy=1131,379
// GUItool: end automatically generated code
//
#define COUNT_OF(a) (sizeof a / sizeof a[0])
//
/* Time Keeping: */
unsigned long t1, t2, t3;
/* iFFT/RPM Interpolation Update Time: */
unsigned long CycleTime_ms = 371.5;
/* Dimension of Spectral Data: */
const int ncol = 8192; // Numner of spectral Lines
const int mrow = 9; // 8 Notches + Idle = 9 RPMs
unsigned long int N = ncol*2;
T MyBuffer[ncol*2]; // Buffer containing 16384 samples from iFFT
int currIndx = 0; // Points at current index of MyBuffer
T a[ncol*4]; // For four1 FFT routine
/* Ampitude Modulation Function Variables: */
T psiAmp = 0.00;
T ModMag = 0.25;
T dTglobal = (T) CycleTime_ms / 1000.0f;
T sqrtTwo = sqrt(2.0);
T PI_over_30 = (PI / 30.0);
T dtLocal = 1.0f/44100.0f;
T dtLocal_x_PI_over_30 = dtLocal * PI_over_30;
// JoyStick Pins:
T RPM_RiseRate = 100.0f; // +100 rpm/sec
T RPM_FallRate = -100.0f; // -100 rpm/sec
const int pin_js_y = 15;
float currRPM, prevRPM;
void setup()
{
/* SETUP AUDIO: */
/////////////////////////////////////////////////////////////////////////////////////////
pinMode(13,OUTPUT);
// Open serial communications and wait for port to open:
Serial.begin(115200); // wait for serial port to connect.
while (!Serial);
delay(100);
if (CrashReport)
{
Serial.println(CrashReport);
CrashReport.clear();
}
AudioMemory(5);
sgtl5000_1.enable();
sgtl5000_1.volume(0.8);
queue1.setBehaviour(AudioPlayQueue::NON_STALLING);
queue1.setMaxBuffers(5);
//////////////////////////////////////////////////////////////////////////////////////////////
/* END SETUP AUDIO */
/* Fillout the sample buffer for Teensy: */
int i,j;
currIndx = 0;
prevRPM = 255.0;
currRPM = 255.5;
/* Call iFFT to Interpolate Spectrum @ operating RPM & generate audio samples: */
t1 = millis();
iFFT();
t2 = millis();
Serial.println(t2-t1);
}
void loop()
{
/* Read current RPM: */
get_RPM();
if (currIndx > 2*ncol-1)
{
iFFT();
currIndx=0;
} else {
int16_t* buf = queue1.getBuffer();
if (NULL != buf)
{
for (int i=0;i<AUDIO_BLOCK_SAMPLES;i++)
{
T A = AmpMod();
buf[i] = (int16_t) 8000.0*A*MyBuffer[currIndx];
currIndx++;
}
queue1.playBuffer();
}
}
}
/* Amplitude Modulation Function: */
T AmpMod()
{
/* w = RPM * PI / 30; psi = psi + w*dT */
psiAmp += currRPM * dtLocal_x_PI_over_30;
/* Modulus Operation: psi < 2*PI */
if (psiAmp > TWO_PI) psiAmp -= TWO_PI;
return ( 1.0 - ModMag*(1.0 - cos(psiAmp)) );
}
/* Get RPM From Analog Pin: */
void get_RPM()
{
int val = analogRead(pin_js_y);
currRPM = (T) map(val, 0, 1023, 255, 907);
currRPM = RateLimiter(dTglobal, RPM_RiseRate, RPM_FallRate, currRPM, prevRPM);
prevRPM = currRPM;
}
/* Interpolate Spectrum & Perform iFFT: */
void iFFT()
{
int i,j,idL=0,idU=1;
T Slope, Mag;
/* Initialize: Very Very Very Important!!! */
for (i=0; i<4*ncol; i++) a[i] = 0.0;
/* Bracket rpm: */
for (i=0; i<mrow-1; i++)
{
if ( (currRPM >= rpm[i]) && (currRPM <= rpm[i+1]) )
{
idL = i;
idU = i+1;
break;
}
}
// Interpolate Spectrum @ operating RPM:
j=0;
for (i=0; i < ncol*2; i +=2)
{
if (i >= ncol)
{
a[i] = a[i+1] = 0.0;
} else {
phi[j] += currRPM * dtLocal_x_PI_over_30;
if (phi[j] > TWO_PI) phi[j] -= TWO_PI;
Slope = (Spectrum[j][idU] - Spectrum[j][idL])/(rpm[idU] - rpm[idL]);
Mag = Spectrum[j][idL] + Slope*(currRPM - rpm[idL]);
a[i] = Mag*cos(phi[j]);
a[i+1] = Mag*sin(phi[j]);
j++;
}
}
// Call FFT:
int isign =-1;
four1(a-1, N, isign);
j=0;
for (i=0; i < ncol*4; i += 2)
{
MyBuffer[j] = sqrtTwo*a[i];
j++;
}
}
/* FFT Routine from Numerical Recipes: */
void four1(T *data, unsigned long nn, int isign)
{
unsigned long n, mmax, m, j, istep, i;
//double wtemp, wr, wpr, wpi, wi, theta;
float wtemp, wr, wpr, wpi, wi, theta;
float tempr, tempi;
n = nn << 1;
j = 1;
for (i=1; i<n; i+=2)
{
if (j > i)
{
swap(data[j],data[i]);
swap(data[j+1],data[i+1]);
}
m = n >> 1;
while (m >= 2 && j > m)
{
j -= m;
m >>= 1;
}
j += m;
}
mmax = 2;
while (n > mmax)
{
istep = mmax << 1;
theta = isign*(2.0*PI/mmax);
wtemp = sin(0.5*theta);
wpr = -2.0*wtemp*wtemp;
wpi = sin(theta);
wr = 1.0;
wi = 0.0;
for (m=1; m<mmax; m+=2)
{
for (i=m; i<=n; i+=istep)
{
j = i + mmax;
tempr = wr*data[j]-wi*data[j+1];
tempi = wr*data[j+1]+wi*data[j];
data[j] = data[i]-tempr;
data[j+1] = data[i+1]-tempi;
data[i] += tempr;
data[i+1] += tempi;
}
wr = (wtemp=wr)*wpr-wi*wpi+wr;
wi = wi*wpr+wtemp*wpi+wi;
}
mmax = istep;
}
}
void swap(T &x, T &y)
{
T temp = x;
x = y;
y = temp;
}
/* Rate-Limiter Function: */
T RateLimiter(T dt, T R, T F, T u, T yo)
{
/* LINEAR DYNAMIC RAMP:
R -> Rising Slew Rate; F -> Falling Slew Rate; t -> Current time t
u -> Input; y -> Output;
Note: F must be negative
*/
T rate, y;
rate = (u - yo)/dt;
if (rate > R) /* Rising Slew Rate: */
{
y = dt*R + yo;
} else if (rate < F) {
y = dt*F + yo; /* Falling Slew Rate: */
} else {
y = u;
}
return y;
}
// EOF