Mech - User contributions [en]

6-DOF PPOD

2010-03-18T22:29:08Z

JonathanDrake: /* Mechanical Design */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com Caitlin Ramsey] (Mechanical Engineering, 2010) 

== Mechanical Design ==

===Parts List===

<ul>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
</ul>

===Construction===

There are two main mechanical components to our PPOD—the base and the disk. The base is a 20 inch diameter disk made of 1/4" 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
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.

===Parts List===

This list is for a 1 speaker, 1 accelerometer setup. Each PIC can theoretically handle 2 speakers.

<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32|NU32 board]] configuration</li>
<li>Low-Pass Filter: 66K resistor, 0.01uF capacitor</li>
<li>Unity Gain Buffer: [http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&name=LM741CNFS-ND LM741] general purpose op-amp</li>
<li>Speaker Amp: [http://www.national.com/ds/LM/LM1875.pdf LM1875] 20W Audio Power Amplifier</li>
<li>Heat sink: attached to LM1875</li>
<li> Accelerometer: [http://www.analog.com/en/mems/low-g-accelerometers/adxl325/products/product.html ADXL325] 3-AXIS ±5 G</li>
<li>Speakers: generic 6 ohm, 2" diameter (INSERT SPEAKER SPECs, link)</li>
</ul>

===Build the Circuit===

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:25:06Z

JonathanDrake: /* Parts List */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
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.

===Parts List===

This list is for a 1 speaker, 1 accelerometer setup. Each PIC can theoretically handle 2 speakers.

<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32|NU32 board]] configuration</li>
<li>Low-Pass Filter: 66K resistor, 0.01uF capacitor</li>
<li>Unity Gain Buffer: [http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&name=LM741CNFS-ND LM741] general purpose op-amp</li>
<li>Speaker Amp: [http://www.national.com/ds/LM/LM1875.pdf LM1875] 20W Audio Power Amplifier</li>
<li>Heat sink: attached to LM1875</li>
<li> Accelerometer: [http://www.analog.com/en/mems/low-g-accelerometers/adxl325/products/product.html ADXL325] 3-AXIS ±5 G</li>
<li>Speakers: generic 6 ohm, 2" diameter (INSERT SPEAKER SPECs, link)</li>
</ul>

===Build the Circuit===

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:24:15Z

JonathanDrake: /* Parts List */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
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.

===Parts List===

This list is for a 1 speaker, 1 accelerometer setup. Each PIC can theoretically handle 2 speakers.

<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32|NU32 board]] configuration</li>
<li>Low-Pass Filter: 66K resistor, 0.01uF capacitor</li>
<li>Unity Gain Buffer: [http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&name=LM741CNFS-ND LM741] general purpose op-amp</li>
<li>Speaker Amp: [http://www.national.com/ds/LM/LM1875.pdf LM1875] 20W Audio Power Amplifier</li>
<li> Accelerometer: [http://www.analog.com/en/mems/low-g-accelerometers/adxl325/products/product.html ADXL325] 3-AXIS ±5 G</li>
<li>Speakers: generic 6 ohm, 2" diameter (INSERT SPEAKER SPECs, link)</li>
</ul>

===Build the Circuit===

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:23:24Z

JonathanDrake: /* Parts List */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
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.

===Parts List===

Items with * indicate setup for 1 speaker. Multiply by 6 for full configuration.

<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32|NU32 board]] configuration</li>
<li>Low-Pass Filter: 66K resistor, 0.01uF capacitor</li>
<li>Unity Gain Buffer: [http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&name=LM741CNFS-ND LM741] general purpose op-amp</li>
<li>Speaker Amp: [http://www.national.com/ds/LM/LM1875.pdf LM1875] 20W Audio Power Amplifier</li>
<li> Accelerometer: [http://www.analog.com/en/mems/low-g-accelerometers/adxl325/products/product.html ADXL325] 3-AXIS ±5 G</li>
<li>Speakers: generic 6 ohm, 2" diameter (INSERT SPEAKER SPECs, link)</li>
</ul>

===Build the Circuit===

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:22:16Z

JonathanDrake: /* Parts List */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
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.

===Parts List===

Items with * indicate setup for 1 speaker. Multiply by 6 for full configuration.

<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32|NU32 board]] configuration</li>
<li>Low-Pass Filter: 66K resistor, 0.01uF capacitor</li>
<li>Unity Gain Buffer: [http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&name=LM741CNFS-ND LM741] general purpose op-amp</li>
<li>Speaker Amp: [http://www.national.com/ds/LM/LM1875.pdf LM1875] 20W Audio Power Amplifier</li>
<li> Accelerometer: [http://www.analog.com/en/mems/low-g-accelerometers/adxl325/products/product.html ADXL325] 3-AXIS ±5 G</li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
</ul>

===Build the Circuit===

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:19:30Z

JonathanDrake: /* Circuit Design */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
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.

===Parts List===

Items with * indicate setup for 1 speaker. Multiply by 6 for full configuration.

<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32|NU32 board]] configuration</li>
<li>Low-Pass Filter: 66K resistor, 0.01uF capacitor</li>
<li>Unity Gain Buffer: [http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&name=LM741CNFS-ND LM741] general purpose op-amp</li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
</ul>

