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