Complete Source Code
#include <hidef.h> /* common defines and macros */
#include "derivative.h" /* derivative-specific definitions */
#include <math.h>
//define constants
//wake up A/D Converter... ATDCTL2
#define ADPU 0b10000000
//set up desired behaviour... ATDCTL3
#define SEQLEN 0b00001000
#define FIFO 0b00000000
#define FRZ 0b00000000
//set up resolution and timing... ATDCTL4
//#define SRES 0b10000000 //8bit resolution
#define SRES 0b00000000 //10bit resolution
#define SMP 0b01100000
#define PRS 0b00000001
//setup data parametets, conversion sequence
//and input channel... ATDCTL5
//#define DATA 0b11000000 //8bit right justified
#define DATA 0b10000000 //10 bits right justified
#define SCAN 0b00000000 //single conversion sequence
#define MULT 0b00000000 //single channel
#define CHAN 0b00000011 //AD channel 3 (AD3)
//wait for seccessful conversion.. ATDSTAT0
#define SCF 0b10000000 //flag for conversion completion
//array size
#define N 10
#define M 3
#define MSDELAY 1360
//LED/BUTTONS
#define LED1 0x4
#define LED2 0x8
#define SW1 0x40
#define SW2 0x80
//Output
#define OUTPUT 0b10101010
#define PATTERN 0b00000010
//constants
#define N 10
#define M 3
#define SENSITIVITY 0.0025 // V/Gause
#define QUISCENT 2.4000 // quiscent voltage
#define ADC 10.00
#define SPAN 4.800
#define PI 3.14156265f
#define HA 180.00f
void calccali(int calibrate[]);
int readAnalog(int i); //read Analog value
void display(int nbr, int base); //display nbr/ base
void delay(int ms);
void calcOffset(int original[][M], int mod[][M], int mid[]);//calculate offset
void fieldReading(int reading[][M]);
void adConv(int offset[][M], float digital[][M]); //store digital reading
void resVector(float pyth[][M], float result[]); //calc magnitudes
void calcAngle(float dig[][M], float angle[][M-1]);//calc angles
void calcResult(float angle[][M-1], float vector[], float result[][M]);//identify negatives and merge angle and vector array
void show(float rdg[][M-1], float result[][M]); //display function
//void calcMag(int offset[][M], int magnitude[]); //calculate offset
void main() {
int reading[N][M];
int offset[N][M];
float vector[N];
float angle[N][M-1] ;
float result[N][M];
//int magnitude[N];
float digital[N][M];
int i,j; // loop counters
int calibrate[M];
//wake up A?D converter... ATDCTL2
ATDCTL2 = ADPU;
delay(2);//~1ms
//set up desired behaviour... ATDCTL3
ATDCTL3 = SEQLEN | FIFO | FRZ;
// set up resolution and timing ... ATDCTL4
ATDCTL4 = SRES | SMP | PRS;
DDRA |=0b10101010;
DDRB |=0b11110000;
DDRJ &= ~(SW1|SW2);
DDRS |= LED1|LED2;
calccali(calibrate);
fieldReading(reading);
calcOffset(reading, offset, calibrate);//calcMag(offset, magnitude);
adConv(offset, digital);//digital value convertion
resVector(digital, vector);//magnitude calculations
calcAngle(digital, angle); //Angle conversion
calcResult(angle, vector, result);//Angle idnetify, merge
show(digital, result); //Show results
EnableInterrupts;
for(;;) {
_FEED_COP(); /* feeds the dog */
} /* loop forever */
/* please make sure that you never leave main */
}
/****************************************************************************/
//Delay Function
// waste cpu cycles #of ms
//input int ms number of ms to waste :D
void delay(int ms){
int i,j;
for(i=0; i<ms; i++)
for(j=0; j<MSDELAY; j++);
}
/****************************************************************************/
//readAnalog
//Function reads analog and converts to digital values
//input int i countes up channels
int readAnalog(int i){
int result=0;
//setup data parameter, conversion sequence and I/P channel... ATDCTL5
//start conversion
ATDCTL5 = DATA | SCAN | MULT | (CHAN+i);
while(!(ATDSTAT0 & SCF)){
PTS = LED2;
delay(100);
PTS = 1;
delay(100);
}
//wait for conversion to be done
result = ATDDR0L+(ATDDR0H<<8); //low + high shfted 8 bits left
return result;
}
/****************************************************************************/
//Display
void display(int nbr, int base){
unsigned int i;
int number;
int pattern;
pattern=0b00000010;
number = (int)(fabs(nbr));
PORTA = (byte)pattern;
for (i=0; i<4; i++){
pattern= pattern <<2;
PORTB =(byte)(( number % base)<<4); //shift 4 bits over, using portB 4-7
number = number/base;
delay(2);
PORTA = 0b00000000;
PORTA=(byte)pattern;
}
}
/*****************************calcOffset**************************************
function: to calculate result offset arrays
inputs: int original[][M] original array
int mod[][M] array after offset
int mid midpoint reference(ref point)
*****************************************************************************/
void calcOffset(int original[][M], int mod[][M], int mid[])
{
int i,j;
//initialize mid point
for(i=0; i<N; i++)
for(j=0; j<M; j++)
mod[i][j]=mid[j];
for(i=0; i<N; i++)
for(j=0; j<M; j++)
mod[i][j] =original[i][j]-mod[i][j];
while(PTJ&SW1){
PTS = LED2|LED1;
delay(250);
PTS = LED1;
delay(250);
PTS = LED2;
delay(250);
}
}
/****************************************************************************/
//function retrieves analog reading from ATD converter
// int reading[][M] holds analog readings
void fieldReading(int reading[][M]){
int i,j;
for (i=0; i<N; i++){
while((PTJ&(SW1|SW2)))
PTS = LED2|LED1;
for (j=0; j<M; j++){
reading[i][j] = readAnalog(j);
}
}
}
/****************************************************************************/
//Function establishes the ref point in x,y,z to calibrate system
//input int calibrate[] 1D array stores calibration values
void calccali(int calibrate[]){
int i;
for (i=0;i<M; i++){
calibrate[i]=readAnalog(i);
while(PTJ&SW1)
display(calibrate[i],10);
delay(500);
}
}
/****************************************************************************/
// function converts the analog input to magnetic field (Gauss)
//input int offset[][M] contains 10 set of 3, number of steps from ref
//int digital[][M] array stores the new converter GAUSS
/****************************************************************************/
void adConv(int offset[][M], float digital[][M]){
float res, gauStep;
int i,j, nbr;
res = SPAN/(pow(2.00,ADC)-1);//resolution
gauStep = res/SENSITIVITY; //Gauss per step
while(PTJ&(SW2))
display((int)(gauStep*1000), 10);
for(i=0;i<N;i++)
for(j=0;j<M;j++){
nbr = (float)(offset[i][j]);
digital[i][j]=nbr*gauStep;
}
}
/**************************************************************************
Function calculates resultat magnitude from three vectors
input float pyth[][M] sets of 3 vectors
float result[] magnitude array
Output n/a
**************************************************************************/
void resVector(float pyth[][M], float result[]) {
int i;
for(i=0;i<N;i++)
result[i] = sqrtf(powf(pyth[i][0],2)+powf(pyth[i][1],2)+powf(pyth[i][2],2)); //magnitude
}
/**************************************************************************
Function calculates angle from x to y axis, then from xy plane to z axis
input: float dig[][] sets of 3 vectors
float angle[][M-1] set of 2 angles
**************************************************************************/
void calcAngle(float dig[][M], float angle[][M-1]) {
int i;
float h1, h2;
for(i=0;i<N;i++) {
h1=sqrtf(powf(dig[i][0],2)+powf(dig[i][1],2));
h2=sqrtf(powf(h1,2)+ powf(dig[i][2],2));
angle[i][0]=asinf(dig[i][1]/h1)*(HA/PI);
angle[i][1]=acosf(h1/h2)*(HA/PI);
}
}
/**************************************************************************
function identifies negative angles then merge angle and vector array
inputs: float angle[][M-1] angle array
vector[] magnitude array
result[][M] merged array
**************************************************************************/
void calcResult(float angle[][M-1], float vector[], float result[][M]){
int i, j;
for(i=0; i<N; i++)
for(j=0; j<(M-1); j++){
if (angle[i][j]<0)
angle[i][j]=fabs(angle[i][j])+1000; //display 1 on the most
//significant digit if negative
}
for(i=0; i<N; i++){
//merge vector and angle array
result[i][0] = vector[i];
result[i][1] = angle[i][0];
result[i][2] = angle[i][1];
}
}
/**************************************************************************
function display digital values and set readings (magnitude and reading)
depending on what switch is pressed
input rdg[][M-1] digital readings
result[][M] merged array
**************************************************************************/
void show(float rdg[][M], float result[][M]){
int i,j;
while(PTJ&(SW1|SW2)){
PTS=LED1|LED2;
delay(2000);
//hold switch 1 or switch 2 to enter each respective sections
if(!(PTJ&SW1)){
PTS=0;
PTS=LED1; //LED1 on for digital
while(PTJ&(SW1|SW2))
display(209, 16); // display 'd1' for digital readings
delay(500);
for(i=0; i<N; i++){
while(PTJ&SW1)
display((24065+i), 16); // display '5E01 + i' set01 plus counter
delay(500);
for(j=0; j<M; j++){
while(PTJ&SW1)
display(rdg[i][j], 10);
delay(500);
}
}
}
else if(!(PTJ&SW2)){
PTS=0;
PTS=LED2;//LED2 on for results
while(PTJ&SW1)
display(255, 16); // display 'ff' for final results
delay(500);
for(i=0; i<N; i++){
while(PTJ&SW1)
display((24065+i), 16); // display '5E01 + i' set01 plus counter
delay(500);
for(j=0; j<(M); j++){
while(PTJ&SW1)
display(result[i][j], 10);
delay(500);
}
}
}
}
}
#include "derivative.h" /* derivative-specific definitions */
#include <math.h>
//define constants
//wake up A/D Converter... ATDCTL2
#define ADPU 0b10000000
//set up desired behaviour... ATDCTL3
#define SEQLEN 0b00001000
#define FIFO 0b00000000
#define FRZ 0b00000000
//set up resolution and timing... ATDCTL4
//#define SRES 0b10000000 //8bit resolution
#define SRES 0b00000000 //10bit resolution
#define SMP 0b01100000
#define PRS 0b00000001
//setup data parametets, conversion sequence
//and input channel... ATDCTL5
//#define DATA 0b11000000 //8bit right justified
#define DATA 0b10000000 //10 bits right justified
#define SCAN 0b00000000 //single conversion sequence
#define MULT 0b00000000 //single channel
#define CHAN 0b00000011 //AD channel 3 (AD3)
//wait for seccessful conversion.. ATDSTAT0
#define SCF 0b10000000 //flag for conversion completion
//array size
#define N 10
#define M 3
#define MSDELAY 1360
//LED/BUTTONS
#define LED1 0x4
#define LED2 0x8
#define SW1 0x40
#define SW2 0x80
//Output
#define OUTPUT 0b10101010
#define PATTERN 0b00000010
//constants
#define N 10
#define M 3
#define SENSITIVITY 0.0025 // V/Gause
#define QUISCENT 2.4000 // quiscent voltage
#define ADC 10.00
#define SPAN 4.800
#define PI 3.14156265f
#define HA 180.00f
void calccali(int calibrate[]);
int readAnalog(int i); //read Analog value
void display(int nbr, int base); //display nbr/ base
void delay(int ms);
void calcOffset(int original[][M], int mod[][M], int mid[]);//calculate offset
void fieldReading(int reading[][M]);
void adConv(int offset[][M], float digital[][M]); //store digital reading
void resVector(float pyth[][M], float result[]); //calc magnitudes
void calcAngle(float dig[][M], float angle[][M-1]);//calc angles
void calcResult(float angle[][M-1], float vector[], float result[][M]);//identify negatives and merge angle and vector array
void show(float rdg[][M-1], float result[][M]); //display function
//void calcMag(int offset[][M], int magnitude[]); //calculate offset
void main() {
int reading[N][M];
int offset[N][M];
float vector[N];
float angle[N][M-1] ;
float result[N][M];
//int magnitude[N];
float digital[N][M];
int i,j; // loop counters
int calibrate[M];
//wake up A?D converter... ATDCTL2
ATDCTL2 = ADPU;
delay(2);//~1ms
//set up desired behaviour... ATDCTL3
ATDCTL3 = SEQLEN | FIFO | FRZ;
// set up resolution and timing ... ATDCTL4
ATDCTL4 = SRES | SMP | PRS;
DDRA |=0b10101010;
DDRB |=0b11110000;
DDRJ &= ~(SW1|SW2);
DDRS |= LED1|LED2;
calccali(calibrate);
fieldReading(reading);
calcOffset(reading, offset, calibrate);//calcMag(offset, magnitude);
adConv(offset, digital);//digital value convertion
resVector(digital, vector);//magnitude calculations
calcAngle(digital, angle); //Angle conversion
calcResult(angle, vector, result);//Angle idnetify, merge
show(digital, result); //Show results
EnableInterrupts;
for(;;) {
_FEED_COP(); /* feeds the dog */
} /* loop forever */
/* please make sure that you never leave main */
}
/****************************************************************************/
//Delay Function
// waste cpu cycles #of ms
//input int ms number of ms to waste :D
void delay(int ms){
int i,j;
for(i=0; i<ms; i++)
for(j=0; j<MSDELAY; j++);
}
/****************************************************************************/
//readAnalog
//Function reads analog and converts to digital values
//input int i countes up channels
int readAnalog(int i){
int result=0;
//setup data parameter, conversion sequence and I/P channel... ATDCTL5
//start conversion
ATDCTL5 = DATA | SCAN | MULT | (CHAN+i);
while(!(ATDSTAT0 & SCF)){
PTS = LED2;
delay(100);
PTS = 1;
delay(100);
}
//wait for conversion to be done
result = ATDDR0L+(ATDDR0H<<8); //low + high shfted 8 bits left
return result;
}
/****************************************************************************/
//Display
void display(int nbr, int base){
unsigned int i;
int number;
int pattern;
pattern=0b00000010;
number = (int)(fabs(nbr));
PORTA = (byte)pattern;
for (i=0; i<4; i++){
pattern= pattern <<2;
PORTB =(byte)(( number % base)<<4); //shift 4 bits over, using portB 4-7
number = number/base;
delay(2);
PORTA = 0b00000000;
PORTA=(byte)pattern;
}
}
/*****************************calcOffset**************************************
function: to calculate result offset arrays
inputs: int original[][M] original array
int mod[][M] array after offset
int mid midpoint reference(ref point)
*****************************************************************************/
void calcOffset(int original[][M], int mod[][M], int mid[])
{
int i,j;
//initialize mid point
for(i=0; i<N; i++)
for(j=0; j<M; j++)
mod[i][j]=mid[j];
for(i=0; i<N; i++)
for(j=0; j<M; j++)
mod[i][j] =original[i][j]-mod[i][j];
while(PTJ&SW1){
PTS = LED2|LED1;
delay(250);
PTS = LED1;
delay(250);
PTS = LED2;
delay(250);
}
}
/****************************************************************************/
//function retrieves analog reading from ATD converter
// int reading[][M] holds analog readings
void fieldReading(int reading[][M]){
int i,j;
for (i=0; i<N; i++){
while((PTJ&(SW1|SW2)))
PTS = LED2|LED1;
for (j=0; j<M; j++){
reading[i][j] = readAnalog(j);
}
}
}
/****************************************************************************/
//Function establishes the ref point in x,y,z to calibrate system
//input int calibrate[] 1D array stores calibration values
void calccali(int calibrate[]){
int i;
for (i=0;i<M; i++){
calibrate[i]=readAnalog(i);
while(PTJ&SW1)
display(calibrate[i],10);
delay(500);
}
}
/****************************************************************************/
// function converts the analog input to magnetic field (Gauss)
//input int offset[][M] contains 10 set of 3, number of steps from ref
//int digital[][M] array stores the new converter GAUSS
/****************************************************************************/
void adConv(int offset[][M], float digital[][M]){
float res, gauStep;
int i,j, nbr;
res = SPAN/(pow(2.00,ADC)-1);//resolution
gauStep = res/SENSITIVITY; //Gauss per step
while(PTJ&(SW2))
display((int)(gauStep*1000), 10);
for(i=0;i<N;i++)
for(j=0;j<M;j++){
nbr = (float)(offset[i][j]);
digital[i][j]=nbr*gauStep;
}
}
/**************************************************************************
Function calculates resultat magnitude from three vectors
input float pyth[][M] sets of 3 vectors
float result[] magnitude array
Output n/a
**************************************************************************/
void resVector(float pyth[][M], float result[]) {
int i;
for(i=0;i<N;i++)
result[i] = sqrtf(powf(pyth[i][0],2)+powf(pyth[i][1],2)+powf(pyth[i][2],2)); //magnitude
}
/**************************************************************************
Function calculates angle from x to y axis, then from xy plane to z axis
input: float dig[][] sets of 3 vectors
float angle[][M-1] set of 2 angles
**************************************************************************/
void calcAngle(float dig[][M], float angle[][M-1]) {
int i;
float h1, h2;
for(i=0;i<N;i++) {
h1=sqrtf(powf(dig[i][0],2)+powf(dig[i][1],2));
h2=sqrtf(powf(h1,2)+ powf(dig[i][2],2));
angle[i][0]=asinf(dig[i][1]/h1)*(HA/PI);
angle[i][1]=acosf(h1/h2)*(HA/PI);
}
}
/**************************************************************************
function identifies negative angles then merge angle and vector array
inputs: float angle[][M-1] angle array
vector[] magnitude array
result[][M] merged array
**************************************************************************/
void calcResult(float angle[][M-1], float vector[], float result[][M]){
int i, j;
for(i=0; i<N; i++)
for(j=0; j<(M-1); j++){
if (angle[i][j]<0)
angle[i][j]=fabs(angle[i][j])+1000; //display 1 on the most
//significant digit if negative
}
for(i=0; i<N; i++){
//merge vector and angle array
result[i][0] = vector[i];
result[i][1] = angle[i][0];
result[i][2] = angle[i][1];
}
}
/**************************************************************************
function display digital values and set readings (magnitude and reading)
depending on what switch is pressed
input rdg[][M-1] digital readings
result[][M] merged array
**************************************************************************/
void show(float rdg[][M], float result[][M]){
int i,j;
while(PTJ&(SW1|SW2)){
PTS=LED1|LED2;
delay(2000);
//hold switch 1 or switch 2 to enter each respective sections
if(!(PTJ&SW1)){
PTS=0;
PTS=LED1; //LED1 on for digital
while(PTJ&(SW1|SW2))
display(209, 16); // display 'd1' for digital readings
delay(500);
for(i=0; i<N; i++){
while(PTJ&SW1)
display((24065+i), 16); // display '5E01 + i' set01 plus counter
delay(500);
for(j=0; j<M; j++){
while(PTJ&SW1)
display(rdg[i][j], 10);
delay(500);
}
}
}
else if(!(PTJ&SW2)){
PTS=0;
PTS=LED2;//LED2 on for results
while(PTJ&SW1)
display(255, 16); // display 'ff' for final results
delay(500);
for(i=0; i<N; i++){
while(PTJ&SW1)
display((24065+i), 16); // display '5E01 + i' set01 plus counter
delay(500);
for(j=0; j<(M); j++){
while(PTJ&SW1)
display(result[i][j], 10);
delay(500);
}
}
}
}
}