servo.c

RTX是当前非常流行的RTX实时操作系统

/*****************************************************************************/
/*                                                                           */
/*    File: servo.c  - Functions for RC servo control                        */
/*                                                                           */
/*    Routines:                                                              */
/*         init_servo      - Sets up ports and timers                        */
/*                                                                           */
/*****************************************************************************/

/***********************************************************************
                            I M P O R T S
***********************************************************************/
#include "windows.h"
#include "rtapi.h"
#include "stdio.h"
#include "math.h"
#include "int32pci.h"		/* Declarations for Int32 timer - I/O bd */
#include "servo.h"			/* exported declarations for this module */
/**********************************************************************/


/***********************************************************************
             F O R W A R D    D E C L A R A T I O N S
***********************************************************************/
/***********************************************************************
            P R I V A T E    D A T A    V A R I A B L E S
***********************************************************************/ 

/***********************************************************************
                    E N T R Y    R O U T I N E S
***********************************************************************/

/*****************************************************************************/
/*                                                                           */
/*   INIT_SERVO                                                               */
/*      Configure timer channel for servo control                            */
/*                                                                           */
/*****************************************************************************/

void init_servo(int chip,int timer) {

	int tmp;
    
	// Make sure Timer Output Bit is set up as an Output 
	if (timer == 1) {
		tmp = Int32_Read_Register(chip,PORT_B_DATA_DIR);
		Int32_Write_Register(chip,PORT_B_DATA_DIR,tmp & ~BIT4);
	} else if (timer == 2) {
		tmp = Int32_Read_Register(chip,PORT_B_DATA_DIR);
		Int32_Write_Register(chip,PORT_B_DATA_DIR,tmp & ~BIT0);
	} else if (timer == 3) {
		tmp = Int32_Read_Register(chip,PORT_C_DATA_DIR);
		Int32_Write_Register(chip,PORT_C_DATA_DIR,tmp & ~BIT0);
	}

	// Set up timer to have Output Pin enabled, in One-shot mode and in Single Cycle
	if (timer == 1) {
		Int32_Write_Register(chip,CNT_1_MODE_SPEC,CNT_MODE_ONE | CNT_MODE_EOE);

		tmp = Int32_Read_Register(chip,MASTER_CONF_CNTR);
		Int32_Write_Register(chip,MASTER_CONF_CNTR,tmp | PORT_B_EN_BIT );

	} else if (timer == 2) {
		Int32_Write_Register(chip,CNT_2_MODE_SPEC,CNT_MODE_ONE | CNT_MODE_EOE);

		tmp = Int32_Read_Register(chip,MASTER_CONF_CNTR);
		Int32_Write_Register(chip,MASTER_CONF_CNTR,tmp | PORT_B_EN_BIT );

	} else if (timer == 3) {
		Int32_Write_Register(chip,CNT_3_MODE_SPEC,CNT_MODE_ONE | CNT_MODE_EOE);
	}

}

void command_servo(int chip,int timer,float period) {
	
	union countload
    {
     char a[2];
     int b;
    }cntld;
	int tmp;
	float ts;

	if (period < 0.001F) ts = 0.001F;
	else if (period > 0.002F) ts = 0.002F;
	else ts = period;

	if (timer == 1) {
		// To Gate off I don't think I need the read
//		tmp = Int32_Read_Register(chip,CNT_1_COM_STA); 
//		Int32_Write_Register(chip,CNT_1_COM_STA,tmp & ~GATE_CNT);

		// Gate off Timer
		Int32_Write_Register(chip,CNT_1_COM_STA,0x0);

		// Clear Timer's IP (Each TC causes an interrupt even if we are not using it
		tmp = Int32_Read_Register(chip,CNT_1_COM_STA);
//		Int32_Write_Register(chip,CNT_1_COM_STA, (tmp && COM_STA_MASK) | CLEAR_IP);
		Int32_Write_Register(chip,CNT_1_COM_STA, CLEAR_IP);

		// Note that 2.5 MHz is the clock frequency of the Xilog chip
		// We want to convert the timer period into number of clocks that the
		// Xilog chip needs to count down.
		cntld.b = (USHORT) (ts*2500000.0F);  /* load the timer's period */
		Int32_Write_Register(chip,CNT_1_TIME_CNST_MSB,cntld.a[1]);
		Int32_Write_Register(chip,CNT_1_TIME_CNST_LSB,cntld.a[0]);

		// enable Timer
		tmp = Int32_Read_Register(chip,MASTER_CONF_CNTR);
		Int32_Write_Register(chip,MASTER_CONF_CNTR,tmp | CNT_1_EN_BIT );

		// Gate and Trigger ON Timer
		Int32_Write_Register(chip,CNT_1_COM_STA,GATE_CNT | TRIG_CNT);

	} else if (timer == 2) {
		// To Gate off I don't think I need the read
//		tmp = Int32_Read_Register(chip,CNT_2_COM_STA); 
//		Int32_Write_Register(chip,CNT_2_COM_STA,tmp & ~GATE_CNT);

		// Gate off Timer
		Int32_Write_Register(chip,CNT_2_COM_STA,0x0);

		// Clear Timer's IP (Each TC causes an interrupt even if we are not using it
		tmp = Int32_Read_Register(chip,CNT_2_COM_STA);
//		Int32_Write_Register(chip,CNT_2_COM_STA, (tmp && COM_STA_MASK) | CLEAR_IP);
		Int32_Write_Register(chip,CNT_2_COM_STA, CLEAR_IP);

		cntld.b = (USHORT) (ts*2500000.0F);  /* load the timer's period */
		Int32_Write_Register(chip,CNT_2_TIME_CNST_MSB,cntld.a[1]);
		Int32_Write_Register(chip,CNT_2_TIME_CNST_LSB,cntld.a[0]);

		// enable Timer
		tmp = Int32_Read_Register(chip,MASTER_CONF_CNTR);
		Int32_Write_Register(chip,MASTER_CONF_CNTR,tmp | CNT_2_EN_BIT );

		// Gate and Trigger ON Timer
		Int32_Write_Register(chip,CNT_2_COM_STA,GATE_CNT | TRIG_CNT);
		
	} else if (timer == 3) {
		// To Gate off I don't think I need the read
//		tmp = Int32_Read_Register(chip,CNT_3_COM_STA); 
//		Int32_Write_Register(chip,CNT_3_COM_STA,tmp & ~GATE_CNT);

		// Gate off Timer
		Int32_Write_Register(chip,CNT_3_COM_STA,0x0);

		// Clear Timer's IP (Each TC causes an interrupt even if we are not using it
		tmp = Int32_Read_Register(chip,CNT_3_COM_STA);
//		Int32_Write_Register(chip,CNT_3_COM_STA, (tmp && COM_STA_MASK) | CLEAR_IP);
		Int32_Write_Register(chip,CNT_3_COM_STA, CLEAR_IP);

		cntld.b = (USHORT) (ts*2500000.0F);  /* load the timer's period */
		Int32_Write_Register(chip,CNT_3_TIME_CNST_MSB,cntld.a[1]);
		Int32_Write_Register(chip,CNT_3_TIME_CNST_LSB,cntld.a[0]);

		// enable Timer
		tmp = Int32_Read_Register(chip,MASTER_CONF_CNTR);
		Int32_Write_Register(chip,MASTER_CONF_CNTR,tmp | PORT_C_CNT_3_EN_BIT );

		// Gate and Trigger ON Timer
		Int32_Write_Register(chip,CNT_3_COM_STA,GATE_CNT | TRIG_CNT);
		
	}

}