===Build the Circuit===

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:18:34Z

JonathanDrake: /* Parts List */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
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.

===Parts List===

Items with * indicate setup for 1 speaker. Multiply by 6 for full configuration.

<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32|NU32 board]] configuration</li>
<li>Low-Pass Filter: 66K resistor, 0.01uF capacitor</li>
<li>Unity Gain Buffer: [http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&name=LM741CNFS-ND|LM741] general purpose op-amp</li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
</ul>

===Build the Circuit===

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:18:14Z

JonathanDrake: /* Parts List */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
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.

===Parts List===

Items with * indicate setup for 1 speaker. Multiply by 6 for full configuration.

<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32|NU32 board]] configuration</li>
<li>Low-Pass Filter: 66K resistor, 0.01uF capacitor</li>
<li>Unity Gain Buffer: [[http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&name=LM741CNFS-ND|LM741]] general purpose op-amp</li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
</ul>

===Build the Circuit===

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:16:40Z

JonathanDrake: /* Circuit Design */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
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.

===Parts List===

Items with * indicate setup for 1 speaker. Multiply by 6 for full configuration.

<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32|NU32 board]] configuration</li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
</ul>

===Build the Circuit===

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:14:46Z

JonathanDrake: /* Circuit Design */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
Parts List:
<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32|NU32 board]] configuration</li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
</ul>

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.

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:14:35Z

JonathanDrake: /* Circuit Design */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
Parts List:
<ul>
<li>PIC32MX460F512L, in [[Introduction_to_the_PIC32 NU32 board]] configuration</li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
<li></li>
</ul>

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.

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:12:58Z

JonathanDrake: /* Circuit Design */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]]
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:12:42Z

JonathanDrake: /* Circuit Design */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|thumb|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:11:34Z

JonathanDrake: /* Results */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

We successfully tested a single speaker/accelerometer setup. The PIC produced a sine wave, through the LPF, which was filtered by the buffer and amplified before driving the speaker. A test mount included an accelerometer rigidly attached to the oscillating part of the speaker. While the speaker vibrated at a 30Hz frequency, the accelerometer readings were fed into the PIC and compared to the input. The mathematical results, in terms of phase and magnitude, compare fairly accurately with oscilloscope readings.

The 6 speaker amplifier circuit seems to be bogged down by noise and power issues. Operation is buggy at best, and varies considerably when external electronics, like oscilloscope leads, are attached. The current required to drive all 6 speakers at once might also be greater than the available current provided by the 12V boardlet adapter.

==Reflections==

6-DOF PPOD

2010-03-18T22:01:34Z

JonathanDrake:

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

==Reflections==

6-DOF PPOD

2010-03-18T22:01:15Z

