newguy
Joined: 24 Jun 2004
Posts: 1903
|
|
 Posted: Thu Oct 10, 2013 4:24 pm |
 |
|
| Code: | #include <18F4680.h>
#device adc=10
#FUSES WDT
#FUSES WDT128 //Watch Dog Timer uses 1:128 Postscale
#FUSES INTRC_IO //Internal RC oscillator, no clock out
#FUSES NOPROTECT //Code not protected from reading
#FUSES BROWNOUT //Reset when brownout detected
#FUSES BORV28 //Brownout reset at 2.8V
#FUSES PUT //Power Up Timer
#FUSES NOCPD //No EE protection
#FUSES STVREN //Stack full/underflow will cause reset
#FUSES NODEBUG //No Debug mode for ICD
#FUSES NOLVP //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
#FUSES NOWRT //Program memory not write protected
#FUSES NOWRTD //Data EEPROM not write protected
#FUSES NOIESO //Internal External Switch Over mode disabled
#FUSES NOFCMEN //Fail-safe clock monitor disabled
#FUSES NOPBADEN //PORTB pins are configured as digital I/O on RESET
#FUSES BBSIZ4K //4K words Boot Block size
#FUSES NOWRTC //configuration not registers write protected
#FUSES NOWRTB //Boot block not write protected
#FUSES NOEBTR //Memory not protected from table reads
#FUSES NOEBTRB //Boot block not protected from table reads
#FUSES NOCPB //No Boot Block code protection
#FUSES NOLPT1OSC //Timer1 configured for higher power operation
#FUSES MCLR //Master Clear pin enabled
#FUSES NOXINST //Extended set extension and Indexed Addressing mode disabled (Legacy mode)
#use delay(clock=8000000,RESTART_WDT)
#use spi(MASTER, DI=PIN_C4, DO=PIN_C5, CLK=PIN_C3)
#include <lcd_driver.c>
#include <math.h>
// Jan 4 2011
// this is a completely tested and working example/driver to read the
// x,y,z accelerations from an ADXL345 accelerometer and convert
// to angle
// both PIC and ADXL run @3.3V (LCD too) so no level converters are required
// PIC runs @8MHz from internal oscillator
#use fast_io (B)
#use fast_io (C)
int1 refresh_screen = FALSE;
#define CS PIN_C0
#define SPI_MODE_0 (SPI_L_TO_H | SPI_XMIT_L_TO_H)
#define SPI_MODE_1 (SPI_L_TO_H)
#define SPI_MODE_2 (SPI_H_TO_L)
#define SPI_MODE_3 (SPI_H_TO_L | SPI_XMIT_L_TO_H)
unsigned int16 x_accel, y_accel, z_accel;
float x_g, y_g, z_g;
float theta;
#include <Accelerometer Driver.c>
#include <Trig Functions.c>
#int_EXT1
void EXT1_isr(void) {
refresh_screen = TRUE;
}
void main() {
setup_oscillator(OSC_8MHZ|OSC_INTRC|OSC_31250|OSC_PLL_OFF); // setup internal oscillator to run @ 8MHz
setup_adc_ports(NO_ANALOGS|VSS_VDD);
setup_adc(ADC_CLOCK_DIV_2|ADC_TAD_MUL_0);
setup_psp(PSP_DISABLED);
setup_spi(SPI_MASTER | SPI_MODE_3 | SPI_CLK_DIV_4);
setup_wdt(WDT_ON);
setup_timer_0(RTCC_OFF);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DISABLED,0,1);
setup_timer_3(T3_DISABLED|T3_DIV_BY_1);
setup_ccp1(CCP_OFF);
setup_comparator(NC_NC_NC_NC);
//enable_interrupts(INT_RTCC);
enable_interrupts(GLOBAL);
// LCD connections as per lcd_driver.c (same as that included in the CCS drivers)
// The LCD is connected as shown on the Lab-X1 schematic:
// PIC pin D0 - LCD D0
// PIC pin D1 - LCD D1
// PIC pin D2 - LCD D2
// PIC pin D3 - LCD D3
// PIC pin D4 - LCD D4
// PIC pin D5 - LCD D5
// PIC pin D6 - LCD D6
// PIC pin D7 - LCD D7
// PIC pin E0 - LCD RS
// PIC pin E1 - LCD E
// PIC pin E2 - LCD R/W
// port B
// B0 - unused - OUTPUT
// B1 - INT1 - input
// B2 - INT2 - input
// B3 - unused - OUTPUT
// B4 - unused - OUTPUT
// B5 - unused - OUTPUT
// B6 - unused - OUTPUT
// B7 - unused - OUTPUT
//port_b_pullups(TRUE);
set_tris_b(0x06);
output_b(0x00);
clear_interrupt(INT_EXT1);
ext_int_edge(1, L_TO_H);
enable_interrupts(INT_EXT1);
// port C
// C0 - /CS - OUTPUT
// C1 - unused - OUTPUT
// C2 - unused - OUTPUT
// C3 - SCLK - OUTPUT
// C4 - MISO - INPUT
// C5 - MOSI - OUTPUT
// C6 - unused - OUTPUT
// C7 - unused - OUTPUT
output_c(0xff);
set_tris_c(0x10);
setup_accelerometer();
set_tris_e(0x00);
delay_ms(100);
lcd_init();
delay_ms(100);
lcd_putc("\fAccelerometer Test");
delay_ms(1000);
while (TRUE) {
restart_wdt();
if (refresh_screen == TRUE) {
refresh_screen = FALSE;
read_accel();
calc_angle(Y_PARALLEL_TO_PIPE);
printf(lcd_putc,"\fAngle=%f", theta);
lcd_putc(0xdf);
}
}
} |
| Code: | Accelerometer Driver.c:
#define WRITE 0x00
#define READ 0x80
#define MULTIBYTE_TRANSFER 0x40
#define BW_RATE_REGISTER 0x2c
#define NO_POWER_SAVING 0x00
#define HZ6_25 0x06
#define DATA_FORMAT_REGISTER 0x31
#define DATA_FORMAT_VALUE 0x0b
#define FIFO_CONTROL_REGISTER 0x38
#define FIFO_CONTROL_VALUE 0x00
#define DATA_REG 0x32
#define POWER_CONTROL_REGISTER 0x2d
#define POWER_CONTROL_VALUE 0x08
#define INT_ENABLE_REGISTER 0x2e
#define INT_ENABLE_VALUE 0x80
#define INT_MAP_REGISTER 0x2f
#define INT_MAP_VALUE 0x7f
#define SIGN_BIT 9
#define ACCEL_MASK 0x01ff
int8 read_accel_register(int8 reg) {
int8 register_value;
output_low(CS);
delay_us(10);
spi_write(READ | reg);
register_value = spi_read(0x00);
delay_us(10);
output_high(CS);
return (register_value);
}
void setup_accelerometer(void) {
// set interrupt enable register
output_low(CS);
delay_us(1);
spi_write(WRITE | INT_ENABLE_REGISTER);
spi_write(INT_ENABLE_VALUE);
delay_us(1);
output_high(CS);
// set interrupt map register
output_low(CS);
delay_us(1);
spi_write(WRITE | INT_MAP_REGISTER);
spi_write(INT_MAP_VALUE);
delay_us(1);
output_high(CS);
// set data format register appropriately
output_low(CS);
delay_us(1);
spi_write(WRITE | DATA_FORMAT_REGISTER);
spi_write(DATA_FORMAT_VALUE);
delay_us(1);
output_high(CS);
// set rate register to 6.25Hz conversion, no power saving mode
output_low(CS);
delay_us(1);
spi_write(WRITE | BW_RATE_REGISTER);
spi_write(NO_POWER_SAVING | HZ6_25);
delay_us(1);
output_high(CS);
// set FIFO control register
output_low(CS);
delay_us(1);
spi_write(WRITE | FIFO_CONTROL_REGISTER);
spi_write(FIFO_CONTROL_VALUE);
delay_us(1);
output_high(CS);
// set power control register appropriately
output_low(CS);
delay_us(1);
spi_write(WRITE | POWER_CONTROL_REGISTER);
spi_write(POWER_CONTROL_VALUE);
delay_us(1);
output_high(CS);
}
void read_accel(void) {
int32 scratch;
float scale;
int1 neg_x = FALSE;
int1 neg_y = FALSE;
int1 neg_z = FALSE;
x_accel = 0;
y_accel = 0;
z_accel = 0;
output_low(CS);
delay_us(1);
spi_write(READ | MULTIBYTE_TRANSFER | DATA_REG);
x_accel = spi_read(0); // x low byte
x_accel = x_accel | ((int32)spi_read(0) << 8); // x high byte
y_accel = spi_read(0); // y low byte
y_accel = y_accel | ((int32)spi_read(0) << 8); // y high byte
z_accel = spi_read(0); // z low byte
z_accel = z_accel | ((int32)spi_read(0) << 8); // z high byte
delay_us(1);
output_high(CS);
if (bit_test(x_accel, SIGN_BIT)) {
neg_x = TRUE;
x_accel = ~x_accel;
x_accel++;
x_accel = x_accel & ACCEL_MASK;
}
else {
neg_x = FALSE;
}
if (bit_test(y_accel, SIGN_BIT)) {
neg_y = TRUE;
y_accel = ~y_accel;
y_accel++;
y_accel = y_accel & ACCEL_MASK;
}
else {
neg_y = FALSE;
}
if (bit_test(z_accel, SIGN_BIT)) {
neg_z = TRUE;
z_accel = ~z_accel;
z_accel++;
z_accel = z_accel & ACCEL_MASK;
}
else {
neg_z = FALSE;
}
// had problems when prototyping this as the 2 methods for calculating angle didn't agree due to
// gain imbalances in the x, y and z accelerations
// workaround is to note that all accelerations must add up to 1 g, so normalize
// each by the the total magnitude to compensate
scratch = ((int32)x_accel * (int32)x_accel) + ((int32)y_accel * (int32)y_accel) + ((int32)z_accel * (int32)z_accel);
scale = sqrt((float)scratch);
x_g = x_accel / scale;
if (neg_x == TRUE) {
x_g = -1 * x_g;
}
y_g = y_accel / scale;
if (neg_y == TRUE) {
y_g = -1 * y_g;
}
z_g = z_accel / scale;
if (neg_z == TRUE) {
z_g = -1 * z_g;
}
} |
Trig Functions.c:
| Code: | #define X_PARALLEL_TO_PIPE 0
#define Y_PARALLEL_TO_PIPE 1
#define Z_PARALLEL_TO_PIPE 2
void calc_angle(int8 which) {
float denominator, theta1, theta2;
int1 add_180_to_theta1 = FALSE;
int1 add_360_to_theta2 = FALSE;
int1 rotate_90 = FALSE;
// if x axis is parallel to pipe
// theta (based on y) = sin^-1(-y/cos(sin^-1(x)))
// theta (based on z) = cos^-1(z/cos(sin^-1(x)))
// if y axis is parallel to pipe
// theta (based on z) = sin^-1(-z/cos(sin^-1(y)))
// theta (based on x) = cos^-1(x/cos(sin^-1(y)))
// if z axis is parallel to pipe
// theta (based on x) = sin^-1(-x/cos(sin^-1(z)))
// theta (based on y) = cos^-1(y/cos(sin^-1(z)))
// y parallel to pipe seems to be the most accurate
switch (which) {
case X_PARALLEL_TO_PIPE:
denominator = cos(asin(x_g));
theta1 = asin((-1 * y_g)/denominator);
theta2 = acos(z_g/denominator);
if (z_g < 0) {
add_180_to_theta1 = TRUE;
}
if (y_g >= 0) {
add_360_to_theta2 = TRUE;
}
break;
case Y_PARALLEL_TO_PIPE:
denominator = cos(asin(y_g));
theta1 = asin((-1 * z_g)/denominator);
theta2 = acos(x_g/denominator);
if (x_g < 0) {
add_180_to_theta1 = TRUE;
}
if (z_g >= 0) {
add_360_to_theta2 = TRUE;
}
rotate_90 = TRUE;
break;
case Z_PARALLEL_TO_PIPE:
denominator = cos(asin(z_g));
theta1 = asin((-1 * x_g)/denominator);
theta2 = acos(y_g/denominator);
if (y_g < 0) {
add_180_to_theta1 = TRUE;
}
if (x_g >= 0) {
add_360_to_theta2 = TRUE;
}
rotate_90 = TRUE;
break;
}
theta1 = theta1 * (180 / (4 * atan(1.0)));
if (add_180_to_theta1 == TRUE) {
theta1 = (-1 * theta1) + 180;
}
if (theta1 < 0) {
theta1 = theta1 + 360;
}
theta2 = theta2 * (180 / (4 * atan(1.0)));
if (add_360_to_theta2 == TRUE) {
theta2 = (-1 * theta2) + 360;
}
if ((theta1 > 270) && (theta2 < 90)) {
theta2 = theta2 + 360;
}
else if ((theta2 > 270) && (theta1 < 90)) {
theta1 = theta1 + 360;
}
theta = (theta1 + theta2) / 2;
if (rotate_90 == TRUE) {
theta = theta + 90;
}
if (theta > 360) {
theta = theta - 360;
}
} |
|
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
