19,28 → 19,27 |
#include <math.h> |
|
#include "global.h" // include our global settings |
#include "uart.h" // include uart function library |
#include "uart2.h" // include uart function library |
#include "rprintf.h" // include printf function library |
#include "timer.h" // include timer function library (timing, PWM, etc) |
#include "a2d.h" // include A/D converter function library |
|
//----- Begin Code ------------------------------------------------------------ |
#define BUFLEN 32 |
#define BUFLEN 64 |
|
int main(void) |
{ |
u08 i=0; |
s16 x=0,y=0; |
double fi; |
s16 fia; |
u16 fib; |
u16 i,x,y; |
double fi, err, fibuf[BUFLEN]; |
s16 fia, erra; |
u16 fib, errb; |
|
// initialize our libraries |
// initialize the UART (serial port) |
uartInit(); |
uartSetBaudRate(9600); |
uartSetBaudRate(0,9600); |
// make all rprintf statements use uart for output |
rprintfInit(uartSendByte); |
rprintfInit(uart0SendByte); |
// initialize the timer system |
timerInit(); |
// turn on and initialize A/D converter |
47,9 → 46,9 |
a2dInit(); |
// configure a2d port (PORTA) as input |
// so we can receive analog signals |
DDRC = 0x00; |
DDRA = 0x00; |
// make sure pull-up resistors are turned off |
PORTC = 0x00; |
PORTA = 0x00; |
|
// set the a2d prescaler (clock division ratio) |
// - a lower prescale setting will make the a2d converter go faster |
68,19 → 67,28 |
|
while(1) |
{ |
fi=0; |
err=0; |
for(i=0; i<BUFLEN; i++) |
{ |
x += a2dConvert10bit(0); |
y += a2dConvert10bit(1); |
{ |
x = a2dConvert10bit(ADC_CH_ADC0); |
y = a2dConvert10bit(ADC_CH_ADC1); |
fibuf[i] = atan2((double)x-511,(double)y-511); // record computed angles to buffer for post processing |
} |
x = x/BUFLEN - 512; |
y = y/BUFLEN - 512; |
for(i=0; i<BUFLEN; i++) fi += fibuf[i]; // sum recorded angles |
|
fi = atan2(y,x) * 180.0 / PI; |
fi = ((fi/BUFLEN)+PI) * 180.0 / PI; // average recorded angles and convert product to degrees |
|
for(i=0; i<BUFLEN; i++) err += (fibuf[i]-fi)*(fibuf[i]-fi); // sum cubic errors |
err = sqrt(err/(BUFLEN-1))/sqrt(BUFLEN); // compute average cubic error |
erra = floor(err); |
errb = floor((err - erra)*1000); |
|
fia = floor(fi); |
fib = floor((fi - fia)); |
rprintf("X:%d Y:%d fi:%d.%d \r\n", x, y, fia, fib); |
fib = floor((fi - fia)*1000); |
|
|
rprintf("fi:%d.%d +- %d.%d \r\n", fia, fib, erra, errb); |
} |
|
return 0; |
} |