JonathanDrake: /* Results and Reflection */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results==

6-DOF PPOD

2010-03-18T21:53:18Z

JonathanDrake:

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}
</pre>

==Results and Reflection==

ME 333 final projects

2010-03-18T21:45:33Z

JonathanDrake: /* 6-DOF PPOD */

See the '''[[ME 333 end of course schedule]]'''.

Final projects for ME 333 in years 2000-2007 can be found
'''[http://lims.mech.northwestern.edu/~design/mechatronics/ here]'''.

__TOC__

== ME 333 Final Projects 2010 ==

=== [[Sample Project Title]] ===

[[Image:Persistence of Vison Display|thumb|150px|Project photo caption.|right]]

You can copy and paste this wiki code to start your wiki page (but don't erase this code). Then replace this text with your own. A few sentences describing what your project does, with a link to a youtube video. Look at other final project wiki pages for ideas, but see [[ME 333 end of course schedule]] for more information on what should be included on your wiki page.

 

=== [[Furuta Pendulum]] ===

[[Image:Picture 1.png|thumb|150px|Furuta Pendulum|right]]

The Furuta pendulum, so named because it was first developed by Katsuhisa Furuta, is a rotational inverted pendulum. In other words, the horizontal arm, which rotates in the horizontal plane, drives the movement of the vertical arm, which is free to rotate in the vertical plane. Out objective was to build a Furuta pendulum that would hold the vertical arm up, as you can see from the [http://www.youtube.com/watch?v=7DtFLKgNUk4 demonstration video].

 

=== [[Music from the Heart -- Music Suit]] ===

[[Image:Music from the heart overview.jpg|thumb|150px|The "music suit" on James, with heart rate detector on his finger.|right]]

This project attempted to create a natural form of musical expression by connecting sensors to the body. Six tilt switches were attached to the wrist, ankles, and shoulders, each controlling a single pitch from the [http://en.wikipedia.org/wiki/Pentatonic_scale pentatonic scale]. The heart beat was obtained using [http://en.wikipedia.org/wiki/Photoplethysmograph photoplethysmography] on the user's finger, and this signal was used to strike a drum in sync with the heart beat.

For a video demonstration, click [http://www.youtube.com/watch?v=YyipByy7m6I here].

 

=== [[Conservation of Angular Momentum Locomotion Robot (Fluffbot)]] ===

[[Image:Isometric view of Fluffbot guts.jpg|thumb|150px|The Fluffbot without Fluff.|right]]

Cute fluffy robot that uses conservation of angular momentum to move forward and backward. The robot's momentum wheel accelerates in the floor-plane. The robot's net angular momentum must remain zero-- a steering wheel guides the Fluffbot to accelerate in the opposite direction. This moves the robot forward in a curved path. The momentum wheel and steering wheel then change direction of acceleration. This repeated process moves the Fluffbot forward in a sinusoidal path.

 

=== [[Differential Drive Mobile Robot]] ===

[[Image:Persistence of Vison Display|thumb|150px|Project photo caption.|right]]

The goal of this project was to create a small differential drive mobile robot that would act as a low cost replacement for the popular E-Puck Robot. The robot uses hybrid stepper motors to allow it to track its position through odometry, has a laser cut acrylic chassis for easy replication and replacement, and a 1500 mAh, 13.2V battery pack for long run time. The robot also uses the NU32 board for its control logic and a XBee chip for communication.

 

=== [[Ferrofluid Art Display]] ===

[[Image:Persistence of Vison Display|thumb|150px|Place holder text for caption .|right]]

A little blurb about our Ferrofluid Art Display will go here. Just a few sentences talking about blah blah lkasjdfal hfalsdjh.

 

=== [[Can Launching Fridge]] ===

[[Image:27_Fridge.jpg|thumb|150px|Project photo caption.|right]]

The goal of the can launching fridge was to create a fridge that would, when initiated by either a remote control or a wired push button, automatically load, aim, and fire a can to multiple predetermined locations. The fridge uses a combination of stepper motors, a DC motor, and solenoids to create the ultimate mix of convenience, fun, and refreshment.

 

=== [[High Speed Motor Control]] ===

[[Image:2dofArmSetUp.jpg|thumb|150px|Project photo caption.|right]]
The project suggested was to design a system for high speed motor control using the PIC 32. To demonstrate the motor control, a two degree of freedom (2-DOF) parallelogram robot arm was designed to follow paths specified in a MATLAB gui.

 

=== [[Variable Frequency Electrosense]] ===

[[Image:TR_JP_PP-sensor.jpg|thumb|150px|Variable Frequency Electrosense|right]]

Our objective was to build upon existing research being done at Northwestern utilizing Electrosense technology by testing if information can be derived from varying the emitter frequency. We sought to send sinusoidal waves at discrete frequencies between 100 Hz and 10 kHz and to read in the sensed wave using a PIC 32’s ADC. We then sent the gathered information to a PC for plotting and analysis. By mounting the sensor on a one dimensional linear actuator we are able to gather additional data about objects and perform object detection and identification algorithms. While our initial results have revealed exciting trends, farther research is necessary before any significant conclusions can be made. A [http://www.youtube.com/watch?v=PJY097L2m1M video] of the project is available on YouTube.

 

=== [[Remote Controlled Wiitar]] ===

[[Image:Wiitar.jpg|thumb|150px|Guitar Controlled by a wiimote: The Wiitar!|right]]
This project uses an array of solenoids to depress stings on the neck of a guitar. A motor over the strings of the guitar turns an arm which strums the instrument, playing the chord depressed by the solenoids. The system is controlled by a Wii Remote.

 

=== [[6-DOF PPOD]] ===

[[Image:Jd-cr-Assembly.jpg|thumb|150px|Project photo caption.|right]]
The PPOD-mini is a miniaturized version of the Programmable Part-feeding Oscillatory Device ([http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/index.htm PPOD]) found in the Laboratory for Intelligent Mechanical Systems (LIMS).

 

=== [[Butterfly Rolling Manipulation]] ===
[[Image:Butterflyteampic.jpg|right|upright=1.3|thumb|Butterfly Project at Mechatronics Fair]]
The Butterfly emulates contact juggling by the stabilization of dynamic rolling. The apparatus rolls a cylinder by rotating the "hand" using a specific trajectory and is able to move the cylinder from one side to the other without losing contact. A video of the Butterfly captured in high speed can be found [http://www.youtube.com/watch?v=dtYv3qNz_LI here].

 

=== [[Haptic Gaming System]] ===

[[Image:Haptikos.jpg|thumb|150px|Haptic Robot.|right]]

An interactive gaming system that allows the user to physically feel a virtual world. The player controls the cursor by moving the red joystick. Two games were created to test the feedback system. The first is a side-scroller in which you avoid hitting moving blocks. The second involves feeling out the virtual shape with nothing other than a blue position dot and a blank screen.

 

== ME 333 Final Projects 2009 ==

=== [[Mozart's Right Hand]] ===
[[Image:mrh_box.JPG|thumb|150px|Mozart's Right Hand|right]]
Mozart's Right Hand is a musical instrument capable of playing two full octaves of the [http://en.wikipedia.org/wiki/Diatonic_scale Diatonic Scale.] The user wears a glove on his right hand and uses motions of the hand and fingers to create different notes that are played with a speaker. The pitch of the note is controlled by the orientation of the user's hand as he rotates it ether from the wrist, the elbow, or the shoulder. The LCD on the front of the box tells the user the pitch that corresponds to his or her current hand orientation. When the user touches together his thumb and index finger, the speaker plays the tone. A [http://www.youtube.com/watch?v=vec-W4QeHQU video] of Mozart's Right Hand in action is available on YouTube.

'''Chosen the OUTSTANDING PROJECT by the students of ME 333.'''

 

=== [[Persistence-of-Vision Display]] ===
[[Image:Persistence of Vison Display|right|thumb|150px]]
This is a fully customizable display implemented using the concept of Persistence of Vision. User-specified images (and even moving images) were displayed by rotating a column of LEDs at speeds faster than 300rpm. Each individual LED was modeled as a row of pixels. Conversely, the rotational position of the panel of LEDs represented the pixel columns of the display; the time interval and rotational speed determined the width of the pixel columns.

 

=== [[Rock Paper Scissors Machine]] ===
[[Image:rps whole thing.JPG|thumb|150px|Rock Paper Scissors Machine|right]]
A machine that will play a fully functioning, intuitive game of [http://en.wikipedia.org/wiki/Rock-paper-scissors Rock/Paper/Scissors] (abbreviated as RPS) with a user. The machine is represented by a human-like hand, capable of seperate and independant wrist, arm, finger and thumb motion. The players' hand goes into a glove equipped with flex sensors, which wirelessly transmits data to the machine based on what the player chose. The machine then reads this data, randomly chooses a throw of its own, and displays what the machine threw, what the human threw, total win/loss/tie info, and winner/loser both on an [http://en.wikipedia.org/wiki/Lcd LCD] screen and in the form of a thumbs up/down/side motion. Video of the machine in action can be found [http://www.youtube.com/watch?v=xbLNBSTTrcE here.]
 

=== [[Three-speaker Chladni Patterns]] ===
[[Image:chladni_660hz|right|thumb|150px]]
This project uses three speakers to generate shapes on a circular aluminum plate depending on which frequency the speakers are playing at. Once the speakers hit a resonant frequency of the plate, salt migrates to the nodes (zero amplitude) regions of the plate to form distinct patterns.
 

=== [[Basketball]] ===
[[Image:Mechatronics2009Bball|right|thumb|150px]]
This project consists of a throwing arm propelled by a Pittman motor is mounted on a turntable and throws the ball into the "hoop." The hoop is wrapped in reflective tape and an IR emitter, receiver pair is used to sense where the IR is reflected most (the hoop with highly reflective tape). An ultrasonic sensor then pings the hoop for the distance of the hoop. With this information, the arm is able to "make a basket." A video can be found [http://www.youtube.com/watch?v=Y466dzP-qiY here].
 

=== [[Robot Drummer]] ===
[[Image:Robot_Drummer.jpg|thumb|400pix|right|Robot Drummer]]
The Robot Drummer is a device that demonstrates high-speed motor control by being able to drum when given commands. Through an RS232 cable, Matlab sends commands to a "master" PIC. The master then sends the commands to two "slave" PICs through I2C communication. The slaves take the commands and implement PID control of the motors.
 

=== [[Automated Fish Refuge]] ===
[[Image:Entire Fish Refuge|right|thumb|200px]]
The automated fish refuge allows for the controlled movement of a fish refuge with the goal of recording specific behavior. The mechanical design is completely adjustable and allows adjustable degrees of oscillating movement and orientation of the refuge. The program is primarily in MATLAB for ease of use and the velocity profile can be a sine, square, triangle, or any function that the user inputs. [http://www.youtube.com/watch?v=wGOKujMhN88 Check out the video!]

 

=== [[Marionette]] ===
[[Image: MarionettePicForIntro.JPG|right|thumb]] The Marionette Project focused on using RC Servos to make a puppet that would do a dance with the press of a button. There were 5 different dances programmed for the marionette, showcasing different styles of movement. The movement had 2 degrees of freedom thanks to using 5 bar linkages and 2 RC servos for each arm. Two more RC Servos were used on the back of the marionette to create the appearance of leg movement. The movements included a Hula dance, Jumping Jacks, and even some moves right out of Saturday Night Fever.

 

=== [[Monkeybot]] ===
[[Image:monkeybot_pic|thumb|right|200px|Monkeybot]]
The monkeybot is a swinging robot capable of moving side to side and climbing. It consists of a two link, double pendulum system with an electro-magnet on each end. At the pivot is a DC motor, which provides an input torque and allows the swinging system to gain energy and climb. Check out the video of the monkeybot climbing [http://www.youtube.com/watch?v=TA2VcH_GDJ0 here] and a later brachiation video [http://www.youtube.com/watch?v=0hfwJEVQyeQ&feature=related here].
 

=== [[PPOD-mini: 6-DOF Shaker]] ===
[[Image:PPOD_mini.JPG|thumb|200x200 px|right|PPOD-mini 6-DOF Shaker]]
The PPOD-mini is a miniaturized version of the Programmable Part-feeding Oscillatory Device ([http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/index.htm PPOD]) found in the Laboratory for Intelligent Mechanical Systems (LIMS) at Northwestern. The PPOD-mini utilizes six speakers that act like actuators. The speakers are connected to a acrylic plate via flexures of tygon and iron. In its current implementation, the phase of the speakers can be controlled independently, giving the device six degrees of freedom. The movement of objects placed on the acrylic plate can be controlled by changing the phases of the speakers.

 

=== [[Automated Xylophone]] ===
[[Image:AutomatedXylophonePicture1.jpg|thumb|200x200 px|right|Automated Xylophone]]
The Automated Xylophone controls several solenoids which hit various pitches on an actual xylophone based on the note selected. The device has two main modes: using the keypad, a user can choose to either play notes in real time or store songs to be played back later. A video of the Automated Xylophone playing in real time mode can be found [http://www.youtube.com/watch?v=_ubpAEyq9kg here].

 

=== [[Vision-based Cannon]] ===
[[Image:SM_Gun_Camera_PIC_Setup.JPG|thumb|200x200 px|right|Vision-based Cannon]]
This project uses a webcam and Matlab to analyze an image and direct a modified USB Missile Launcher to fire at targets found in the image.

 

== ME 333 Final Projects 2008 ==

=== [[IR Tracker]] ===

[[Image:IR_Tracker_Main.jpg|right|thumb|200px]]

The IR Tracker (aka "Spot") is a device that follows a moving infrared light. It continuously detects the position of an infrared emitter in two axes, and then tracks the emitter with a laser. [[Media:MT_MS_AZ_TrackerVideo.mp4|See Spot Run.]]

'''Chosen the OUTSTANDING PROJECT by the students of ME 333.'''

 

=== [[Robot Snake]] ===
[[Image:HLSSnakeMain.jpg|right|thumb|200px]]

This remote control robotic snake uses servo motors with a traveling sine wave motion profile to mimic serpentine motion. The robotic snake is capable of moving forward, left, right and in reverse.

[http://www.youtube.com/watch?v=r_GOOFLnI6w Video of the robot snake]

Featured on [http://blog.makezine.com/archive/2009/03/well_documented_robotic_snake.html Makezine.com].
 

=== [[Programmable Stiffness Joint]] ===

[[Image:SteelToePic2.jpg|thumb|200px|The 'Steel Toe' programmable stiffness joint|right]]

The Programmable Stiffness Joint varies rotational stiffness as desired by the user. It is the first step in modeling the mechanical impedance of the human ankle joint (both stiffness and damping) for the purpose of determining the respective breakdown of the two properties over the gait cycle.

 

=== [[Magnetic based sample purification]] ===

 

=== [[Continuously Variable Transmission]] ===

[[image:CVT_system.JPG|thumb|200px]]

This prototype is a proof of concept model of a variable ratio transmission to be implemented in the 2008-2009 Formula SAE competition vehicle. The gear ratio is determined by the distances between the pulley halves which are controllable electronically.

 

=== [[Granular Flow Rotating Sphere]] ===
[[Image:Team-21-main-picture.JPG|right|thumb|200px]]
This device will be used to study the granular flow of particles within a rotating sphere. The sphere is filled with grains of varying size and then rotated about two different axes according to a series of position and angular velocity inputs.

 

=== [[Vibratory Clock]] ===

[[Image:Vibratory_Clock.jpg|right|thumb|Vibratory Clock|200px]]

The Vibratory Clock allows a small object to act as an hour "hand" on a horizontal circular platform that is actuated from underneath by three speakers. The object slides around the circular platform, impelled by friction forces due to the vibration. [http://www.youtube.com/watch?v=KhgTNCfdwZw Check it out!]

 

=== [[WiiMouse]] ===

[[Image:HPIM1027.jpg|right|thumb|200px]]

The WiiMouse is a handheld remote that can be used to move a cursor on a windows-based PC, via accelerometer input captured through device movement.

 

=== [[Intelligent Oscillation Controller]] ===

[[image:ME333_learning_oscillator.jpg|thumb|200px]]

This device "learns" a forcing function that is applied to a spring and mass system to match an arbitrary, periodic acceleration profile.

 

=== [[Baseball]] ===

[[Image:Baseball_Playfield.jpg|Sweet Baseball Game|right|thumb|200px]]

An interactive baseball game inspired by pinball, featuring pitching, batting, light up bases and a scoreboard to keep track of the game.

 

=== [[Ball Balancing Challenge]] ===

[[Image:Ballbalancechallenge.JPG|right|thumb|200px]]

An interactive game involving ball balancing on a touchscreen with touchscreen feedback and joystick action.

6-DOF PPOD

2010-03-18T21:08:25Z

JonathanDrake: /* Circuit Design */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>[[Image:6amps.jpg|thumb|Six speaker amplification circuits, shown here without attached heatsinks.]]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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

File:6amps.jpg

2010-03-18T21:07:15Z

JonathanDrake:

6-DOF PPOD

2010-03-18T21:06:10Z

JonathanDrake: /* Team Members */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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 ===
[mailto:jdrake@u.northwestern.edu Jonathan Drake] (Mechanical Engineering, 2010) 
[mailto:ramsey.caitlin@gmail.com 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

6-DOF PPOD

2010-03-18T21:05:18Z

JonathanDrake: /* Overview */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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, which, when sent through a low-pass filter, 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 [[PIC32MX:_FFT_of_Analog_Input|Fast Fourier Transform (FFT)]], 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

6-DOF PPOD

2010-03-18T21:03:36Z

JonathanDrake: /* Overview */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

6-DOF PPOD

2010-03-18T21:03:25Z

JonathanDrake: /* Overview */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]Our work on the portable 6 degree-of-freedom (DOF) programmable part-feeding oscillatory device (PPOD) is based off of the previous work of [http://lims.mech.northwestern.edu/projects/frictioninducedforcefields/ Tom Vose] in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as [[PPOD-mini:_6-DOF_Shaker|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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

6-DOF PPOD

2010-03-18T21:00:58Z

JonathanDrake: /* Overview */

== Overview ==

[[Image:Jd-cr-Assembly.jpg|thumb|PPOD showing acrylic plate, base, horizontal and vertical speakers.]]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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

File:Jd-cr-Assembly.jpg

2010-03-18T20:59:25Z

JonathanDrake:

File:Assembly.jpg

2010-03-18T20:59:02Z

JonathanDrake:

6-DOF PPOD

2010-03-18T20:58:07Z

JonathanDrake: /* Overview */

== Overview ==

(INSERT PHOTO, WHOLE RIG)

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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

6-DOF PPOD

2010-03-18T20:57:58Z

JonathanDrake: /* Overview */

== Overview ==

(INSERT PHOTO, WHOLE RIG)
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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

6-DOF PPOD

2010-03-18T20:54:04Z

JonathanDrake: /* Circuit Design */

== 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<ol>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>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.</li>
<li>The accelerometer (again, theoretically 3, but we are explaining 1) is attached to the vibrating table disk at key attachment points, 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.</li>
</ol>

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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

6-DOF PPOD

2010-03-18T20:51:32Z

JonathanDrake: /* Code */

== 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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. [[media:PPOD.zip|Download the zip file]] containing all necessary project files.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

File:PPOD.zip

2010-03-18T20:36:39Z

JonathanDrake:

6-DOF PPOD

2010-03-18T20:34:48Z

JonathanDrake: /* Code */

== 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

===Initialization===
<ul>
<li>Declares constants, functions, and variables to initialize the program</li>
<li>Declares multi-dimensional, real-imaginary matrices to handle sampling and FFT analysis</li>
<li>Initializes UART, Interrupts, LEDs, ADC, PWM, and sampling functionality</li>
<li>Builds a vector containing PWM duty cycles values (to be referenced later) in sinusoidal form</li>
<li>Performs a series of matrix operations, showing the steps needed to solve for the transfer function (NOTE: this needs to be completed by a future team)</li>
</ul>

===Continuous Operation===
<ul>
<li>Changes PWM duty cycle with an interrupt routine, period set by user</li>
<li>Samples speaker signal input (for feedback) and 3 axes of 1 accelerometer with another interrupt routine, period set by user</li>
<li>If "User" button pressed, FFT the speaker input, x/y/z axis from accelerometer</li>
<li>Using FFT results for first harmonic (e.g. 30Hz), compute transfer function from input (speaker sine wave) to x-axis, y-axis, z-axis</li>
<li>Output data over UART2</li>

</ul>

<pre>

/*
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;
}

</pre>

6-DOF PPOD

2010-03-18T20:25:53Z

JonathanDrake: /* Code */

== 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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.

<pre>

/*
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;
}

</pre>

6-DOF PPOD

2010-03-18T20:18:43Z

JonathanDrake: /* Code */

== 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

[[Image:Jd-cr-Full-schematic.jpg|Full schematic, showing PIC, LPF, Buffer, Amplifier, and Accelerometer]] 
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 ==

<pre><code>

/*
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;
}

</code></pre>

6-DOF PPOD

2010-03-18T20:16:55Z

JonathanDrake:

6-DOF PPOD

2010-03-17T18:36:05Z

JonathanDrake: /* Team Members */

6-DOF PPOD

2010-03-17T18:32:56Z

JonathanDrake: /* Circuit Design */

6-DOF PPOD

2010-03-17T18:32:43Z

JonathanDrake: /* Circuit Design */

6-DOF PPOD

2010-03-17T02:53:06Z

JonathanDrake: /* Team Members */

6-DOF PPOD

2010-03-17T02:53:00Z

JonathanDrake: /* Team Members */

6-DOF PPOD

2010-03-17T02:51:52Z

JonathanDrake: /* Team Members */

6-DOF PPOD

2010-03-17T02:51:04Z

JonathanDrake: /* Team Members */

File:24-Drake-Ramsey.jpg

2010-03-17T02:50:49Z

JonathanDrake:

6-DOF PPOD

2010-03-17T02:49:19Z

JonathanDrake: /* Circuit Design */

6-DOF PPOD

2010-03-17T02:46:11Z

JonathanDrake: /* Circuit Design */

File:Jd-cr-Full-schematic.jpg

2010-03-17T02:45:50Z

JonathanDrake:

6-DOF PPOD

2010-03-17T02:44:54Z

JonathanDrake: /* Circuit Design */

6-DOF PPOD

2010-03-17T02:43:38Z

JonathanDrake: /* Overview */

6-DOF PPOD

2010-03-17T02:42:23Z

JonathanDrake: New page: == 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 Tom Vose in the Laboratory for I...

== 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 Tom Vose in the Laboratory for Intelligent Mechanical Systems (LIMS), as well as 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, which, when sent through a low-pass filter, 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), 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) 
INSERT TEAM PICTURE

== 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: 
<ul>
<li>1/2" diameter rod is superglued to the interior cavity of the speaker</li>
<li>1/4" diameter rod is inserted into a hole in the half inch diameter rod</li>
<li>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</li>
<li>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. </li>
<li>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.</li>
</ul>

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 ==

INSERT SYSTEM SCHEMATIC (“big picture”) 
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.