I wrote one for a 202/210 earlier this year.. I'll attach it. At first, I didn't know what I was doing, but then after I got out some paper and did the math/optimization it ended up exactly like Kevin Watson's gyro code. It was kinda weird. But anyway, because of the similarity, I just used his framework so everything would make sense more quickly to those who already used it.
If you look at the h file, you see the variables you have to setup. If you look at the clock, and the resolution it gives, then you quickly realize why the limitations of the PIC we use would make data from the accel useless once we start integrating, but I'll leave the deciding up to you.
ACCEL_SCALE_FACTOR is the same as the k in the manual. There is a defined value for it, but you have to test the accel inline with gravity to see where 1g is.
ACCEL_CLOCK_RATE / ACCEL_SAMPLES_PER_UPDATE should never really be less than 50, I guess. I don't remember my exact reason for setting it to three at the time (so maybe commenting isn't such a bad idea
).
accel.h
Code:
/*******************************************************************************
* FILE NAME: x_accel.h <FRC VERSION>
*
* DESCRIPTION:
* This file contains all the header information for x_accel.c.
*
*******************************************************************************/
#ifndef _X_ACCEL_H_
#define _X_ACCEL_H_
/* Put long list of definitions here - ALA user_routines.h in 2004 */
#define FALSE 0
#define TRUE !FALSE
#define ACCEL_SCALE_FACTOR 85 / 1000 /* T1 width that represents 1g of acceleration */
#define ACCEL_SCALE_FACTOR_INVERSE 1000 / 85 /* T1 width that represents 1g of acceleration, inverted */
#define ACCEL_CLOCK_RATE 12800 /* Frequency of the clock used for timing t1 and t2 */
#define ACCEL_SAMPLES_PER_UPDATE 3 /* Number of samples to average for one acceleration output */
void Initialize_Accelerometer(void);
void Disable_Accelerometer(void);
long Get_Acceleration_X(void);
long Get_Acceleration_Y(void);
void Set_Velocity_X(long);
void Set_Velocity_Y(long);
long Get_Velocity_X(void);
long Get_Velocity_Y(void);
void Start_Accelerometer_Bias_Calc(void);
void Stop_Accelerometer_Bias_Calc(void);
void AccelTask_X(void);
void AccelTask_Y(void);
#endif
accel.c
Code:
/*******************************************************************************
* FILE NAME: x_accel.c <FRC VERSION>
*
* DESCRIPTION:
* This file contains all that is needed to receive information from the
* accelerometer.
*
*******************************************************************************/
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "x_accel.h"
#include "x_timer.h"
volatile unsigned int accel_bias_x;
volatile unsigned int accel_bias_y;
volatile signed int accel_accel_x;
volatile signed int accel_accel_x_prev;
volatile signed int accel_accel_y;
volatile signed int accel_accel_y_prev;
volatile long accel_velocity_x;
volatile long accel_velocity_y;
volatile unsigned int accum_x;
volatile unsigned int accum_y;
volatile unsigned char samples_x;
volatile unsigned char samples_y;
volatile unsigned long bias_accum_x;
volatile unsigned long bias_accum_y;
volatile unsigned int bias_samples_x;
volatile unsigned int bias_samples_y;
volatile unsigned char accel_task_x_state;
volatile unsigned char accel_task_y_state;
volatile unsigned long accel_clock_x;
volatile unsigned long accel_clock_y;
volatile unsigned long accel_x_t1;
volatile unsigned long accel_y_t1;
volatile unsigned long accel_t2;
volatile unsigned long accel_t2_sum;
unsigned char calc_accel_bias;
void Initialize_Accelerometer(void)
{
accum_x = 0;
accum_y = 0;
samples_x = 0;
samples_y = 0;
accel_accel_x_prev = 0;
accel_accel_y_prev = 0;
accel_velocity_x = 0;
accel_velocity_y = 0;
accel_task_x_state = 0;
accel_task_y_state = 0;
calc_accel_bias = FALSE;
// initialize external interrupt 1 (INT2 on user 18F8520)
TRISBbits.TRISB2 = 1; // make sure the RB2/INT2 pin is configured as an input [108]
INTCON3bits.INT2IP = 0; // 0: interrupt 1 is low priority (leave at 0 for IFI controllers) [91]
INTCON2bits.INTEDG2 = 1; // 1: trigger on the rising-edge
INTCON3bits.INT2IE = 1; // 1: enable interrupt 1
// initialize external interrupt 2 (INT3 on user 18F8520)
TRISBbits.TRISB3 = 1; // make sure the RB3/CCP2/INT3 pin is configured as an input [108]
INTCON2bits.INT3IP = 0; // 0: interrupt 2 is low priority (leave at 0 for IFI controllers) [90]
INTCON2bits.INTEDG3 = 1; // 1: trigger on the rising-edge
INTCON3bits.INT3IE = 1; // 1: enable interrupt 2
}
void Disable_Accelerometer(void)
{
INTCON3bits.INT2IE = 0;
INTCON3bits.INT3IE = 0;
}
long Get_Acceleration_X(void) // returns in m/s/s multiplied by 10000
{
long temp_accel_accel_x;
INTCON3bits.INT2IE = 0;
temp_accel_accel_x = (long)accel_accel_x;
INTCON3bits.INT2IE = 1;
return (temp_accel_accel_x * (long)98000 / (long)accel_t2 / (long)ACCEL_SAMPLES_PER_UPDATE * (long)ACCEL_SCALE_FACTOR_INVERSE);
}
long Get_Acceleration_Y(void) // returns in m/s/s multiplied by 10000
{
long temp_accel_accel_y;
INTCON3bits.INT3IE = 0;
temp_accel_accel_y = (long)accel_accel_y;
INTCON3bits.INT3IE = 1;
return (temp_accel_accel_y * (long)98000 / (long)accel_t2 / (long)ACCEL_SAMPLES_PER_UPDATE * (long)ACCEL_SCALE_FACTOR_INVERSE);
}
long Get_Velocity_X(void) // returns in m/s multiplied by 10000
{
long temp_accel_velocity_x;
INTCON3bits.INT2IE = 0;
temp_accel_velocity_x = accel_velocity_x;
INTCON3bits.INT2IE = 1;
return (temp_accel_velocity_x * 98000L / (long)ACCEL_CLOCK_RATE * (long)ACCEL_SCALE_FACTOR_INVERSE);
}
long Get_Velocity_Y(void) // returns in m/s multiplied by 10000
{
long temp_accel_velocity_y;
INTCON3bits.INT3IE = 0;
temp_accel_velocity_y = accel_velocity_y;
INTCON3bits.INT3IE = 1;
return (temp_accel_velocity_y * 98000L / (long)ACCEL_CLOCK_RATE * (long)ACCEL_SCALE_FACTOR_INVERSE);
}
void Set_Velocity_X(long temp_accel_velocity_x) // input in m/s multiplied by 10000
{
INTCON3bits.INT2IE = 0;
accel_velocity_x = temp_accel_velocity_x * (long)ACCEL_SCALE_FACTOR * (long)ACCEL_CLOCK_RATE / 98000L;
INTCON3bits.INT2IE = 1;
}
void Set_Velocity_Y(long temp_accel_velocity_y) // input in m/s multiplied by 10000
{
INTCON3bits.INT3IE = 0;
accel_velocity_y = temp_accel_velocity_y * (long)ACCEL_SCALE_FACTOR * (long)ACCEL_CLOCK_RATE / 98000L;
INTCON3bits.INT3IE = 1;
}
void Start_Accelerometer_Bias_Calc(void)
{
if(calc_accel_bias == FALSE)
{
INTCON3bits.INT2IE = 0;
INTCON3bits.INT3IE = 0;
bias_accum_x = 0;
bias_accum_y = 0;
bias_samples_x = 0;
bias_samples_y = 0;
calc_accel_bias = TRUE;
accum_x = 0;
accum_y = 0;
samples_x = 0;
samples_y = 0;
accel_t2 = 0;
accel_t2_sum = 0;
INTCON3bits.INT2IE = 1;
INTCON3bits.INT3IE = 1;
}
}
void Stop_Accelerometer_Bias_Calc(void)
{
if(calc_accel_bias == TRUE)
{
INTCON3bits.INT2IE = 0;
INTCON3bits.INT3IE = 0;
accel_bias_x = (unsigned int)(bias_accum_x / (long)bias_samples_x);
accel_bias_y = (unsigned int)(bias_accum_y / (long)bias_samples_y);
accel_t2 = accel_t2_sum / (((unsigned long)bias_samples_y * (unsigned long)ACCEL_SAMPLES_PER_UPDATE) + (unsigned long)samples_y - (unsigned long)1);
accel_accel_x_prev = 0;
accel_accel_y_prev = 0;
calc_accel_bias = FALSE;
accum_x = 0;
accum_y = 0;
samples_x = 0;
samples_y = 0;
INTCON3bits.INT2IE = 1;
INTCON3bits.INT3IE = 1;
}
}
void AccelTask_X(void)
{
accel_clock_x = Get_Clock_12800();
switch(accel_task_x_state)
{
case 0: // Very first time
{
accel_x_t1 = accel_clock_x;
accel_task_x_state = 1;
INTCON2bits.INTEDG2 = 0;
break;
}
case 1: // Has fallen from 1 to 0
{
accel_x_t1 = accel_clock_x - accel_x_t1;
accel_task_x_state = 2;
INTCON2bits.INTEDG2 = 1;
break;
}
case 2: // Has risen from 0 to 1, and not first time
{
accum_x = accum_x + (unsigned int)accel_x_t1;
samples_x++;
if(samples_x >= ACCEL_SAMPLES_PER_UPDATE)
{
if(calc_accel_bias == TRUE)
{
bias_accum_x = bias_accum_x + (unsigned long)accum_x;
bias_samples_x++;
}
else
{
accel_accel_x = (signed int)(accum_x - accel_bias_x);
accel_velocity_x = accel_velocity_x + (((long) accel_accel_x + (long) accel_accel_x_prev)>>1);
accel_accel_x_prev = accel_accel_x;
}
accum_x = 0;
samples_x = 0;
}
accel_x_t1 = accel_clock_x;
accel_task_x_state = 1;
INTCON2bits.INTEDG2 = 0;
break;
}
default:
{
accel_task_x_state = 0;
INTCON2bits.INTEDG2 = 1;
}
}
}
void AccelTask_Y(void)
{
accel_clock_y = Get_Clock_12800();
switch(accel_task_y_state)
{
case 0: // Very first time
{
accel_y_t1 = accel_clock_y;
accel_task_y_state = 1;
INTCON2bits.INTEDG3 = 0;
break;
}
case 1: // Has fallen from 1 to 0
{
accel_y_t1 = accel_clock_y - accel_y_t1;
accel_task_y_state = 2;
INTCON2bits.INTEDG3 = 1;
break;
}
case 2: // Has risen from 0 to 1, and not first time
{
accum_y = accum_y + (unsigned int)accel_y_t1;
samples_y++;
if ((accel_t2 == 0) && (calc_accel_bias == TRUE))
{
accel_t2 = accel_clock_y;
} else if (calc_accel_bias == TRUE)
{
accel_t2_sum = accel_t2_sum + accel_clock_y - accel_t2;
accel_t2 = 0;
}
if(samples_y >= ACCEL_SAMPLES_PER_UPDATE)
{
if(calc_accel_bias == TRUE)
{
bias_accum_y = bias_accum_y + (unsigned long)accum_y;
bias_samples_y++;
}
else
{
accel_accel_y = (signed int)(accum_y - accel_bias_y);
accel_velocity_y = accel_velocity_y + (((long) accel_accel_y + (long) accel_accel_y_prev)>>1);
accel_accel_y_prev = accel_accel_y;
}
accum_y = 0;
samples_y = 0;
}
accel_y_t1 = accel_clock_y;
accel_task_y_state = 1;
INTCON2bits.INTEDG3 = 0;
break;
}
default:
{
accel_task_y_state = 0;
INTCON2bits.INTEDG3 = 1;
}
}
}
put this in user_routines.c :: void User_Initialization (void), after including the accel.h file:
Code:
Initialize_Accelerometer();
and then put calls to AccelTask_X() and AccelTask_Y() in the respective interrupts (X is on 1, and Y is on 2, I believe -- if you change, then modify the Initialize function of the accelerometer also) in user_routines_fast.c :: void InterruptHandlerLow ()
x_timer.h
Code:
/*******************************************************************************
* FILE NAME: x_timer.h <FRC VERSION>
*
* DESCRIPTION:
* This file contains all the header information for x_timer.c.
*
*******************************************************************************/
#ifndef _X_TIMER_H_
#define _X_TIMER_H_
/* Put long list of definitions here - ALA user_routines.h in 2004 */
/* Put variables here */
/* Put function prototypes here */
void Timer2Task(void);
unsigned long Get_Clock_12800(void);
void Set_Clock_12800(unsigned long);
void Initialize_Timer_2(void);
void Initialize_Timer_4(void);
#endif
x_timer.c
Code:
/*******************************************************************************
* FILE NAME: x_timer.c <FRC VERSION>
*
* DESCRIPTION:
* This file contains all that is needed to have nice timers.
*
*******************************************************************************/
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "x_timer.h"
#include "x_gyro.h"
volatile unsigned long Clock_12800 = 0;
void Timer2Task(void)
{
static unsigned char divisor = 0;
Clock_12800++; // increment the clock
/* if (++divisor >= 8) // This is the 800Hz clock
{
divisor = 0;
GyroTask(); // Do gyro stuff..
} */
}
unsigned long Get_Clock_12800(void)
{
unsigned long temp_clock_12800;
PIE1bits.TMR2IE = 0;
temp_clock_12800 = Clock_12800;
PIE1bits.TMR2IE = 1;
// return the clock_25000 to caller
return(temp_clock_12800);
}
void Set_Clock_12800(unsigned long temp_clock_12800)
{
PIE1bits.TMR2IE = 0;
Clock_12800 = temp_clock_12800;
PIE1bits.TMR2IE = 1;
}
/*******************************************************************************
*
* FUNCTION: Initialize_Timer_2()
*
* PURPOSE: Initializes the timer 2 hardware.
*
* CALLED FROM: user_routines.c/User_Initialization()
*
* PARAMETERS: None
*
* RETURNS: Nothing
*
* COMMENTS: Timer 2 documentation starts on page 141 of the data sheet.
*
* Timer 2 will be setup with these parameters to generate
* a 1000Hz interrupt rate:
*
* 1) The prescaler is set to divide the 10MHz system clock
* by 4 (i.e., 1:4) so that TMR2 will be clocked at 2.5MHz.
*
* 2) The PR2 register is set to 249 so that the TMR2
* register will roll-over (i.e., transition from 249 -> 0)
* at a 10,000Hz rate (2.5MHz/250).
*
* 3) The postscaler is set to generate an interrupt every
* tenth time the TMR2 register rolls-over (10,000Hz/10=1000Hz).
*
* 4) The timer 2 interrupt enable bit (TMR2IE) is set to
* 1 to enable the timer 2 interrupt.
*
* 5) Finally, timer 2 is allowed to run by setting the TMR2ON
* bit to 1.
*
*******************************************************************************/
void Initialize_Timer_2(void)
{ // This timer running at 12800Hz
// 6400: 1:4, 1:15, 24
// 12800: 1:4, 1:15, 11
// 25600: 1:16, 1:6, 2
// 51200: 1:4, 1:12, 2
TMR2 = 0x00; // 8-bit timer 2 register (this is readable and writable)
//
PR2 = 24; // 13.0208 - 2 timer 2 period register - timer 2 increments to this
// value then resets to zero on the next clock and starts
// all over again
//
T2CONbits.T2OUTPS0 = 0; // T2OUTPS3 T2OUTPS2 T2OUTPS1 T2OUTPS0
T2CONbits.T2OUTPS1 = 1; // 0 0 0 0 1:1 postscaler
T2CONbits.T2OUTPS2 = 1; // 0 0 0 1 1:2 postscaler
T2CONbits.T2OUTPS3 = 1; // 0 0 1 0 1:3 postscaler
// 0 0 1 1 1:4 postscaler
// 0 1 0 0 1:5 postscaler
// 0 1 0 1 1:6 postscaler
// 0 1 1 0 1:7 postscaler
// 0 1 1 1 1:8 postscaler
// 1 0 0 0 1:9 postscaler
// 1 0 0 1 1:10 postscaler
// 1 0 1 0 1:11 postscaler
// 1 0 1 1 1:12 postscaler
// 1 1 0 0 1:13 postscaler
// 1 1 0 1 1:14 postscaler
// 1 1 1 0 1:15 postscaler
// 1 1 1 1 1:16 postscaler
//
T2CONbits.T2CKPS0 = 1; // T2CKPS1 T2CKPS0
T2CONbits.T2CKPS1 = 0; // 0 0 1:1 prescaler (clock = 10MHz/each tick=100ns)
// 0 1 1:4 prescaler (clock = 2.5MHz/each tick=400ns)
// 1 x 1:16 prescaler (clock = 625KHz/each tick=1.6us) (T2CKPS0 doesn't matter)
//
PIE1bits.TMR2IE = 1; // 0: disable timer 2 interrupt on PR2 match
// 1: enable timer 2 interrupt on PR2 match
// if the prescaler is enabled (i.e., greater than 1:1), this
// match will occur n times (where n is the postscaler value)
// before an interrupt will be generated
//
IPR1bits.TMR2IP = 0; // 0: timer 2 overflow interrupt is low priority (leave at 0 for IFI controllers)
// 1: timer 2 overflow interrupt is high priority
//
T2CONbits.TMR2ON = 1; // 0: timer 2 is disabled
// 1: timer 2 is enabled (running)
}
void Initialize_Timer_4(void)
{ // This timer set to 800Hz
TMR4 = 0x00; // 8-bit timer 4 register (this is readable and writable)
//
PR4 = 69; // timer 4 period register - timer 4 increments to this
// value then resets to zero on the next clock and starts
// all over again
//
T4CONbits.T4OUTPS0 = 0; // T4OUTPS3 T4OUTPS2 T4OUTPS1 T4OUTPS0
T4CONbits.T4OUTPS1 = 1; // 0 0 0 0 1:1 postscaler
T4CONbits.T4OUTPS2 = 0; // 0 0 0 1 1:2 postscaler
T4CONbits.T4OUTPS3 = 1; // 0 0 1 0 1:3 postscaler
// 0 0 1 1 1:4 postscaler
// 0 1 0 0 1:5 postscaler
// 0 1 0 1 1:6 postscaler
// 0 1 1 0 1:7 postscaler
// 0 1 1 1 1:8 postscaler
// 1 0 0 0 1:9 postscaler
// 1 0 0 1 1:10 postscaler
// 1 0 1 0 1:11 postscaler
// 1 0 1 1 1:12 postscaler
// 1 1 0 0 1:13 postscaler
// 1 1 0 1 1:14 postscaler
// 1 1 1 0 1:15 postscaler
// 1 1 1 1 1:16 postscaler
//
T4CONbits.T4CKPS0 = 0; // T4CKPS1 T4CKPS0
T4CONbits.T4CKPS1 = 1; // 0 0 1:1 prescaler (clock = 10MHz/each tick=100ns)
// 0 1 1:4 prescaler (clock = 2.5MHz/each tick=400ns)
// 1 x 1:16 prescaler (clock = 625KHz/each tick=1.6us) (T2CKPS0 doesn't matter)
//
PIE3bits.TMR4IE = 1; // 0: disable timer 4 interrupt on PR4 match
// 1: enable timer 4 interrupt on PR4 match
// if the prescaler is enabled (i.e., greater than 1:1), this
// match will occur n times (where n is the postscaler value)
// before an interrupt will be generated
//
IPR3bits.TMR4IP = 0; // 0: timer 4 interrupt is low priority (leave at 0 for IFI controllers)
// 1: timer 4 interrupt is high priority
//
T4CONbits.TMR4ON = 1; // 0: timer 4 is disabled
// 1: timer 4 is enabled (running)
}
You can give that a shot, and see if it works for you.