Chief Delphi

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)
-   Programming (http://www.chiefdelphi.com/forums/forumdisplay.php?f=51)
-   -   Accelerometer code (http://www.chiefdelphi.com/forums/showthread.php?t=38442)

ImmortalAres 02-06-2005 10:59
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.

Ryan M. 02-06-2005 13:27
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:

Kevin Watson 02-06-2005 13:33
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

Joel J 02-06-2005 13:34
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
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.

ImmortalAres 02-06-2005 13:54
Re: Accelerometer code
 
3 Attachment(s)
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(); */

/******************************************************************************/
/******************************************************************************/
/******************************************************************************/

Kevin Watson 02-06-2005 14:04
Re: Accelerometer code
 
Quote:
Originally Posted by Joel J.
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.
You find it wierd that two code monkey demi-gods would end up writing the same code? Hmmm... A better understanding of the Tao of Programming is needed here, grasshopper <grin>.

-Kevin

Kevin Watson 02-06-2005 14:10
Re: Accelerometer code
 
Quote:
Originally Posted by ImmortalAres
ok, based on kevin's response i'm clearly missing something with the interrupts being called. here's the code...
Where does Initialize_Accelerometer() get called?

-Kevin

ImmortalAres 02-06-2005 14:18
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

Mike Bortfeldt 02-06-2005 14:19
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

Joel J 02-06-2005 14:20
Re: Accelerometer code
 
Quote:
Originally Posted by ImmortalAres
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
About interrupts:
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.

Joel J 02-06-2005 14:22
Re: Accelerometer code
 
Quote:
Originally Posted by Mike Bortfeldt
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
The 202 outputs PWM signals.. I, next fall, was going to use the newer accelerometer that outputs analog signals, as the resulting output would be more useful, because of this particular processor (I didn't realize this person is at Clarkson, heh). So maybe you should just go right to investigating that one? Also, you can look at the setups from http://www.sparkfun.com/shop/ , they could make your life a bit easier (unlike my headache this build).

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.

Kevin Watson 02-06-2005 14:25
Re: Accelerometer code
 
Quote:
Originally Posted by ImmortalAres
that is a very good question, apparently i deleted the call by accident once. i'll see if it helps any. Thanks.

~Will
Are you using the EDU-RC? I ask because I have code that will work with your accelerometer, but is dependent on the EDU-RC.

-Kevin

Mike Bortfeldt 02-06-2005 14:25
Re: Accelerometer code
 
Joel,

My apologies, your right. I've used the analog one in the past and thought this was the same.

Mike

ImmortalAres 02-06-2005 14:35
Re: Accelerometer code
 
Quote:
Originally Posted by Joel J.
TActually, 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.[/i]
yes, you can build an op amp to get the analog signal back but our research advisor (Carroll) wants us to just use another PIC that will solve the problem of processing power.

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

ImmortalAres 02-06-2005 14:45
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 :)

CapnBFG 02-06-2005 14:51
Re: Accelerometer code
 
Quote:
Originally Posted by Joel J.
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.
Joel, I am working with ImmortalAres on this project; I'm handling all hardware aspects. Do you already have the model 203 accelerometer or were you just planning on buying one in the future? The reason I ask is because when we organized the parts cabinet we found three accelerometers in addition to the one we are using, and I have yet to check what models each of them are.

Joel J 02-06-2005 14:53
Re: Accelerometer code
 
Quote:
Originally Posted by ImmortalAres
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 :)
Well.. thats the main problem with the accelerometer running on a PWM.. the timer to get even decent results is too much for the proc (and it looks like your timer is set to 1MHz, which is just not gonna happen -- 12K was just about too much). I'd say just get the analog version shipped in, and go from there. Still stick with the off-board processor setup, though. Did he say if you could use a DSP?

Joel J 02-06-2005 14:54
Re: Accelerometer code
 
Quote:
Originally Posted by CapnBFG
Joel, I am working with ImmortalAres on this project; I'm handling all hardware aspects. Do you already have the model 203 accelerometer or were you just planning on buying one in the future? The reason I ask is because when we organized the parts cabinet we found three accelerometers in addition to the one we are using, and I have yet to check what models each of them are.
I have no idea what we have. But I do know that we don't have the 203, because its a newer model (came out in the middle of the season). If you are the hardware guy, then you can probably look at the new interfaces they have to the accelerometers, and see if you can work with them better (they have a couple self-contained units, based on the 203 -- http://www.analog.com/en/subCat/0,28...0%255F,00.html , at the very bottom of the page).

ImmortalAres 02-06-2005 14:57
Re: Accelerometer code
 
yea, it looks like i'm gonna try to push Carroll to get us the 203 one. For whatever reason he was adamant about using a PWM and not getting something to convert it. hopefully he goes for getting a new accelerometer entirely.

also,

http://microchip.com/stellent/idcplg...&part=DM163022

that's what we plan on using for the 2nd PIC

Mike Bortfeldt 02-06-2005 15:09
Re: Accelerometer code
 
One thing I don't see in your code is where you reset the Port B interrupt flag (INTCONBits.RBIF), but that could easily be in your InterruptHandlerLow routine. If possible could you also post your user_routines_fast.c code as it might help.
Another thing you will probably encounter later on is when you calculate your yacceleration you are dividing a 16 bit integer Int_4_Period (maximum 65535) by 206124 which will always result in 0 as the PIC will be performing integer math.

ImmortalAres 02-06-2005 15:13
Re: Accelerometer code
 
is the PIC capable of doing floating point math at all?
if not i really should just give up on this pwm accelerometer and get the 203 joel is talking about which is what i'm planning on doing now.

Thanks for all the help you guys. Its much appreciated

Joel J 02-06-2005 15:19
Re: Accelerometer code
 
Quote:
Originally Posted by ImmortalAres
is the PIC capable of doing floating point math at all?
if not i really should just give up on this pwm accelerometer and get the 203 joel is talking about which is what i'm planning on doing now.

Thanks for all the help you guys. Its much appreciated
It can, but its really a death sentence. If you want to use it (I don't recommend it--at all), then you'd write
Code:
float kVariableName; // Float variable with name kVariableName
, like always.

ImmortalAres 02-06-2005 15:47
Re: Accelerometer code
 
ok, would a 40 Mhz PIC do a better job at it or is it just a bad idea in general because then i need an analog signal (203 model) since if you look in the comments in the accel.c file i need float point math to convert pwm periods into anything useful

Joel J 02-06-2005 16:19
Re: Accelerometer code
 
Quote:
Originally Posted by ImmortalAres
ok, would a 40 Mhz PIC do a better job at it or is it just a bad idea in general because then i need an analog signal (203 model) since if you look in the comments in the accel.c file i need float point math to convert pwm periods into anything useful
the PWM accelerometer comes with a 14-bit A/D converter, whereas the PICS usually have a 10-bit converter. That was their claimed advantage, but quite frankly "PWM interfaces" are a real pain, and for that reason, Analog is phasing out those line of accelerometers. So you may as well be with the game.

If you are using the offboard processor, and it is at that speed, then you'd probably get the accuracy you need. In all implementations of this type of accelerometer, the interrupts become, for the most part, the main focus -- ie, they are set on high priority interrupts -- and not much else is going on. So, you decide.

Also, as far as floating point goes, I used a fixed-point setup (10 bits for the decimal) to get the accuracy I wanted, without having to resort to floats. For some reason, Dr. Carroll didn't like that (added complexity or some other claim), but its commonly used. Chris Hibner did a presentation on it recently ( http://www.usfirst.org/robotics/2005...rogramming.ppt ).

Oh, and here is a "relevant" example of said fixed-point setup in use:
Code:
#define RADIX_SHIFT_TIMES 10/* Shift a value this number of places. In other words or multiply (or divide) by 2^number_of_places */
#define RADIX_SHIFT_MULTIPLIES_BY 1024/* this is the effective multiplier/divider. it is 2^RADIX_SHIFT_TIMES */
Code:
void applyMotionControl(pDriveInfo stream)
{
long er = 0, el = 0, VID, WID;

//er = (2.0*Mrobot*R*Kd1*Vd-2.0*Mrobot*R*Kd1*V+2.0*Mrobot*R*Kp1*Vd_int-2.0*Mrobot*R*Kp1*V_int+2.0*Mrobot*R*Ki1*Vd_int_int-2.0*Mrobot*R*Ki1*V_int_int+Kg*Kg*Kt*Kb*wr*rout+rout*rout*b*Mrobot*R*Kp2*Wd_int-rout*rout*b*Mrobot*R*Kp2*W_int+rout*rout*b*Mrobot*R*Ki2*Wd_int_int-rout*rout*b*Mrobot*R*Ki2*W_int_int+rout*rout*b*Mrobot*R*Kd2*Wd-rout*rout*b*Mrobot*R*Kd2*W);
//el = (2.0*Mrobot*R*Kd1*Vd-2.0*Mrobot*R*Kd1*V+2.0*Mrobot*R*Kp1*Vd_int-2.0*Mrobot*R*Kp1*V_int+2.0*Mrobot*R*Ki1*Vd_int_int-2.0*Mrobot*R*Ki1*V_int_int-rout*rout*b*Mrobot*R*Kp2*Wd_int+rout*rout*b*Mrobot*R*Kp2*W_int-rout*rout*b*Mrobot*R*Ki2*Wd_int_int+rout*rout*b*Mrobot*R*Ki2*W_int_int-rout*rout*b*Mrobot*R*Kd2*Wd+rout*rout*b*Mrobot*R*Kd2*W+Kg*Kg*Kt*Kb*wl*rout);

if (stream->gear == HIGH)
{
VID = (Kd1_high*(stream->Vd - stream->V)) + (Kp1_high*(stream->Vd_int - stream->V_int)) + (Ki1_high*(stream->Vd_int_int - stream->V_int_int));
} else {
VID = (Kd1_low*(stream->Vd - stream->V)) + (Kp1_low*(stream->Vd_int - stream->V_int)) + (Ki1_low*(stream->Vd_int_int - stream->V_int_int));
}

VID = VID >> RADIX_SHIFT_TIMES; // Squared radix value, so divide by it, to restore original
VID = VID * DD_Mrobot_R_2;              // Squared radix value, but don't divide because of later

if (stream->gear == HIGH)
{
WID = (Kd2_high*(stream->Wd - stream->W)) + (Kp2_high*(stream->Wd_int - stream->W_int)) + (Ki2_high*(stream->Wd_int_int - stream->W_int_int));
} else {
WID = (Kd2_low*(stream->Wd - stream->W)) + (Kp2_low*(stream->Wd_int - stream->W_int)) + (Ki2_low*(stream->Wd_int_int - stream->W_int_int));
}

WID = WID >> RADIX_SHIFT_TIMES;// Squared radix value, so divide by it, to restore original
WID = WID * DD_rout_rout_b_Mrobot_R;// Squared radix value, but don't divide because of later

if (stream->gear == HIGH)
{
er = (VID + WID + (DD_Kg_Kg_Kt_Kb_rout_high*stream->wr)) / DD_rout_Kg_Kt_high;  // Added already squared radix value to newly squared radix value
el = (VID - WID + (DD_Kg_Kg_Kt_Kb_rout_high*stream->wl)) / DD_rout_Kg_Kt_high;  // No need to divide by radix to even out, because the already present division evens things out
} else {
er = (VID + WID + (DD_Kg_Kg_Kt_Kb_rout_low*stream->wr)) / DD_rout_Kg_Kt_low;  // Added already squared radix value to newly squared radix value
el = (VID - WID + (DD_Kg_Kg_Kt_Kb_rout_low*stream->wl)) / DD_rout_Kg_Kt_low;  // No need to divide by radix to even out, because the already present division evens things out
}

er = er >> RADIX_SHIFT_TIMES;// Since we are ready to use the value, get rid of multiplier
el = el >> RADIX_SHIFT_TIMES;// Since we are ready to use the value, get rid of multiplier

stream->rx = el;// TEMPORARY -- REMOVE!!!!!!
stream->ry = er;

// Do not integrate unless the outputs are below saturation
if (er > stream->max_output)
{
er = stream->max_output;
} else if (er < stream->min_output)
{
er = stream->min_output;
} else if (el > stream->max_output)
{
el = stream->max_output;
} else if (el < stream->min_output)
{
el = stream->min_output;
} else {
// Integrate values (remember to add check for saturation)
stream->Vd_int = safeAdd_long(stream->Vd_int, ((Ts * (stream->Vd + stream->Vd_prev)) >> (RADIX_SHIFT_TIMES + 1)));
  stream->Vd_int_int = safeAdd_long(stream->Vd_int_int, ((Ts * (stream->Vd_int + stream->Vd_int_prev)) >> (RADIX_SHIFT_TIMES + 1)));

    stream->Wd_int = safeAdd_long(stream->Wd_int, ((Ts * (stream->Wd + stream->Wd_prev)) >> (RADIX_SHIFT_TIMES + 1)));
  stream->Wd_int_int = safeAdd_long(stream->Wd_int_int, ((Ts * (stream->Wd_int + stream->Wd_int_prev)) >> (RADIX_SHIFT_TIMES + 1)));
}

// Store previous values (do this no matter what)
stream->Vd_prev = stream->Vd;
stream->Vd_int_prev = stream->Vd_int;

stream->Wd_prev = stream->Wd;
stream->Wd_int_prev = stream->Wd_int;

// Return value to user
stream->left_output = (unsigned char)limit_to_range(((signed int)el+(signed int)127), (signed int)MIN_JOY, (signed int)MAX_JOY);
stream->right_output = (unsigned char)limit_to_range(((signed int)er+(signed int)127), (signed int)MIN_JOY, (signed int)MAX_JOY);

// stream->left_output = p1_y;
// stream->right_output = p2_y;
}
Heh.

Kevin Watson 02-06-2005 16:51
Re: Accelerometer code
 
Okay, lets take a deep breath... Ready? Okay, here's my thinking:

1) Stick with the PWM output because, as Joel pointed out, this device has got a higher resolution ADC on-chip and as all the analog signals stay on-chip, you'll be dealing with fewer noise sources.

2) If you're going to use an external microcontroller to handle the accelerometer interface, use the CCP capture hardware to do the work for you. This won't work on the full-size FRC robot controller because IFI has output buffers on the CCP pins, which precludes them being used as inputs (the smaller EDU robot controller does not have these buffers). I've already written code that will work with the device you're using that uses the CCP capture hardware. If you promise to send me a copy of your project write-up, I'll publish a link to it <grin>.

