6-DOF PPOD
From Mech
Overview
Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of (INSERT LINK?) Tom Vose in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as (INSERT LINK) Ankur Bakshi, Donald Redding, and Ben Tollberg from the previous class. The PPOD is a vibrating table driven by six speakers acting as actuators. The speakers vibrate at specific frequencies and phases to create force patterns on the surface of the disk that are capable of moving small objects on pre-determined paths. Previous versions used MATLAB to control the entire system.
For our project, we attempted to control the speakers and feedback operations using only the NU_PIC32 microcontroller board. The PIC32 periodically varies a pulse width modulation (PWM) duty cycle (INSERT LINK), which, when sent through a low-pass filter (INSERT LINK), forms a sinusoidal wave that is amplifies to drive the speakers. Readings from accelerometers mounted on the PPOD are compared with the sinusoidal waves using Fast Fourier Transform (FFT) (INSERT LINK), thus creating a transfer function matrix. This matrix characterizes the relationship between the input and the output of the system, and represents the PPOD’s calibration.
Team Members
Jonathan Drake (Mechanical Engineering, 2010)
Caitlin Ramsey (Mechanical Engineering, 2010)
Mechanical Design
There are two main mechanical components to our PPOD—the base and the disk. The base is a 20 inch diameter disk made of ¼” thick acrylic. This is where the speakers and electrical components are attached. There are 3 speakers used for vertical motion and 3, on shelves attached to the disk, used for horizontal motion. The motion of the speakers is transferred to the disk, the vibrating surface, through a series of mechanical connections:
- 1/2" diameter rod is superglued to the interior cavity of the speaker
- 1/4" diameter rod is inserted into a hole in the half inch diameter rod
- Flexible tube is then fitted over the 1/4" rod. On the other end of the flexible rod another small quarter inch rod is inserted
- The smaller quarter inch rod is inserted into rectangles that have a quarter inch hole cut out of the center. The rectangle is then glued to the underside of the disk.
- The horizontal speakers have an extra piece each, a two inch rectangle that is glued to the disk one half and the rectangle is glue to the side of the rectangle. The flexible tubing allows the disk to move without losing the connection with the speakers.
The accelerometers are attached to 1x1x1 inch cubes of solid acrylic that are glued to the underside of the disk. There are three accelerometers attached to the table. The accelerometer mount side of the cube has four holes drilled into each corner to screw the accelerometer board into place. A notch was milled out of the surface for the solder tips, allowing the accelerometer to be flush with the surface of the cube ensuring an accurate reading.
The disk is 11.5 inch in diameter cut from quarter inch thick acrylic. The speaker attachments are all 60 degrees apart, alternating between vertical and horizontal speakers. All speakers are attached in a symmetrical pattern ensuring the accuracy of the calibration program and the force patterns on the surface. Acrylic was chosen for all the components because of its workability in the Ford machine shop and for its ability to be glued together quite easily.
Circuit Design
The PIC32 controls the entire system. We will explain the circuit, following the path from PIC to speaker, for one speaker. Theoretically, if our project was extended, 6 speakers would be included, but we concentrated on a proof of concept with one speaker subsystem. We connected an output compare (OC1) pin to a low pass filter (LPF) circuit, which converts the square wave duty cycle of the PWM into a sinusoidal wave. We chose appropriate values for the resistor (66K) and capacitor (0.01uF) that gave us a good resolution and response time compared to the duty cycle period coming from the PWM. The output of the LPF was connected to the non-inverting input of a unity gain buffer, as well as back into an analog input pin (AN0) on the PIC. This simple LM741 opamp conditions the noisy sine wave of the PIC for input into the amplification circuit. We powered this opamp with +/- 12V. The output of the buffer is connected to the input of the audio amplification circuit, designated Vin on the diagram. This chip, the LM1875, is powered with +/- 12V, and has a gain of 20 (modified by a resistor value). To deal with noise, we boosted the capacitors connected across the rails to 1000uF. The output of the audio amp was connected to the positive terminal of the speaker. The negative terminal of the speaker was connected to ground. The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk, and powered by +3.0V. The X, Y, and Z-axis pins of the accelerometer are connected to analog input pins on the PIC: AN1, AN2, and AN3 respectively. We used a +/- 12V boardlet to power the buffer and amplifier, and the NU32 board’s +3.3V to power the accelerometer.
Code
This code operates 1 speaker and reads in 3 axes from 1 accelerometer. It performs several matrix operations on the quest to form a 9x6 transfer function matrix, but it does not fully solve the problem.
The main code for the project can be found in the file pwm.c. Other C files include cplx.c, which handles complex number declarations and operations, and matrix.c, which handles matrix operations necessary for the transfer function. (INSERT LINK) Download the zip file containing all necessary project files.
/*
PWM.c
*/
/** INCLUDES ***************************************************/
#include "HardwareProfile.h"
#include <math.h>
#include "dsplib_dsp.h"
#include "fftc.h"
#include "cplx.h"
/** Constants **************************************************/
#define TRUE 1
#define FALSE 0
#define DESIRED_BAUDRATE (19200) // The desired BaudRate for RS232
#define PI (3.14159265)
#define N (64) // number of samples per cycle, ALSO CHANGE LOG2N BELOW!
#define SAMPLES_PER_FFT (448) // multiply N by the number of cycles you want to use in the FFT
#define fftc fft16c64 // from fftc.h, for N = 256 use fft16c256, for N = 1024 use fft16c1024
//#define SAMPLE_FREQ (7680) // (freq of wave to generate)*(N, # samples per cycle)
#define NUM_HARMONICS (1)
#define NUM_SPEAKERS (1)
#define NUM_ACCEL (1)
#define PTS_PER_DUTY (256) // set how many times per cycle the duty cycle is changed
#define INPUT_A15 PORTAbits.RA15 // digital input, switch for SendRS232Data()
#define T_MATRIX_ROWS (3)
#define T_MATRIX_COLS (2)
/** Function Declarations **************************************/
void initADC();
void initPWM();
void initPWMinterrupt();
void initSamplingInterrupt();
void initUART2(int pbClk);
void computeFFT();
void sendDataRS232();
void transposeMatrix ();
void eye(float * matrix, int elements, float scalar);
void disp(float * matrix, int rows, int cols);
void multiplyMatrix (float * A, float * B, float * C, int aRows, int aCols, int bRows, int bCols);
void matrix_inverse(float *Min, float *Mout, int actualsize);
float mV_to_duty(float mV);
float getMag(float re, float im);
float getPhase(float re, float im);
float analog_to_mV(float ai);
float mV_to_G(float mV);
float maxG(int b[], int n);
//void zero_dout();
/** Global Variables *******************************************/
char uart_buffer[64];
float mr[2][2] = {{0,0},{0,0}};
/*** sine wave related vars ***/
float freq = 30; // sine wave freq, in Hz, to be generated by PWM
int mVpp = 200; // mV peak-to-peak
float p = 0; // phase angle shift
int duty_cycles[PTS_PER_DUTY] = {0}; // vector to hold duty cycles
int sampleIndex = 0; // index for duty cycle change & sampling
int pwmIndex = 0; // increments to PTS_PER_DUTY
//long int freqVector[N]; // holds multiples of 1st harmonic
// FFT related vars
int computeFFTflag = FALSE; // indicate that an fft computation has been requested
int FFT_RUNNING = FALSE;
int log2N = 6; // e.g. log2(256) = 8
int16c scratch[N]; // for FFT function
int16c dout[N]; // holds computed FFT until transmission, to be reused a few times
// Transfer Function vars
//struct Complex G; // temp. transfer function (tf), single element
//float G_mag; // temp. tf magnitude
//float G_phase; // temp. tf phase angle
struct Complex TF[6][6][NUM_HARMONICS] = {0}; // transfer function matrix
// speaker signal readings (to be FFT "u")
// e.g. speaker[0][]: speaker number #1
int16c speaker[NUM_SPEAKERS][SAMPLES_PER_FFT];
// accelerometer readings (to be FFT "y")
// e.g. accel[0][0][]: accelerometer #1, x-axis
int16c accel[NUM_ACCEL][3][SAMPLES_PER_FFT];
// single-sided FFT result for speaker loopback input
struct Complex sFFT[NUM_SPEAKERS][NUM_HARMONICS];
// single-sided FFT result for accelerometers
// x'',y'',z'',roll,pitch,yaw; 1 harmonic
struct Complex aFFT[NUM_ACCEL][3][NUM_HARMONICS];
//
/*struct Complex testNum;
struct Complex testDen;
struct Complex testRes;*/
float graphFFT[3][32];
int max = 0;
int curr = 0;
float mV = 0;
float grav = 0;
/** Main Function **********************************************/
int main(void)
{
int pbClk,i,j,k;
float dt,amp,offset;
char RS232_Out_Buffer[64];
// Set all analog pins to be digital I/O
// This line of code only needs to be used if your pins are Analog Input (B port)
AD1PCFG = 0xFFFF;
// set inputs
TRISAbits.TRISA15 = 1;
TRISBbits.TRISB0 = 1;
TRISBbits.TRISB6 = 1;
TRISBbits.TRISB7 = 1;
// Configure the proper PB frequency and the number of wait states
pbClk = SYSTEMConfigPerformance(SYS_FREQ);
// Allow vector interrupts
INTEnableSystemMultiVectoredInt();
// initialize functions
mInitAllLEDs();
initUART2(pbClk);
initADC();
// ...
// make sine wave vector
dt = 1/freq/PTS_PER_DUTY;
//dt = 8.33333e-4;
//mVamp = (mV_top-mV_bottom)/2;
amp = mV_to_duty(mVpp); // peak-to-peak amplitude of sine wave
offset = 2*amp;
//sprintf(RS232_Out_Buffer, "%3.1f %3.1f %3.1f\r\n", dt, amp, offset);
//putsUART2(RS232_Out_Buffer);
for (i=0; i<PTS_PER_DUTY; i++) {
duty_cycles[i] = amp*sin(2*PI*freq*(i*dt)+p)+offset;
//sprintf(uart_buffer, "i=%d, %d\r\n", i, duty_cycles[i]);
//putsUART2(uart_buffer);
}
// zero some vectors
for (i=0; i<N; i++) {
dout[i].re = 0;
dout[i].im = 0;
}
// ...
initPWM();
initPWMinterrupt();
initSamplingInterrupt();
mLED_0_Toggle();
putsUART2("\r\n***** SYSTEM INITIALIZED *****\r\n\n");
// end inits
/*
int x = 510;
sprintf(RS232_Out_Buffer,"%d %3.1f %3.1f\n\r", x, analog_to_mV(x), mV_to_G(analog_to_mV(x)));
putsUART2(RS232_Out_Buffer);
/*
/* putsUART2("\r\n\n TRANSPOSE \r\n");
float M1[3][2] = {{1,2},{3,4},{5,6}};
float M2[2][3];
transposeMatrix(M1,M2,3,2);
sprintf(RS232_Out_Buffer,"%3.1f %3.1f %3.1f\n\r%3.1f %3.1f %3.1f", M2[0][0], M2[0][1], M2[0][2], M2[1][0], M2[1][1], M2[1][2]);
putsUART2(RS232_Out_Buffer);
*/
putsUART2("*** MATRIX MULTIPLICATION TEST ***\r\n");
float a[2][3] = {{1,0,2},{-1,3,1}};
float B[3][2] = {{3,1},{2,1},{1,0}};
float C[2][2];
multiplyMatrix(&a,&B,&C,2,3,3,2);
disp(&C,2,2);
putsUART2("*** INVERSE TEST ***\r\n");
float mat1[2][2] = {{1,2},{3,4}};
float mati[2][2];
matrix_inverse(&mat1, &mati, 2);
disp(&mati, 2, 2);
float pi = 3.141592654;
float th1 = 0;
float th2 = 2*pi/3;
float th3 = 4*pi/3;
float h = -1;
float r = 5;
sprintf(RS232_Out_Buffer,"Constants:\r\n%3.3f %3.3f %3.3f %3.3f %3.1f %3.1f \r\n\n", pi, th1, th2, th3, h, r);
putsUART2(RS232_Out_Buffer);
float A[9][6] = {
{1,0,0, 0, r*sinf(th1), -h},
{0,1,0, -r*sinf(th1), 0, r*cosf(th1)},
{0,0,1, h, -r*cosf(th1), 0},
{1,0,0, 0, r*sinf(th2), -h},
{0,1,0, -r*sinf(th2), 0, r*cosf(th2)},
{0,0,1, h, -r*cosf(th2), 0},
{1,0,0, 0, r*sinf(th3), -h},
{0,1,0, -r*sinf(th3), 0, r*cosf(th3)},
{0,0,1, h, -r*cosf(th3), 0}
};
float rot1[3][3] = {
{cosf(th1), 0, -sinf(th1)},
{0, 1, 0},
{sinf(th1), 0, cosf(th1)}};
float rot2[3][3] = {
{cosf(th2), 0, -sinf(th2)},
{0, 1, 0},
{sinf(th2), 0, cosf(th2)}};
float rot3[3][3] = {
{cosf(th3), 0, -sinf(th3)},
{0, 1, 0},
{sinf(th3), 0, cosf(th3)}};
float b2a1[3][1] = {0,1,0};
float b2a2[3][1] = {0,1,0};
float b2a3[3][1] = {0,1,0};
/********** BUILD B VECTOR FROM ROTATION, ACCELERATIONS *************/
float b_sub[3][1],b[9][1];
// build piece from accel#1
multiplyMatrix(&rot1,&b2a1,&b_sub,3,3,3,1);
b[0][0] = b_sub[0][0];
b[1][0] = b_sub[1][0];
b[2][0] = b_sub[2][0];
//disp(&b_sub, 3, 1);
// build piece from accel#2
multiplyMatrix(&rot2,&b2a2,&b_sub,3,3,3,1);
b[3][0] = b_sub[0][0];
b[4][0] = b_sub[1][0];
b[5][0] = b_sub[2][0];
//disp(&b_sub, 3, 1);
// build piece from accel#3
multiplyMatrix(&rot3,&b2a3,&b_sub,3,3,3,1);
b[6][0] = b_sub[0][0];
b[7][0] = b_sub[1][0];
b[8][0] = b_sub[2][0];
//disp(&b_sub, 3, 1);
putsUART2("*** b ***\r\n");
disp(&b, 9, 1);
/********** CALCULATE X! *************/
putsUART2("*** A ***\r\n");
disp(&A, 9, 6);
// transpose A
float At[6][9];
transposeMatrix(A,At,9,6); // 9x6 -> 6x9
putsUART2("*** At ***\r\n");
disp(&At,6,9);
// A*At
float AxAt[6][6];
multiplyMatrix(&A,&At,&AxAt,9,6,6,9); // 9x6 * 6x9 = 6x6
putsUART2("*** AxAt ***\r\n");
disp(&AxAt,6,6);
// invert the product of A*At
float AxAtinv[6][6];
matrix_inverse(&AxAt,&AxAtinv,6); // 6x6 -> 6x6
putsUART2("*** AxAtinv ***\r\n");
disp(&AxAtinv,6,6);
float crap[6][6] = {{4,0,0,0,0,0},{0,4,0,0,0,0},{0,0,4,0,0,0},{0,0,0,4,0,0},{0,0,0,0,4,0},{0,0,0,0,0,4}};
float invcrap[6][6];
matrix_inverse(&crap,&invcrap,6);
putsUART2("*** (crap) ***\r\n");
disp(&invcrap,6,6);
// inv(A*At)*At
/* float AxAtinvxAt[6][9];
multiplyMatrix(&AxAtinv,&At,&AxAtinvxAt,6,6,6,9); // 6x6 * 6x9 = 6x9
putsUART2("*** AxAtinvxAt ***\r\n");
disp(&AxAtinvxAt,6,9);
// inv(A*At)*At*b = X
float X[6][1];
multiplyMatrix(&AxAtinvxAt,&b,&X,6,9,9,1); // 6x9 * 9x1 = 6x1
putsUART2("*** X ***\r\n");
disp(&X,6,1);
*/
while(1)
{
if (!swUser) { // if user button is pressed
computeFFTflag = TRUE;
}
if ((computeFFTflag == TRUE) & (sampleIndex == (SAMPLES_PER_FFT-1))) {
putsUART2("\r\nFFT start\r\n");
computeFFT();
while(FFT_RUNNING){}
sendDataRS232();
computeFFTflag = FALSE;
putsUART2("FFT done\r\n\n");
}
if (!INPUT_A15) {
sprintf(RS232_Out_Buffer,"maxG= %d %3.1f %3.1f\n\r", max, analog_to_mV(max), mV_to_G(analog_to_mV(max)));
putsUART2(RS232_Out_Buffer);
/*for (i=0; i<SAMPLES_PER_FFT; i+=2) {
sprintf(RS232_Out_Buffer,"(%d) %d %d %d\n\r", i, speaker[0][i].re, accel[0][0][i].re, accel[0][1][i].re);
putsUART2(RS232_Out_Buffer);
}*/
}
}
CloseOC1(); // turn off PWM
} //end main
/** Interrupt Handlers *****************************************/
// PWM interupt code for the timer 4
void __ISR( _TIMER_4_VECTOR, ipl6) T4Interrupt( void)
{
char RS232_Out_Buffer[64];
// set duty cycle
SetDCOC1PWM(duty_cycles[pwmIndex]);
// at the end of 1 cycle?
if (pwmIndex==(PTS_PER_DUTY-1)) {
//sprintf(RS232_Out_Buffer, "%d %d\r\n", pwmIndex, duty_cycles[pwmIndex]);
//putsUART2(RS232_Out_Buffer);
mLED_1_Toggle();
pwmIndex=0; // reset sampleIndex to start cycle again
} else {
// increment sampleIndex
pwmIndex++;
}
// clear interrupt flag and exit
mT4ClearIntFlag();
} // end T4 Interrupt
// interupt code for the timer 3
void __ISR( _TIMER_3_VECTOR, ipl7) T3Interrupt( void)
{
char RS232_Out_Buffer[64];
int a,s,axis,ai;
ai = 0; // which analog-in pin (start with B0)
/****** READ SPEAKER SIGNALS ******/
for (s=0; s<NUM_SPEAKERS; s++) {
speaker[s][sampleIndex].re = ReadADC10(ai);
speaker[s][sampleIndex].im = 0;
ai+=1;
}
/****** ACCELEROMETERs ******/
for (a=ai; a<NUM_ACCEL; a++) {
for (axis=0; axis<3; axis++) {
accel[a][axis][sampleIndex].re = ReadADC10(ai);
accel[a][axis][sampleIndex].im = 0;
ai+=1;
}
}
// max g?
int curr = accel[0][1][sampleIndex].re;
if (curr > max) {
max = curr;
}
// if you have collected SAMPLES_PER_FFT samples, then restart
if (sampleIndex==(SAMPLES_PER_FFT-1)) {
sampleIndex=0; // reset sampleIndex to start cycle again
} else {
// increment sampleIndex
sampleIndex++;
}
// clear interrupt flag and exit
mT3ClearIntFlag();
} // end T3 Interrupt
/** Other Functions ********************************************/
void initADC(void) {
// configure and enable the ADC
CloseADC10(); // ensure the ADC is off before setting the configuration
// define setup parameters for OpenADC10
// Turn module on | output in integer | trigger mode auto | enable autosample
#define PARAM1 ADC_MODULE_ON | ADC_FORMAT_INTG | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_ON
// define setup parameters for OpenADC10
// ADC ref external | disable offset test | enable scan mode | perform 12 samples | use one buffer | use MUXA mode
#define PARAM2 ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_ON | ADC_SAMPLES_PER_INT_12 | ADC_ALT_BUF_OFF | ADC_ALT_INPUT_OFF
// define setup parameters for OpenADC10
// use ADC internal clock | set sample time
#define PARAM3 ADC_CONV_CLK_INTERNAL_RC | ADC_SAMPLE_TIME_15
// define setup parameters for OpenADC10
#define PARAM4 ENABLE_AN0_ANA | ENABLE_AN1_ANA | ENABLE_AN2_ANA | ENABLE_AN3_ANA | ENABLE_AN4_ANA | ENABLE_AN5_ANA | ENABLE_AN6_ANA | ENABLE_AN7_ANA | ENABLE_AN8_ANA | ENABLE_AN9_ANA | ENABLE_AN10_ANA | ENABLE_AN11_ANA
// define setup parameters for OpenADC10
// do not assign channels to scan
// don't skip channel 1,2,3,4
#define PARAM5 SKIP_SCAN_AN12 | SKIP_SCAN_AN13 | SKIP_SCAN_AN14 | SKIP_SCAN_AN15
// configure
SetChanADC10(ADC_CH0_NEG_SAMPLEA_NVREF);
OpenADC10( PARAM1, PARAM2, PARAM3, PARAM4, PARAM5 ); // configure ADC using parameter define above
EnableADC10(); // Enable the ADC
while ( ! mAD1GetIntFlag() ) { } // wait for the first conversion to complete so there will be valid data in ADC result registers
}
void initPWM(void)
{
// init OC1 module, on pin D0
OpenOC1( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0);
// init Timer2 mode and period PR2...
// if set for 30Hz, PR=41666=0xA2C2, Pre = 64
// if set for 20kHz, PR=3999=0x0F9F, Pre = 1
OpenTimer2( T2_ON | T2_PS_1_1 | T2_SOURCE_INT, 0x0F9F); // every 1/20000 seconds
SetDCOC1PWM(0);
}
void initPWMinterrupt(void)
{
OpenTimer4( T4_ON | T4_PS_1_1 | T4_SOURCE_INT, 0x28B0); // every 1/freq/PTS_PER_DUTY seconds
mT4SetIntPriority( 6);
mT4ClearIntFlag();
mT4IntEnable( 1);
}
void initSamplingInterrupt(void)
{
// init Timer3 mode and period (PR3)
OpenTimer3( T3_ON | T3_PS_1_1 | T3_SOURCE_INT, 0xA2C2); // every 1/freq/N seconds
mT3SetIntPriority( 7); // set Timer3 Interrupt Priority
mT3ClearIntFlag(); // clear interrupt flag
mT3IntEnable( 1); // enable timer3 interrupts
}
void initUART2(int pbClk)
{
// define setup Configuration 1 for OpenUARTx
// Module Enable
// Work in IDLE mode
// Communication through usual pins
// Disable wake-up
// Loop back disabled
// Input to Capture module from ICx pin
// no parity 8 bit
// 1 stop bit
// IRDA encoder and decoder disabled
// CTS and RTS pins are disabled
// UxRX idle state is '1'
// 16x baud clock - normal speed
#define config1 UART_EN | UART_IDLE_CON | UART_RX_TX | UART_DIS_WAKE | UART_DIS_LOOPBACK | UART_DIS_ABAUD | UART_NO_PAR_8BIT | UART_1STOPBIT | UART_IRDA_DIS | UART_DIS_BCLK_CTS_RTS| UART_NORMAL_RX | UART_BRGH_SIXTEEN
// define setup Configuration 2 for OpenUARTx
// IrDA encoded UxTX idle state is '0'
// Enable UxRX pin
// Enable UxTX pin
// Interrupt on transfer of every character to TSR
// Interrupt on every char received
// Disable 9-bit address detect
// Rx Buffer Over run status bit clear
#define config2 UART_TX_PIN_LOW | UART_RX_ENABLE | UART_TX_ENABLE | UART_INT_TX | UART_INT_RX_CHAR | UART_ADR_DETECT_DIS | UART_RX_OVERRUN_CLEAR
// Open UART2 with config1 and config2
OpenUART2( config1, config2, pbClk/16/DESIRED_BAUDRATE-1); // calculate actual BAUD generate value.
// Configure UART2 RX Interrupt with priority 2
ConfigIntUART2(UART_INT_PR2 | UART_RX_INT_EN);
}
void sendDataRS232()
{
int i;
char RS232_Out_Buffer[64]; // max characters per line (line feed and carriage return count)
sprintf(RS232_Out_Buffer,"START 1 1\n\r"); //print START. MATLAB uses this as a start delimiter
putsUART2(RS232_Out_Buffer);
//sprintf(RS232_Out_Buffer,"ROWS=%d\n\r", N); //print the number of rows, so matlab can make the correct sized buffer
//putsUART2(RS232_Out_Buffer);
//int limit = SAMPLES_PER_FFT;
int limit = 10;
for (i=0; i<limit; i++) {
//sprintf(RS232_Out_Buffer,"%d %d %d\n\r", speaker[0][i].re, accel[0][0][i].re, accel[0][1][i].re);
sprintf(RS232_Out_Buffer,"%3.1f %3.1f %3.1f\n\r", graphFFT[0][i], graphFFT[1][i], graphFFT[2][i]);
putsUART2(RS232_Out_Buffer);
}
sprintf(RS232_Out_Buffer,"END 1 1\n\r"); //output end so matlab knows we're done
putsUART2(RS232_Out_Buffer);
}
void computeFFT()
{
int a,i,j,h,s;
char RS232_Out_Buffer[64]; // max characters per line (line feed and carriage return count)
mT3IntEnable(0); //turns off interrupt while computing FFT
FFT_RUNNING = TRUE;
/******** TEST COMPLEX OPERATIONS ********/
/* testNum.r = 1;
testNum.i = 2;
testDen.r = 4;
testDen.i = -3;
testRes = C_div(testNum,testDen);
sprintf(RS232_Out_Buffer,"test: %3.1f+%3.1f*i, mag=%3.1f, phase=%3.1f\n\r", testRes.r, testRes.i, getMag(testRes.r,testRes.i), getPhase(testRes.r,testRes.i));
putsUART2(RS232_Out_Buffer);
*/
/******** FFT THE SPEAKER SIGNALS *******/
for (s=0; s<NUM_SPEAKERS; s++) {
mips_fft16(dout, speaker[s], fftc, scratch, log2N); // input: speaker[s], output: dout
for (i=0; i<(N/2); i++) {
graphFFT[0][i] = getMag(dout[i].re,dout[i].im);
//sprintf(RS232_Out_Buffer,"(S) %d, %d+%d*i, %3.1f\n\r", i, dout[i].re, dout[i].im, getMag(dout[i].re,dout[i].im));
//putsUART2(RS232_Out_Buffer);
}
for(h = 1; h<=NUM_HARMONICS; h++) {
sFFT[s][h].r = dout[h].re;
sFFT[s][h].i = dout[h].im;
}
/* sprintf(RS232_Out_Buffer,"speaker#%d, %3.1f+%3.1f*i\n\r", s, sFFT[s][1].r, sFFT[s][1].i);
putsUART2(RS232_Out_Buffer);
*/
}
/******** ACCELEROMETER FFT *******/
for (a=0; a<NUM_ACCEL; a++) {
/******** x-axis FFT *******/
if ((a==0)|(a==1)) {
mips_fft16(dout, accel[a][0], fftc, scratch, log2N);
for (i=0; i<(N/2); i++) {
graphFFT[1][i] = getMag(dout[i].re,dout[i].im);
}
for(h = 1; h<=NUM_HARMONICS; h++)
{
aFFT[a][0][h].r = dout[h].re;
aFFT[a][0][h].i = dout[h].im;
}
/* sprintf(RS232_Out_Buffer,"accel#%d, X-axis, %d+%d*i\n\r", a, aFFT[a][0][1].r, aFFT[a][0][1].i);
putsUART2(RS232_Out_Buffer);
*/
}
/******** y-axis FFT *******/
if ((a==0)|(a==2)) {
mips_fft16(dout, accel[a][1], fftc, scratch, log2N);
for (i=0; i<(N/2); i++) {
graphFFT[2][i] = getMag(dout[i].re,dout[i].im);
}
for(h = 1; h<=NUM_HARMONICS; h++)
{
aFFT[a][1][h].r = dout[h].re;
aFFT[a][1][h].i = dout[h].im;
}
/* sprintf(RS232_Out_Buffer,"accel#%d, Y-axis, %d+%d*i\n\r", a, aFFT[a][1][1].r, aFFT[a][1][1].i);
putsUART2(RS232_Out_Buffer);
*/
}
/******** z-axis FFT *******/
if ((a==1)|(a==2)) {
mips_fft16(dout, accel[a][2], fftc, scratch, log2N);
for(h = 1; h<=NUM_HARMONICS; h++)
{
aFFT[a][2][h].r = dout[h].re;
aFFT[a][2][h].i = dout[h].im;
}
/* sprintf(RS232_Out_Buffer,"accel#%d, Z-axis, %d+%d*i\n\r", a, aFFT[a][2][1].r, aFFT[a][2][1].i);
putsUART2(RS232_Out_Buffer);
*/
}
}
/******** FIND TRANSFER FUNCTIONS *******/
TF[0][0][1] = C_div(aFFT[0][0][1],sFFT[0][1]);
TF[1][0][1] = C_div(aFFT[0][1][1],sFFT[0][1]);
for(i = 0; i < 2; i++) {
// output: frequency vector, the calculated fft, raw samples
sprintf(RS232_Out_Buffer,"TF(%d,0,1): %3.1f+%3.1f*i, mag=%3.1f, phase=%3.1f\n\r", i, TF[i][0][1].r, TF[i][0][1].i, getMag(TF[i][0][1].r, TF[i][0][1].i), getPhase(TF[i][0][1].r, TF[i][0][1].i));
putsUART2(RS232_Out_Buffer);
}
// SHOW MAX G-FORCE
// int n = SAMPLES_PER_FFT;
// sprintf(RS232_Out_Buffer,"maxG: %3.1f %3.1f\n\r", maxG(accel[0][0],n), maxG(accel[0][1],n));
// putsUART2(RS232_Out_Buffer);
FFT_RUNNING = FALSE;
mT3IntEnable(1); //turn interrupt back on
}
void transposeMatrix (float M1[9][6], float M2[6][9], int rows, int cols) {
int i, j;
for ( i = 0; i < rows; i++) {
for ( j = 0; j < cols; j++) {
M2[j][i] = M1[i][j];
}
}
}
void multiplyMatrix (float * A, float * B, float * C, int aRows, int aCols, int bRows, int bCols)
{
/*
Action: A*B=C
Requirements: aCols = bRows
*/
int i, j, k, cCols;
cCols = aRows;
for (i = 0; i < aRows; i++) {
for (j = 0; j < bCols; j++) {
C[(cCols*i)+j] = 0;
for (k = 0; k < aCols; k++) {
C[(cCols*i)+j] += A[(aCols*i)+k]*B[(bCols*k)+j];
}
}
}
}
void disp(float * matrix, int rows, int cols) {
int i, j;
char buff[8];
for (i=0; i<rows; i++) {
for (j=0; j<cols; j++) {
sprintf(buff,"%6.3f ", matrix[(cols*i)+j]);
putsUART2(buff);
}
putsUART2("\r\n");
}
putsUART2("\r\n");
}
void eye(float * matrix, int elements, float scalar) {
int i,j;
for (i=0; i<elements; i++) {
for (j=0; j<elements; i++) {
matrix[(elements*i)+j] = scalar;
}
}
}
void matrix_inverse(float *Min, float *Mout, int actualsize) {
/* This function calculates the inverse of a square matrix
*
* matrix_inverse(double *Min, double *Mout, int actualsize)
*
* Min : Pointer to Input square Double Matrix
* Mout : Pointer to Output (empty) memory space with size of Min
* actualsize : The number of rows/columns
*
* Notes:
* - the matrix must be invertible
* - there's no pivoting of rows or columns, hence,
* accuracy might not be adequate for your needs.
*
* Code is rewritten from c++ template code Mike Dinolfo
*/
/* Loop variables */
int i, j, k;
/* Sum variables */
float sum,x;
/* Copy the input matrix to output matrix */
for(i=0; i<actualsize*actualsize; i++) { Mout[i]=Min[i]; }
/* Add small value to diagonal if diagonal is zero */
for(i=0; i<actualsize; i++)
{
j=i*actualsize+i;
if((Mout[j]<1e-12)&&(Mout[j]>-1e-12)){ Mout[j]=1e-12; }
}
/* Matrix size must be larger than one */
if (actualsize <= 1) return;
for (i=1; i < actualsize; i++) {
Mout[i] /= Mout[0]; /* normalize row 0 */
}
for (i=1; i < actualsize; i++) {
for (j=i; j < actualsize; j++) { /* do a column of L */
sum = 0.0;
for (k = 0; k < i; k++) {
sum += Mout[j*actualsize+k] * Mout[k*actualsize+i];
}
Mout[j*actualsize+i] -= sum;
}
if (i == actualsize-1) continue;
for (j=i+1; j < actualsize; j++) { /* do a row of U */
sum = 0.0;
for (k = 0; k < i; k++) {
sum += Mout[i*actualsize+k]*Mout[k*actualsize+j];
}
Mout[i*actualsize+j] = (Mout[i*actualsize+j]-sum) / Mout[i*actualsize+i];
}
}
for ( i = 0; i < actualsize; i++ ) /* invert L */ {
for ( j = i; j < actualsize; j++ ) {
x = 1.0;
if ( i != j ) {
x = 0.0;
for ( k = i; k < j; k++ ) {
x -= Mout[j*actualsize+k]*Mout[k*actualsize+i];
}
}
Mout[j*actualsize+i] = x / Mout[j*actualsize+j];
}
}
for ( i = 0; i < actualsize; i++ ) /* invert U */ {
for ( j = i; j < actualsize; j++ ) {
if ( i == j ) continue;
sum = 0.0;
for ( k = i; k < j; k++ ) {
sum += Mout[k*actualsize+j]*( (i==k) ? 1.0 : Mout[i*actualsize+k] );
}
Mout[i*actualsize+j] = -sum;
}
}
for ( i = 0; i < actualsize; i++ ) /* final inversion */ {
for ( j = 0; j < actualsize; j++ ) {
sum = 0.0;
for ( k = ((i>j)?i:j); k < actualsize; k++ ) {
sum += ((j==k)?1.0:Mout[j*actualsize+k])*Mout[k*actualsize+i];
}
Mout[j*actualsize+i] = sum;
}
}
}
float mV_to_duty(float mV) {
float result = 0.817*mV+40.833;
return result;
}
float getMag(float re, float im) {
float mag = sqrtf((re*re)+(im*im));
return mag;
}
float getPhase(float re, float im) {
float phase = atanf(im/re);
return phase;
}
float analog_to_mV(float ai) {
float r = ai*3300/1023;
return r;
}
float mV_to_G(float mV) {
float r = (mV-1650)/174;
return r;
}
float maxG(int b[], int n) {
int max = 0;
int c;
for (c=0; c<n; c++) {
if(b[c]>max) {
max = b[c];
}
}
float r = mV_to_G(analog_to_mV(max));
return r;
}
