|
|
|
![]() |
|
|||||||
|
||||||||
![]() |
| Thread Tools |
Rating:
|
Display Modes |
|
#1
|
|||
|
|||
|
Accelerometer code
I've been working on writing code for an accelerometer that outputs a PWM signal. I've been trying to adapt some of Kevin's code that dealt with an infrared sensor that output PWM signals.
the following is Kevin's with a few variable name changes switch(RB5_State) // current state determines how the function behaves { case 0: // falling-edge detected (beginning of the pulse) Int_4_Down_Edge_Count_High = Clock; // get a snapshot of the time tbuf = TMR1L; // TMR1L must be read before TMR1H Int_4_Down_Edge_Count_Low = TMR1H; Int_4_Down_Edge_Count_Low <<= 8; Int_4_Down_Edge_Count_Low += tbuf; break; // now wait for the rising-edge interrupt to happen... case 1: // rising-edge detected (end of the pulse) Int_4_Up_Edge_Count_High = Clock; // get a snapshot of the time tbuf = TMR1L; Int_4_Up_Edge_Count_Low = TMR1H; Int_4_Up_Edge_Count_Low <<= 8; Int_4_Up_Edge_Count_Low += tbuf; // determine the pulse-width period by determining the time // difference between the falling-edge and rising-edge interrupts if (Int_4_Up_Edge_Count_High == Int_4_Down_Edge_Count_High) { // this is quicker because the 16-bit system clock hasn't changed and therefore has no effect on the outcome Int_4_Period = Int_4_Up_Edge_Count_Low - Int_4_Down_Edge_Count_Low; } else { //this works because the pulse-width will always be less than one clock tick(= 65536 timer ticks) //(=0.0000001 seconds) 1 * 10^-7 sec (0.1 micro seconds) Int_4_Period = 65536 - Int_4_Down_Edge_Count_Low + Int_4_Up_Edge_Count_Low; } break; // now wait for another falling-edge interrupt to happen... now i have my accelerometer plugged into digital input 3 and 4 (x and y PWMs) as far as i can tell the RB5 pin never switches state because when i printf("data:%d",Int_4_Down_Edge_Count_High); or Int_4_Up_Edge_Count_High the value is always 0 so the clock value never gets assigned meaning it never entered the switch statement. It does the same thing for the interrupt on digital input 3 Any words of advice or help would be greatly appreciated. Thanks. |
|
#2
|
||||
|
||||
|
Re: Accelerometer code
(Use [code] brackets when applicable.
)Well... Could we see the rest of the code? One thing I can think of right now is the docs recommend putting a \r at the end of the printf... :shrug: |
|
#3
|
||||
|
||||
|
Re: Accelerometer code
Some questions:
1) Are you setting up and enabling the interrupts? If so, let's see the code. 2) Is your function getting called from InterruptHandlerLow()? 3) What platform are you using? 4) Which accelerometer are you using? -Kevin |
|
#4
|
||||
|
||||
|
Re: Accelerometer code
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 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;
}
}
}
Code:
Initialize_Accelerometer(); 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 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)
}
Last edited by Joel J : 02-06-2005 at 13:43. Reason: including timer files, so the thing will run.. heh. |
|
#5
|
|||
|
|||
|
Re: Accelerometer code
ok, based on kevin's response i'm clearly missing something with the interrupts
being called. here's the code i'm using htis accelerometer http://www.analog.com/UploadedFiles/...DXL202EB_a.pdf Code:
/*
*Alpha 1 for accelerometer
* //accelerometer has pwm frequencies of 263.158 Hz (3.8 ms) (we thought)
*
*/
#include "myaccelerometer.h"
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_picdefs.h"
volatile int xacceleration;
volatile int yacceleration;
unsigned int Clock = 0; // upper 16 bits of the 32 bit system clock
// timer 1 contains the lower 16 bits
// each clock tick is worth 26.21 milliseconds
void Initialize_Accelerometer(void)
{
// initialize and start timer 1
Initialize_Timer_1();
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// initialize external interrupts 3-6 (KBI0 - KBI3 on user 18F8520)
TRISBbits.TRISB4 = 1; // make sure RB4/KBI0 is setup as an input [108]
TRISBbits.TRISB5 = 1; // make sure RB5/KBI1 is setup as an input [108]
INTCON2bits.RBIP = 0; // interrupts 3 through 6 will use the low priority interrupt [90]
INTCONbits.RBIE = 1; // enable interrupts 3 through 6 [89]
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
int Get_xAcceleration(void)//called from user level to get current calculated x acceleration value
{
int temp_xacceleration;
//disable timer 1 interrupt
PIE1bits.TMR1IE = 0;
temp_xacceleration = xacceleration;
//timer 1 interrupt back on
PIE1bits.TMR1IE = 1;
// Return the x acceleration to the caller.
return(temp_xacceleration);
}
int Get_yAcceleration(void)//called from user level to get current calculated y acceleration value
{
int temp_yacceleration;
//disable timer 1 interrupt
PIE1bits.TMR1IE = 0;
temp_yacceleration = yacceleration;
//timer 1 interrupt back on
PIE1bits.TMR1IE = 1;
// Return the y acceleration to the caller.
return(temp_yacceleration);
}
void Initialize_Timer_1(void)
{
/////////////////////////////////////////////////////////////////////////////////////////////////////
// initialize and start timer 1
IPR1bits.TMR1IP = 0; // timer 1 will use the low priority interrupt [98]
PIE1bits.TMR1IE = 1; // enable timer1 roll-over interrupt [95]
T1CONbits.RD16 = 1; // use 16-bit read/write operations with timer 1 [135]
T1CONbits.T1CKPS0 = 0; // timer 1 clock will use a 4:1 prescale value [135]
T1CONbits.T1CKPS1 = 1;
T1CONbits.T1OSCEN = 0; // turn-off the timer 1 oscillator to save power [135]
T1CONbits.TMR1CS = 0; // timer 1 will use the internal 10MHz clock [135]
T1CONbits.TMR1ON = 1; // start timer 1 [135]
////////////////////////////////////////////////////////////////////////////////////////////////////
}
void Timer_1_Int_Handler(void)
{
Clock++; // increment the upper 16-bits of the system clock
}
void Int_3_Handler(unsigned char RB4_State) //parts of this provided by K. Watson from JPL who helps out FIRST teams with code
//it appeared as help for pwm input for an infrared tracking device
{
unsigned char tempbuf; // temporary data buffer
unsigned int Int_3_Period; // pulse-width period
unsigned int Int_3_Up_Edge_Count_Low; // lower 16-bits of the rising-edge time snapshot
unsigned int Int_3_Up_Edge_Count_High; // upper 16-bits of the rising-edge time snapshot
static unsigned int Int_3_Down_Edge_Count_Low; // lower 16-bits of the falling-edge time snapshot
static unsigned int Int_3_Down_Edge_Count_High; // upper 16-bits of the falling-edge time snapshot
switch(RB4_State) // current state determines how the function behaves
{
case 0: // falling-edge detected (beginning of the pulse)
Int_3_Down_Edge_Count_High = Clock; // get a snapshot of the time
tempbuf = TMR1L; // TMR1L must be read before TMR1H
Int_3_Down_Edge_Count_Low = TMR1H;
Int_3_Down_Edge_Count_Low <<= 8;
Int_3_Down_Edge_Count_Low += tempbuf;
break; // now wait for the rising-edge interrupt to happen...
case 1: // rising-edge detected (end of the pulse)
Int_3_Up_Edge_Count_High = Clock; // get a snapshot of the time
tempbuf = TMR1L;
Int_3_Up_Edge_Count_Low = TMR1H;
Int_3_Up_Edge_Count_Low <<= 8;
Int_3_Up_Edge_Count_Low += tempbuf;
// determine the pulse-width period by determining the time
// difference between the falling-edge and rising-edge interrupts
if (Int_3_Up_Edge_Count_High == Int_3_Down_Edge_Count_High)
{
// this is quicker because the 16-bit system clock hasn't changed and therefore has no effect on the outcome
Int_3_Period = Int_3_Up_Edge_Count_Low - Int_3_Down_Edge_Count_Low;
}
else
{
//this works because the pulse-width will always be less than one clock tick(= 65536 timer ticks)
//(=0.0000001 seconds) 1 * 10^-7 sec (0.1 micro seconds)
Int_3_Period = 65536 - Int_3_Down_Edge_Count_Low + Int_3_Up_Edge_Count_Low;
}
break; // now wait for another falling-edge interrupt to happen...
}
/*
A(g)=(T1/T2 - 0.5)/0.0125
T2 is set by resistor (1.2 M Ohm resistor yields 9.6 ms T2)
at rest 0g=50% Duty cycle
(Int_3_Period/65536)/26.21 ----> yields Int_3_Period in ms
{[(Int_3_Period in ms)/(9.6 ms T2)]-0.5}/0.0125 -------> yields acceleration in g's
but from what i understand this thing doesn't like floating point numbers
so, plug it into a TI-89 with Int_3 as x, go to approx mode and hit expand
you get this:
(x/206124) - 40
so we will use this instead as its both simpler and only deals with integers
for x's smaller than 500,000 there is essentially no error in using this approx (0.0002 percent)
*/
// xacceleration=(Int_3_Period/206124) - 40; THIS IS THE ONE WE WANT WHEN IT WORKS
// xacceleration=(Int_3_Period); ---------> this doesn't even give me anything (seems that Int_3_Period always remains at 0)
//xacceleration=Int_3_Down_Edge_Count_High; ------>never gets here or at least it says its always 0
//xacceleration=Int_3_Up_Edge_Count_High; ---------> never gets here either
}
//parts of this provided by K. Watson from JPL who helps out FIRST teams with code
//it appeared as help for pwm input for an infrared tracking device
void Int_4_Handler(unsigned char RB5_State)
{
unsigned char tbuf; // temporary data buffer
unsigned int Int_4_Period; // pulse-width period
unsigned int Int_4_Up_Edge_Count_Low; // lower 16-bits of the rising-edge time snapshot
unsigned int Int_4_Up_Edge_Count_High; // upper 16-bits of the rising-edge time snapshot
static unsigned int Int_4_Down_Edge_Count_Low; // lower 16-bits of the falling-edge time snapshot
static unsigned int Int_4_Down_Edge_Count_High; // upper 16-bits of the falling-edge time snapshot
switch(RB5_State) // current state determines how the function behaves
{
case 0: // falling-edge detected (beginning of the pulse)
Int_4_Down_Edge_Count_High = Clock; // get a snapshot of the time
tbuf = TMR1L; // TMR1L must be read before TMR1H
Int_4_Down_Edge_Count_Low = TMR1H;
Int_4_Down_Edge_Count_Low <<= 8;
Int_4_Down_Edge_Count_Low += tbuf;
break; // now wait for the rising-edge interrupt to happen...
case 1: // rising-edge detected (end of the pulse)
Int_4_Up_Edge_Count_High = Clock; // get a snapshot of the time
tbuf = TMR1L;
Int_4_Up_Edge_Count_Low = TMR1H;
Int_4_Up_Edge_Count_Low <<= 8;
Int_4_Up_Edge_Count_Low += tbuf;
// determine the pulse-width period by determining the time
// difference between the falling-edge and rising-edge interrupts
if (Int_4_Up_Edge_Count_High == Int_4_Down_Edge_Count_High)
{
// this is quicker because the 16-bit system clock hasn't changed and therefore has no effect on the outcome
Int_4_Period = Int_4_Up_Edge_Count_Low - Int_4_Down_Edge_Count_Low;
}
else
{
//this works because the pulse-width will always be less than one clock tick(= 65536 timer ticks)
//(=0.0000001 seconds) 1 * 10^-7 sec (0.1 micro seconds)
Int_4_Period = 65536 - Int_4_Down_Edge_Count_Low + Int_4_Up_Edge_Count_Low;
}
break; // now wait for another falling-edge interrupt to happen...
}
/*
A(g)=(T1/T2 - 0.5)/0.0125
T2 is set by resistor (1.2 M Ohm resistor yields 9.6 ms T2)
at rest 0g=50% Duty cycle
(Int_4_Period/65536)*10000 ----> yields Int_4_Period in ms
{[(Int_4_Period in ms)/(9.6 ms T2)]-0.5}/0.0125 -------> yields acceleration in g's
but from what i understand this thing doesn't like floating point numbers
so, plug it into a TI-89 with Int_3 as x, go to approx mode and hit expand
you get this:
(x/206124) - 40
so we will use this instead as its both simpler and only deals with integers
for x's smaller than 500,000 there is essentially no error in using this approx (0.0002 percent)
*/
yacceleration=(Int_4_Period/206124) - 40;
}
Code:
/* *Alpha 1 for accelerometer *goal: get any sort of reading off the signal pins *and print it to the screen so we can see what we are dealing with * */ #ifndef _accelerometer_h #define _accelerometer_h void Initialize_Accelerometer(void); // initializes and starts the accelerometer code int Get_xAcceleration(void); // returns the current measured x acceleration int Get_yAcceleration(void); // returns the current measured y acceleration void Initialize_Timer_1(void); // initializes and starts timer 1 void Int_3_Handler(unsigned char); // interrupt 3 routine (x axis) power yellow void Int_4_Handler(unsigned char); // interrupt 4 routine (y axis) red white void Timer_1_Int_Handler(void); //timer routine // some handy macros #define HIBYTE(value) ((unsigned char)(((unsigned int)(value)>>8)&0xFF)) #define LOBYTE(value) ((unsigned char)(value)) // state machine defines #define WAITING_FOR_UP_EDGE 0 #define WAITING_FOR_DOWN_EDGE 1 extern unsigned int Clock; //in myaccel.c #endif Code:
/*******************************************************************************
* FILE NAME: user_routines.c <FRC VERSION>
*
* DESCRIPTION:
* This file contains the default mappings of inputs
* (like switches, joysticks, and buttons) to outputs on the RC.
*
* USAGE:
* You can either modify this file to fit your needs, or remove it from your
* project and replace it with a modified copy.
*
*******************************************************************************/
#include <stdio.h>
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "user_Serialdrv.h"
#include "myaccelerometer.h"
/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/
extern unsigned char aBreakerWasTripped;
/*******************************************************************************
* FUNCTION NAME: Limit_Switch_Max
* PURPOSE: Sets a PWM value to neutral (127) if it exceeds 127 and the
* limit switch is on.
* CALLED FROM: this file
* ARGUMENTS:
* Argument Type IO Description
* -------- ------------- -- -----------
* switch_state unsigned char I limit switch state
* *input_value pointer O points to PWM byte value to be limited
* RETURNS: void
*******************************************************************************/
void Limit_Switch_Max(unsigned char switch_state, unsigned char *input_value)
{
if (switch_state == CLOSED)
{
if(*input_value > 127)
*input_value = 127;
}
}
/*******************************************************************************
* FUNCTION NAME: Limit_Switch_Min
* PURPOSE: Sets a PWM value to neutral (127) if it's less than 127 and the
* limit switch is on.
* CALLED FROM: this file
* ARGUMENTS:
* Argument Type IO Description
* -------- ------------- -- -----------
* switch_state unsigned char I limit switch state
* *input_value pointer O points to PWM byte value to be limited
* RETURNS: void
*******************************************************************************/
void Limit_Switch_Min(unsigned char switch_state, unsigned char *input_value)
{
if (switch_state == CLOSED)
{
if(*input_value < 127)
*input_value = 127;
}
}
/*******************************************************************************
* FUNCTION NAME: Limit_Mix
* PURPOSE: Limits the mixed value for one joystick drive.
* CALLED FROM: Default_Routine, this file
* ARGUMENTS:
* Argument Type IO Description
* -------- ---- -- -----------
* intermediate_value int I
* RETURNS: unsigned char
*******************************************************************************/
unsigned char Limit_Mix (int intermediate_value)
{
static int limited_value;
if (intermediate_value < 2000)
{
limited_value = 2000;
}
else if (intermediate_value > 2254)
{
limited_value = 2254;
}
else
{
limited_value = intermediate_value;
}
return (unsigned char) (limited_value - 2000);
}
/*******************************************************************************
* FUNCTION NAME: User_Initialization
* PURPOSE: This routine is called first (and only once) in the Main function.
* You may modify and add to this function.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void User_Initialization (void)
{
Set_Number_of_Analog_Channels(SIXTEEN_ANALOG); /* DO NOT CHANGE! */
/* FIRST: Set up the I/O pins you want to use as digital INPUTS. */
digital_io_01 = digital_io_02 = digital_io_03 = digital_io_04 = INPUT;
digital_io_05 = digital_io_06 = digital_io_07 = digital_io_08 = INPUT;
digital_io_09 = digital_io_10 = digital_io_11 = digital_io_12 = INPUT;
digital_io_13 = digital_io_14 = digital_io_15 = digital_io_16 = INPUT;
digital_io_18 = INPUT; /* Used for pneumatic pressure switch. */
/*
Note: digital_io_01 = digital_io_02 = ... digital_io_04 = INPUT;
is the same as the following:
digital_io_01 = INPUT;
digital_io_02 = INPUT;
...
digital_io_04 = INPUT;
*/
/* SECOND: Set up the I/O pins you want to use as digital OUTPUTS. */
digital_io_17 = OUTPUT; /* Example - Not used in Default Code. */
/* THIRD: Initialize the values on the digital outputs. */
rc_dig_out17 = 0;
/* FOURTH: Set your initial PWM values. Neutral is 127. */
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
/* FIFTH: Set your PWM output types for PWM OUTPUTS 13-16.
/* Choose from these parameters for PWM 13-16 respectively: */
/* IFI_PWM - Standard IFI PWM output generated with Generate_Pwms(...) */
/* USER_CCP - User can use PWM pin as digital I/O or CCP pin. */
Setup_PWM_Output_Type(IFI_PWM,IFI_PWM,IFI_PWM,IFI_PWM);
/*
Example: The following would generate a 40KHz PWM with a 50% duty cycle on the CCP2 pin:
CCP2CON = 0x3C;
PR2 = 0xF9;
CCPR2L = 0x7F;
T2CON = 0;
T2CONbits.TMR2ON = 1;
Setup_PWM_Output_Type(USER_CCP,IFI_PWM,IFI_PWM,IFI_PWM);
*/
/* Add any other initialization code here. */
Putdata(&txdata); /* DO NOT CHANGE! */
Serial_Driver_Initialize();
Initialize_Serial_Comms();
//printf("IFI 2005 User Processor Initialized ...\r"); /* Optional - Print initialization message. */
/* Note: use a '\r' rather than a '\n' with the new compiler (v2.4) */
User_Proc_Is_Ready(); /* DO NOT CHANGE! - last line of User_Initialization */
}
/*******************************************************************************
* FUNCTION NAME: Process_Data_From_Master_uP
* PURPOSE: Executes every 26.2ms when it gets new data from the master
* microprocessor.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void Process_Data_From_Master_uP(void)
{
static unsigned char i;
static unsigned char delay;
int xaccel;
int yaccel;
Getdata(&rxdata); /* Get fresh data from the master microprocessor. */
Default_Routine(); /* Optional. See below. */
Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
/* Eample code to check if a breaker was ever tripped. */
if (aBreakerWasTripped)
{
for (i=1;i<29;i++)
{
if (Breaker_Tripped(i))
User_Byte1 = i; /* Update the last breaker tripped on User_Byte1 (to demonstrate the use of a user byte)
// Normally, you do something else if a breaker got tripped (ex: limit a PWM output) */
}
}
///////////////////////////////////////////////////////////////////
//get x and y accel and print them to the serial port
xaccel=Get_xAcceleration();
printf("The x accel: %d", xaccel);
//////////////////////////////////////////////////////////////
yaccel=Get_yAcceleration();
printf("The y accel: %d", yaccel);
//////////////////////////////////////////////////////////////////
Putdata(&txdata); /* DO NOT CHANGE! */
}
/*******************************************************************************
* FUNCTION NAME: Default_Routine
* PURPOSE: Performs the default mappings of inputs to outputs for the
* Robot Controller.
* CALLED FROM: this file, Process_Data_From_Master_uP routine
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void Default_Routine(void)
{
/*---------- Analog Inputs (Joysticks) to PWM Outputs-----------------------
*--------------------------------------------------------------------------
* This maps the joystick axes to specific PWM outputs.
*/
pwm01 = p1_y;
pwm02 = p2_y;
pwm03 = p3_y;
pwm04 = p4_y;
pwm05 = p1_x;
pwm06 = p2_x;
pwm07 = p3_x;
pwm08 = p4_x;
pwm09 = p1_wheel;
pwm10 = p2_wheel;
pwm11 = p3_wheel;
pwm12 = p4_wheel;
/*---------- 1 Joystick Drive ----------------------------------------------
*--------------------------------------------------------------------------
* This code mixes the Y and X axis on Port 1 to allow one joystick drive.
* Joystick forward = Robot forward
* Joystick backward = Robot backward
* Joystick right = Robot rotates right
* Joystick left = Robot rotates left
* Connect the right drive motors to PWM13 and/or PWM14 on the RC.
* Connect the left drive motors to PWM15 and/or PWM16 on the RC.
*/
/*---------- Buttons to Relays----------------------------------------------
*--------------------------------------------------------------------------
* This default code maps the joystick buttons to specific relay outputs.
* Relays 1 and 2 use limit switches to stop the movement in one direction.
* The & used below is the C symbol for AND
*/
relay1_fwd = p1_sw_trig & rc_dig_in01; /* FWD only if switch1 is not closed. */
relay1_rev = p1_sw_top & rc_dig_in02; /* REV only if switch2 is not closed. */
relay2_fwd = p2_sw_trig & rc_dig_in03; /* FWD only if switch3 is not closed. */
relay2_rev = p2_sw_top & rc_dig_in04; /* REV only if switch4 is not closed. */
relay3_fwd = p3_sw_trig;
relay3_rev = p3_sw_top;
relay4_fwd = p4_sw_trig;
relay4_rev = p4_sw_top;
relay5_fwd = p1_sw_aux1;
relay5_rev = p1_sw_aux2;
relay6_fwd = p3_sw_aux1;
relay6_rev = p3_sw_aux2;
relay7_fwd = p4_sw_aux1;
relay7_rev = p4_sw_aux2;
relay8_fwd = !rc_dig_in18; /* Power pump only if pressure switch is off. */
relay8_rev = 0;
/*---------- PWM outputs Limited by Limit Switches ------------------------*/
Limit_Switch_Max(rc_dig_in05, &pwm03);
Limit_Switch_Min(rc_dig_in06, &pwm03);
Limit_Switch_Max(rc_dig_in07, &pwm04);
Limit_Switch_Min(rc_dig_in08, &pwm04);
Limit_Switch_Max(rc_dig_in09, &pwm09);
Limit_Switch_Min(rc_dig_in10, &pwm09);
Limit_Switch_Max(rc_dig_in11, &pwm10);
Limit_Switch_Min(rc_dig_in12, &pwm10);
Limit_Switch_Max(rc_dig_in13, &pwm11);
Limit_Switch_Min(rc_dig_in14, &pwm11);
Limit_Switch_Max(rc_dig_in15, &pwm12);
Limit_Switch_Min(rc_dig_in16, &pwm12);
/*---------- ROBOT FEEDBACK LEDs------------------------------------------------
*------------------------------------------------------------------------------
* This section drives the "ROBOT FEEDBACK" lights on the Operator Interface.
* The lights are green for joystick forward and red for joystick reverse.
* Both red and green are on when the joystick is centered. Use the
* trim tabs on the joystick to adjust the center.
* These may be changed for any use that the user desires.
*/
if (user_display_mode == 0) /* User Mode is Off */
{ /* Check position of Port 1 Joystick */
if (p1_y >= 0 && p1_y <= 56)
{ /* Joystick is in full reverse position */
Pwm1_green = 0; /* Turn PWM1 green LED - OFF */
Pwm1_red = 1; /* Turn PWM1 red LED - ON */
}
else if (p1_y >= 125 && p1_y <= 129)
{ /* Joystick is in neutral position */
Pwm1_green = 1; /* Turn PWM1 green LED - ON */
Pwm1_red = 1; /* Turn PWM1 red LED - ON */
}
else if (p1_y >= 216 && p1_y <= 255)
{ /* Joystick is in full forward position*/
Pwm1_green = 1; /* Turn PWM1 green LED - ON */
Pwm1_red = 0; /* Turn PWM1 red LED - OFF */
}
else
{ /* In either forward or reverse position */
Pwm1_green = 0; /* Turn PWM1 green LED - OFF */
Pwm1_red = 0; /* Turn PWM1 red LED - OFF */
} /*END Check position of Port 1 Joystick
/* Check position of Port 2 Y Joystick
(or Port 1 X in Single Joystick Drive Mode) */
if (p2_y >= 0 && p2_y <= 56)
{ /* Joystick is in full reverse position */
Pwm2_green = 0; /* Turn pwm2 green LED - OFF */
Pwm2_red = 1; /* Turn pwm2 red LED - ON */
}
else if (p2_y >= 125 && p2_y <= 129)
{ /* Joystick is in neutral position */
Pwm2_green = 1; /* Turn PWM2 green LED - ON */
Pwm2_red = 1; /* Turn PWM2 red LED - ON */
}
else if (p2_y >= 216 && p2_y <= 255)
{ /* Joystick is in full forward position */
Pwm2_green = 1; /* Turn PWM2 green LED - ON */
Pwm2_red = 0; /* Turn PWM2 red LED - OFF */
}
else
{ /* In either forward or reverse position */
Pwm2_green = 0; /* Turn PWM2 green LED - OFF */
Pwm2_red = 0; /* Turn PWM2 red LED - OFF */
} /* END Check position of Port 2 Joystick */
/* This drives the Relay 1 and Relay 2 "Robot Feedback" lights on the OI. */
Relay1_green = relay1_fwd; /* LED is ON when Relay 1 is FWD */
Relay1_red = relay1_rev; /* LED is ON when Relay 1 is REV */
Relay2_green = relay2_fwd; /* LED is ON when Relay 2 is FWD */
Relay2_red = relay2_rev; /* LED is ON when Relay 2 is REV */
Switch1_LED = !(int)rc_dig_in01;
Switch2_LED = !(int)rc_dig_in02;
Switch3_LED = !(int)rc_dig_in03;
} /* (user_display_mode = 0) (User Mode is Off) */
else /* User Mode is On - displays data in OI 4-digit display*/
{
User_Mode_byte = backup_voltage*10; /* so that decimal doesn't get truncated. */
}
} /* END Default_Routine(); */
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
Last edited by ImmortalAres : 02-06-2005 at 14:02. |
|
#6
|
||||
|
||||
|
Re: Accelerometer code
Quote:
-Kevin |
|
#7
|
||||
|
||||
|
Re: Accelerometer code
Quote:
-Kevin |
|
#8
|
|||
|
|||
|
Re: Accelerometer code
that is a very good question, apparently i deleted the call by accident once. i'll see if it helps any. Thanks.
~Will |
|
#9
|
|||
|
|||
|
Re: Accelerometer code
I could be way off base, but I believe the accelerometer you are using outputs analog signals (0-5 V) and should be connect to an AI port, not a digital.
Mike |
|
#10
|
||||
|
||||
|
Re: Accelerometer code
Quote:
Interrupt code here - http://www.kevin.org/frc Interrupt white paper - http://www.chiefdelphi.com/forums/pa...le&paperid=272 And you can run a search on here to see what you find. |
|
#11
|
||||
|
||||
|
Re: Accelerometer code
Quote:
edit: Actually, in hindsight, I do remember the 202's manual telling a way that the analog signal could be salvaged. I think it involved the use of a few capacitors. You can maybe read the manual, and see if that is a feasible option for you. Also, the accelerometer I was mentioning is the 203: http://www.analog.com/en/prod/0%2C28...L203%2C00.html . It has about 2.5 times less error, and a more usable signal. Its the way to go. Last edited by Joel J : 02-06-2005 at 14:34. Reason: after thought.. |
|
#12
|
||||
|
||||
|
Re: Accelerometer code
Quote:
-Kevin |
|
#13
|
|||
|
|||
|
Re: Accelerometer code
Joel,
My apologies, your right. I've used the analog one in the past and thought this was the same. Mike |
|
#14
|
|||
|
|||
|
Re: Accelerometer code
Quote:
also: http://people.clarkson.edu/~lanahamc/media/IMG_1546.jpg i don't know what RC that is, maybe joel can help me out on that one Last edited by ImmortalAres : 02-06-2005 at 14:39. |
|
#15
|
|||
|
|||
|
Re: Accelerometer code
Code:
/*******************************************************************************
* FILE NAME: user_routines.c <FRC VERSION>
*
* DESCRIPTION:
* This file contains the default mappings of inputs
* (like switches, joysticks, and buttons) to outputs on the RC.
*
* USAGE:
* You can either modify this file to fit your needs, or remove it from your
* project and replace it with a modified copy.
*
*******************************************************************************/
#include <stdio.h>
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "user_Serialdrv.h"
#include "myaccelerometer.h"
/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/
extern unsigned char aBreakerWasTripped;
/*******************************************************************************
* FUNCTION NAME: Limit_Switch_Max
* PURPOSE: Sets a PWM value to neutral (127) if it exceeds 127 and the
* limit switch is on.
* CALLED FROM: this file
* ARGUMENTS:
* Argument Type IO Description
* -------- ------------- -- -----------
* switch_state unsigned char I limit switch state
* *input_value pointer O points to PWM byte value to be limited
* RETURNS: void
*******************************************************************************/
void Limit_Switch_Max(unsigned char switch_state, unsigned char *input_value)
{
if (switch_state == CLOSED)
{
if(*input_value > 127)
*input_value = 127;
}
}
/*******************************************************************************
* FUNCTION NAME: Limit_Switch_Min
* PURPOSE: Sets a PWM value to neutral (127) if it's less than 127 and the
* limit switch is on.
* CALLED FROM: this file
* ARGUMENTS:
* Argument Type IO Description
* -------- ------------- -- -----------
* switch_state unsigned char I limit switch state
* *input_value pointer O points to PWM byte value to be limited
* RETURNS: void
*******************************************************************************/
void Limit_Switch_Min(unsigned char switch_state, unsigned char *input_value)
{
if (switch_state == CLOSED)
{
if(*input_value < 127)
*input_value = 127;
}
}
/*******************************************************************************
* FUNCTION NAME: Limit_Mix
* PURPOSE: Limits the mixed value for one joystick drive.
* CALLED FROM: Default_Routine, this file
* ARGUMENTS:
* Argument Type IO Description
* -------- ---- -- -----------
* intermediate_value int I
* RETURNS: unsigned char
*******************************************************************************/
unsigned char Limit_Mix (int intermediate_value)
{
static int limited_value;
if (intermediate_value < 2000)
{
limited_value = 2000;
}
else if (intermediate_value > 2254)
{
limited_value = 2254;
}
else
{
limited_value = intermediate_value;
}
return (unsigned char) (limited_value - 2000);
}
/*******************************************************************************
* FUNCTION NAME: User_Initialization
* PURPOSE: This routine is called first (and only once) in the Main function.
* You may modify and add to this function.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void User_Initialization (void)
{
Set_Number_of_Analog_Channels(SIXTEEN_ANALOG); /* DO NOT CHANGE! */
/* FIRST: Set up the I/O pins you want to use as digital INPUTS. */
digital_io_01 = digital_io_02 = digital_io_03 = digital_io_04 = INPUT;
digital_io_05 = digital_io_06 = digital_io_07 = digital_io_08 = INPUT;
digital_io_09 = digital_io_10 = digital_io_11 = digital_io_12 = INPUT;
digital_io_13 = digital_io_14 = digital_io_15 = digital_io_16 = INPUT;
digital_io_18 = INPUT; /* Used for pneumatic pressure switch. */
/*
Note: digital_io_01 = digital_io_02 = ... digital_io_04 = INPUT;
is the same as the following:
digital_io_01 = INPUT;
digital_io_02 = INPUT;
...
digital_io_04 = INPUT;
*/
/* SECOND: Set up the I/O pins you want to use as digital OUTPUTS. */
digital_io_17 = OUTPUT; /* Example - Not used in Default Code. */
/* THIRD: Initialize the values on the digital outputs. */
rc_dig_out17 = 0;
/* FOURTH: Set your initial PWM values. Neutral is 127. */
pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
/* FIFTH: Set your PWM output types for PWM OUTPUTS 13-16.
/* Choose from these parameters for PWM 13-16 respectively: */
/* IFI_PWM - Standard IFI PWM output generated with Generate_Pwms(...) */
/* USER_CCP - User can use PWM pin as digital I/O or CCP pin. */
Setup_PWM_Output_Type(IFI_PWM,IFI_PWM,IFI_PWM,IFI_PWM);
/*
Example: The following would generate a 40KHz PWM with a 50% duty cycle on the CCP2 pin:
CCP2CON = 0x3C;
PR2 = 0xF9;
CCPR2L = 0x7F;
T2CON = 0;
T2CONbits.TMR2ON = 1;
Setup_PWM_Output_Type(USER_CCP,IFI_PWM,IFI_PWM,IFI_PWM);
*/
/* Add any other initialization code here. */
Putdata(&txdata); /* DO NOT CHANGE! */
Serial_Driver_Initialize();
Initialize_Serial_Comms();
Initialize_Accelerometer();
//printf("IFI 2005 User Processor Initialized ...\r"); /* Optional - Print initialization message. */
/* Note: use a '\r' rather than a '\n' with the new compiler (v2.4) */
User_Proc_Is_Ready(); /* DO NOT CHANGE! - last line of User_Initialization */
}
/*******************************************************************************
* FUNCTION NAME: Process_Data_From_Master_uP
* PURPOSE: Executes every 26.2ms when it gets new data from the master
* microprocessor.
* CALLED FROM: main.c
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void Process_Data_From_Master_uP(void)
{
static unsigned char i;
static unsigned char delay;
int xaccel;
int yaccel;
Getdata(&rxdata); /* Get fresh data from the master microprocessor. */
Default_Routine(); /* Optional. See below. */
Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
/* Eample code to check if a breaker was ever tripped. */
if (aBreakerWasTripped)
{
for (i=1;i<29;i++)
{
if (Breaker_Tripped(i))
User_Byte1 = i; /* Update the last breaker tripped on User_Byte1 (to demonstrate the use of a user byte)
// Normally, you do something else if a breaker got tripped (ex: limit a PWM output) */
}
}
///////////////////////////////////////////////////////////////////
//get x and y accel and print them to the serial port
xaccel=Get_xAcceleration();
printf("The x accel: %d", xaccel);
//////////////////////////////////////////////////////////////
yaccel=Get_yAcceleration();
printf("The y accel: %d", yaccel);
//////////////////////////////////////////////////////////////////
Putdata(&txdata); /* DO NOT CHANGE! */
}
/*******************************************************************************
* FUNCTION NAME: Default_Routine
* PURPOSE: Performs the default mappings of inputs to outputs for the
* Robot Controller.
* CALLED FROM: this file, Process_Data_From_Master_uP routine
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void Default_Routine(void)
{
/*---------- Analog Inputs (Joysticks) to PWM Outputs-----------------------
*--------------------------------------------------------------------------
* This maps the joystick axes to specific PWM outputs.
*/
pwm01 = p1_y;
pwm02 = p2_y;
pwm03 = p3_y;
pwm04 = p4_y;
pwm05 = p1_x;
pwm06 = p2_x;
pwm07 = p3_x;
pwm08 = p4_x;
pwm09 = p1_wheel;
pwm10 = p2_wheel;
pwm11 = p3_wheel;
pwm12 = p4_wheel;
/*---------- 1 Joystick Drive ----------------------------------------------
*--------------------------------------------------------------------------
* This code mixes the Y and X axis on Port 1 to allow one joystick drive.
* Joystick forward = Robot forward
* Joystick backward = Robot backward
* Joystick right = Robot rotates right
* Joystick left = Robot rotates left
* Connect the right drive motors to PWM13 and/or PWM14 on the RC.
* Connect the left drive motors to PWM15 and/or PWM16 on the RC.
*/
/*---------- Buttons to Relays----------------------------------------------
*--------------------------------------------------------------------------
* This default code maps the joystick buttons to specific relay outputs.
* Relays 1 and 2 use limit switches to stop the movement in one direction.
* The & used below is the C symbol for AND
*/
relay1_fwd = p1_sw_trig & rc_dig_in01; /* FWD only if switch1 is not closed. */
relay1_rev = p1_sw_top & rc_dig_in02; /* REV only if switch2 is not closed. */
relay2_fwd = p2_sw_trig & rc_dig_in03; /* FWD only if switch3 is not closed. */
relay2_rev = p2_sw_top & rc_dig_in04; /* REV only if switch4 is not closed. */
relay3_fwd = p3_sw_trig;
relay3_rev = p3_sw_top;
relay4_fwd = p4_sw_trig;
relay4_rev = p4_sw_top;
relay5_fwd = p1_sw_aux1;
relay5_rev = p1_sw_aux2;
relay6_fwd = p3_sw_aux1;
relay6_rev = p3_sw_aux2;
relay7_fwd = p4_sw_aux1;
relay7_rev = p4_sw_aux2;
relay8_fwd = !rc_dig_in18; /* Power pump only if pressure switch is off. */
relay8_rev = 0;
/*---------- PWM outputs Limited by Limit Switches ------------------------*/
Limit_Switch_Max(rc_dig_in05, &pwm03);
Limit_Switch_Min(rc_dig_in06, &pwm03);
Limit_Switch_Max(rc_dig_in07, &pwm04);
Limit_Switch_Min(rc_dig_in08, &pwm04);
Limit_Switch_Max(rc_dig_in09, &pwm09);
Limit_Switch_Min(rc_dig_in10, &pwm09);
Limit_Switch_Max(rc_dig_in11, &pwm10);
Limit_Switch_Min(rc_dig_in12, &pwm10);
Limit_Switch_Max(rc_dig_in13, &pwm11);
Limit_Switch_Min(rc_dig_in14, &pwm11);
Limit_Switch_Max(rc_dig_in15, &pwm12);
Limit_Switch_Min(rc_dig_in16, &pwm12);
/*---------- ROBOT FEEDBACK LEDs------------------------------------------------
*------------------------------------------------------------------------------
* This section drives the "ROBOT FEEDBACK" lights on the Operator Interface.
* The lights are green for joystick forward and red for joystick reverse.
* Both red and green are on when the joystick is centered. Use the
* trim tabs on the joystick to adjust the center.
* These may be changed for any use that the user desires.
*/
if (user_display_mode == 0) /* User Mode is Off */
{ /* Check position of Port 1 Joystick */
if (p1_y >= 0 && p1_y <= 56)
{ /* Joystick is in full reverse position */
Pwm1_green = 0; /* Turn PWM1 green LED - OFF */
Pwm1_red = 1; /* Turn PWM1 red LED - ON */
}
else if (p1_y >= 125 && p1_y <= 129)
{ /* Joystick is in neutral position */
Pwm1_green = 1; /* Turn PWM1 green LED - ON */
Pwm1_red = 1; /* Turn PWM1 red LED - ON */
}
else if (p1_y >= 216 && p1_y <= 255)
{ /* Joystick is in full forward position*/
Pwm1_green = 1; /* Turn PWM1 green LED - ON */
Pwm1_red = 0; /* Turn PWM1 red LED - OFF */
}
else
{ /* In either forward or reverse position */
Pwm1_green = 0; /* Turn PWM1 green LED - OFF */
Pwm1_red = 0; /* Turn PWM1 red LED - OFF */
} /*END Check position of Port 1 Joystick
/* Check position of Port 2 Y Joystick
(or Port 1 X in Single Joystick Drive Mode) */
if (p2_y >= 0 && p2_y <= 56)
{ /* Joystick is in full reverse position */
Pwm2_green = 0; /* Turn pwm2 green LED - OFF */
Pwm2_red = 1; /* Turn pwm2 red LED - ON */
}
else if (p2_y >= 125 && p2_y <= 129)
{ /* Joystick is in neutral position */
Pwm2_green = 1; /* Turn PWM2 green LED - ON */
Pwm2_red = 1; /* Turn PWM2 red LED - ON */
}
else if (p2_y >= 216 && p2_y <= 255)
{ /* Joystick is in full forward position */
Pwm2_green = 1; /* Turn PWM2 green LED - ON */
Pwm2_red = 0; /* Turn PWM2 red LED - OFF */
}
else
{ /* In either forward or reverse position */
Pwm2_green = 0; /* Turn PWM2 green LED - OFF */
Pwm2_red = 0; /* Turn PWM2 red LED - OFF */
} /* END Check position of Port 2 Joystick */
/* This drives the Relay 1 and Relay 2 "Robot Feedback" lights on the OI. */
Relay1_green = relay1_fwd; /* LED is ON when Relay 1 is FWD */
Relay1_red = relay1_rev; /* LED is ON when Relay 1 is REV */
Relay2_green = relay2_fwd; /* LED is ON when Relay 2 is FWD */
Relay2_red = relay2_rev; /* LED is ON when Relay 2 is REV */
Switch1_LED = !(int)rc_dig_in01;
Switch2_LED = !(int)rc_dig_in02;
Switch3_LED = !(int)rc_dig_in03;
} /* (user_display_mode = 0) (User Mode is Off) */
else /* User Mode is On - displays data in OI 4-digit display*/
{
User_Mode_byte = backup_voltage*10; /* so that decimal doesn't get truncated. */
}
} /* END Default_Routine(); */
/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
i added the initialization of the acceleromter. so now i'm left with the solid red led telling me i have a code error ![]() |
![]() |
| Thread Tools | |
| Display Modes | Rate This Thread |
|
|
Similar Threads
|
||||
| Thread | Thread Starter | Forum | Replies | Last Post |
| Out of the Box Camera Code | russell | Programming | 9 | 21-10-2009 05:28 |
| Some things i'm trying to understand on Kevin's Accelerometer Code | Squall | Programming | 2 | 03-02-2005 12:01 |
| Team THRUST - Kevin's Code and Camera Code Combine | Chris_Elston | Programming | 3 | 31-01-2005 22:28 |
| Example accelerometer code released. | Kevin Watson | Programming | 0 | 20-01-2005 03:45 |
| heres the code. y this not working | omega | Programming | 16 | 31-03-2004 15:18 |