3) What resistor values did you choose to program the t2 frequency (If you don't know what I'm blathering about, you'd better read the data sheet <grin>). If you're using this accelerometer in a control application, anything higher than 50-100Hz is overkill. If you're integrating the output, you'll want to set it to at least twice the accelerometer's bandwidth (lest Harry Nyquist rise from his grave, and smite thy project), but ideally you'll want to set it as high as possible.

-Kevin (Who didn't get nearly enough sleep and has consumed way too much coffee)

Mike Bortfeldt 02-06-2005 17:07
Re: Accelerometer code
 
You could also do your fixed point math another way by calculating your result in units mg's rather than g's by multiplying the appropriate values by 1000. It is not as efficient as bit shifting since you actually have to use multiply and divide statements, but may be a bit more readable.
If you do stick with just the IFI controller, you can probably get reasonable accuracy as the code comments indicate a period of 9.6 milliseconds for the accelerometer. While a low priority interrupt can be delayed depending on what the process is doing, you should be able to maintain an error of less than 1% due to interrupt delay.

Mike

ImmortalAres 02-06-2005 17:31
Re: Accelerometer code
 
The resistor value was preset when the device made it into our hands at 1.2 Mega ohms making T2 somewhere in the range of 250 Hz if i remember correctly. The exact calculation is written in the comments i have in the code.

We have to write up a paper for the symposium at the end of the summer and i would be more than willing to send you a copy but as of now there is no formal write up

I'll see about sticking with the PWMs. I have to wait until the 2nd PIC gets in.

Thanks for the advice


~Will

Kevin Watson 02-06-2005 19:18
Re: Accelerometer code
 
Quote:
Originally Posted by ImmortalAres
We have to write up a paper for the symposium at the end of the summer and i would be more than willing to send you a copy but as of now there is no formal write up
Here is some code that'll get you started: http://kevin.org/frc/edu_accel_lcd.zip. It's designed to work with an external serial LCD, so you'll need to do some porting.

-Kevin

Gdeaver 04-06-2005 01:02
Re: Accelerometer code
 
So far the discussion has been on low range accelerometers. Remember, it's on a robot and there may be times when there is a impact or bump that will drive the unit to saturation. It can take a considerable amount of time to recover from a saturated condition. It's enough to affect the integration. Accelerometers are also excellent vibration sensors. One thing that is often over looked is the natural resonance frequency of the sensor. Both the chip package and the sensor have a resonate point that is high enough not to worry about but your going to have to mount the chip on a carrier. You have to worry about the resonance point of the chip-carrier-mounting system.
Glue your chip to the board. Use a thick high grade PC board. More ridged the better. Use some isolation mounting to mount the board to the bot and pay attention to what part of the robot you mount it to. Try to isolate it from the drive train vibration. I think you might have better results if you went to a mid range analog unit. Run the output through and op amp with a gain of one and a 400 HZ low pass filter. Mount the pic18, op amp and sensor on a small ridged carrier that is mounted to a ridge large mass part of the robot with some isolation mounting hardware. Over sample in software to further reduce noise. Let the pic coproc do the calculations and send a periodic update to the FRC .

The Freescale MMA3201D has a low pass fiter on the chip.


All times are GMT -5. The time now is 11:52.

Powered by vBulletin® Version 3.6.4
Copyright ©2000 - 2017, Jelsoft Enterprises Ltd.
Copyright © Chief Delphi