stepmotor_godknows.c

网上下载的一个rtx例子

/*****************************************************************************
***FILE:    stepmotor.C (Program the INT32 board to drive stepper motors   ***
******************************************************************************
****                                                                      ****
*****************************************************************************/
#include <windows.h>
#include <stdio.h>
#include <rtapi.h>
#include "cslrtx.h" // only needed for the define MAX_BUFF_LEN
#include "rtidrv.h"
#include "encodrbd.h"
#include "rtsscom.h"
#include "step.h"
#include "int32pci.h"
#include "servo.h"


// globals

//Servo Variables
volatile float g_servo1_periodnew;
volatile float g_servo1_period;
volatile float g_servo2_periodnew;
volatile float g_servo2_period;
volatile int g_NewGains = 0;
volatile int g_dontupdateflag = 0; //originally set to send data to VB

//Resistor Values
volatile float g_photo0 = 0;
volatile float g_photo1 = 0;

//Digital Inputs
volatile int g_digitalia = 0;

//Optical Encoder Values
volatile float g_optical0;	//y direction
volatile float g_optical1;	//x direction

volatile float g_senddata[20];

//--------------------------------------------
//Specifically for the AUTONOMOUS MODE
//--------------------------------------------

volatile float g_photo0_array[100];
volatile float g_photo1_array[100];
volatile float g_photo0_butter[100];
volatile float g_photo1_butter[100];

void			FunctionINIT();				//used to initialize arrays to zero

volatile float	y_total;
volatile float	r_new;
volatile float	turncount;					//counter--how many times timer handler is called
volatile double g_newheading;
volatile double g_heading;
volatile float	g_x;
volatile float	g_y;
volatile float	x_old;
volatile float	y_old;
volatile double counts;
volatile float	leftcount;					// number of timer calls for LiSAR to turn left
volatile float	rightcount;	

volatile int	turntime = 100;				//used to limit how many times the AUTO timer is used
volatile int	forwardtime = 200;			//used to limit how far it goes before it looks around
volatile int	checktime = 100;			//how long it looks left, then right
volatile int	countertime = 0;			//used to count up to either turn,forward, or checktime
volatile int	targettime = 300;			//amount of time to go forward before checking for target again

volatile long int g_time = 0;				//timer counter for saved data

volatile int	DoFlag = 0;					//used to skip or enter Mode switch case
volatile int	TargetFlag = 0;				//used to indicate when Target has been spotted
volatile int	TactileFlag = 0;			//tells LiSAR to complete tactile manuever
volatile int	FoundFlag = 0;				//tells us when target has been found

volatile int	mod_digit = 0;				//digitalia mod (10)
volatile float	percentlight = (float)0.10;	//percentage of ambient light, LiSAR uses to get agreement
volatile int	MODE = 0;					//Mode number used to control LiSAR
volatile float	delta = (float)0.0001;		//change in servo period, the larger, the faster it goes

//Make a butter worth filter, [B,A] = butter(6,40/500)
//#define			FILTER_ORDER 6
//volatile float g_b[FILTER_ORDER+1] = {2.497222526886417e-006F, 1.498333516131850e-005F, 3.745833790329625e-005F, 4.994445053772834e-005F, 3.745833790329625e-005F, 1.498333516131850e-005F, 2.497222526886417e-006};
//volatile float g_a[FILTER_ORDER+1] = {1.000000000000000e+000F, -5.029438351421609e+000F, 1.060704218377969e+001F, -1.199931581621670e+001F, 7.675474548200203e+000F, -2.631055128473950e+000F, 3.774523863740888e-001};

#define		FILTER_ORDER 3
volatile float g_b[FILTER_ORDER+1] = {0.0134F,0.0267F,0.0134F};
volatile float g_a[FILTER_ORDER+1] = {1.0000F,-1.6475F,0.7009F};

// declaration of timer handler (Actual Function at end of file)
int RTFCNDCL TmrManual(PVOID unused);
int RTFCNDCL TmrAuto(PVOID unused);

//-----------------------------------------------
// MAIN -- start the timer and wait 10 seconds.
//-----------------------------------------------
VOID main(int argc, char **argv) {

	LONG command;
	int quit_status = 0; // quit flag

	float tmpdata[20];				// temporary float buffer
	char tmpbuff[MAX_BUFF_LEN];		// temporary char buffer
	LARGE_INTEGER time;				// LARGE_INTEGER in a 64bit integer (See VC++ ON-Line Help)

    HANDLE	hTmrManual;
	HANDLE	hTmrAuto;
	int tmr_status_manual = 0;		// timer on manual flag
	int tmr_status_auto = 0;		// timer on auto flag
	int i = 0;

	//------------------------------------
    // Setup and start MANUAL timer
    //------------------------------------
    if (!(hTmrManual = RtCreateTimer(
	NULL,                   // Security - NULL is none
	0,                      // Stack size - 0 is use default
	TmrManual,				// Timer handler
	NULL,                   // NULL context (argument to handler)
	RT_PRIORITY_MAX,        // Priority
	CLOCK_FASTEST)))        // Always use fastest available clock
    {
	printf("ERROR: Could not create the RTX timer.\n");
	ExitProcess( 1);
    }

	//------------------------------------
    // Setup and start AUTONOMOUS timer
    //------------------------------------
    if (!(hTmrAuto = RtCreateTimer(
	NULL,                   // Security - NULL is none
	0,                      // Stack size - 0 is use default
	TmrAuto,				// Timer handler
	NULL,                   // NULL context (argument to handler)
	RT_PRIORITY_MAX,        // Priority
	CLOCK_FASTEST)))        // Always use fastest available clock
    {
	printf("ERROR: Could not create the RTX timer.\n");
	ExitProcess( 1);
    }

	// Initialize Communication with Win32 DLL
	if (InitRTSS_Communication() != 0) {
		printf("Error: Could not setup communication with DLL\n");
		ExitProcess(1);
	}

	// Int32_Initialize must be called before any other INT32 (int32.c) functions
	// It enables port I/O for the I/O space used by the INT32 board
	Int32_Initialize();
	Int32_Reset(1);
	Int32_Reset(2);

	// Initialize flag
	quit_status = 0;
	tmr_status_manual = 0;
	tmr_status_auto = 0;

	// Loop until told to quit
	while (quit_status == 0) {

		//  Continue to wait for a command from DLL
		//  Also every 20 milliseconds check to see if Timer Handler
		//  Has activated an event.  Presently this code does not use
		//  events but it can be easily added.  You may want to talk to
		//  TA on how to add events calls.  rtsscomm.c has all the needed
		//  functions to call events and you will have to modify your
		//  VB program to handle events.
		command = WaitforCommandandProcessEvents(20);

		switch( command ) {
			case 1: // Digital Output
				GetSentData(tmpdata,tmpbuff);
				dig_out((int)tmpdata[0]);
				break;
			case 2: // Digital Input
				tmpdata[0] = (float) dig_in();
				SetSendData(tmpdata);
				break;
			case 0xC: // Quit
				quit_status = 1;
				break;
			case 0x12: // init servos 
				if (tmr_status_manual == 0) {
					init_servo(1,3);
					init_servo(2,3);
				}
				break;
			case 0x13: // MANUAL
				//make sure autonomous timer isn't on
				if(tmr_status_auto == 1)
				{
					RtCancelTimer(hTmrAuto, NULL);
					tmr_status_auto = 0;
				}

				GetSentData(tmpdata,tmpbuff);
				g_servo1_periodnew = tmpdata[0];
				g_servo2_periodnew = tmpdata[1];
				g_NewGains = 1;
				// Check that timer has not already been started
				if (tmr_status_manual == 0) {
					

					// Start timer with given sample Period
					// The period is loaded in units of 100ns
					// Note that the minumun period is .5 milliseconds for a
					// RTSS timer.  .1 ms is possible but some NT registry values
					// have to be changed.  Ask your TA.
					time.QuadPart = (LONGLONG) (0.010*10000000.0 + 0.5);
					if (!RtSetTimerRelative( hTmrManual, &time, &time)) {
						printf("ERROR: Could not set and start the RTAPI timer.\n");
						ExitProcess( 2);
					}
					// Flag that Start Command Active
					tmr_status_manual = 1;
				}

				break;
			case 0x14: // AUTONOMOUS
				//make sure manual timer isn't on
				if(tmr_status_manual == 1)
				{
					RtCancelTimer(hTmrManual, NULL);
					tmr_status_manual = 0;
				}

				FunctionINIT();	//Initialize Autonomous variables

				//Get Turn times that have been sent from VB
				GetSentData(tmpdata,tmpbuff);

				turntime = (int)tmpdata[0];
				forwardtime = (int)tmpdata[1];
				checktime = (int)tmpdata[2];
				percentlight = (float)tmpdata[3];
				delta = (float)tmpdata[4];
				targettime = (int)tmpdata[5];
				
				MODE = 10;	//Begin LiSAR to check surroundings
				DoFlag = 0;
				TactileFlag = 0;
				TargetFlag = 0;
				FoundFlag = 0;
				g_x = 0;
				g_y = 0;
				x_old = 0;
				y_old = 0;
				g_heading = 0;
				g_newheading = 0;
				r_new = 0;
				y_total = 0;

				// Initialize Encoder
				init_7166();

				// Initialize Save Data
				init_savedata();

				// Check that timer has notalready been started
				if (tmr_status_auto == 0) {
					

					// Start timer with given sample Period
					// The period is loaded in units of 100ns
					// Note that the minumun period is .5 milliseconds for a
					// RTSS timer.  .1 ms is possible but some NT registry values
					// have to be changed.  Ask your TA.
					time.QuadPart = (LONGLONG) (0.010*10000000.0 + 0.5);
					if (!RtSetTimerRelative( hTmrAuto, &time, &time)) {
						printf("ERROR: Could not set and start the RTAPI timer.\n");
						ExitProcess( 2);
					}
					// Flag that Start Command Active
					tmr_status_auto = 1;

					//Initialize timer counter
					g_time = 0;

				}

				break;
			case 0x15:	// cancel timer, but don't quit
				if(tmr_status_manual == 1)
				{
					RtCancelTimer(hTmrManual, NULL);
					tmr_status_manual = 0;
				}

				if(tmr_status_auto == 1)
				{
					RtCancelTimer(hTmrAuto, NULL);
					tmr_status_auto = 0;
				}
				break;
			case 0x405: // Send stored data up to Win32 DLL and then to VB program
				// Flag telling the timer handler not to update the g_senddata
				// array until the entire array has been loaded into tmpdata
				g_dontupdateflag = 1;

				for(i = 0; i < 20; i++)
				{
					tmpdata[i] = g_senddata[i];
				}
				// flag timer handler that it is again ok to update
				// g_senddata
				g_dontupdateflag = 0;

				// Send data to Server DLL
				SetSendData(tmpdata);

				break;
			case 0x406: // Save Data Command
				// Get Filename to save data to
				GetSentData(tmpdata,tmpbuff);
				// Only perform the save to file if we are in Halt mode
				if((tmr_status_auto == 0)&&(tmr_status_manual == 0))
				{
					// Save data saved with the 'savedata' function to file tmpbuff
					if (savedatafile(tmpbuff) != 0) 
					{
						printf("Error saving data");
					}
				}
				break;
			default:
				break;
		}
	}

	//
	// Stop and delete the timer.
	//
    RtDeleteTimer(hTmrManual);
	RtDeleteTimer(hTmrAuto);
	// Close Communication Objects
	DeleteRTSS_Communication();

    ExitProcess( 0);
}

int RTFCNDCL TmrManual(PVOID unused) { // MANUAL SHMANUAL
	
	// Update Servo Period with new value when background task
	// flags that there are new gains
	if (g_NewGains == 1) {
		g_servo1_period = g_servo1_periodnew;
		g_servo2_period = g_servo2_periodnew;
		// Flag Newgains back to NO
		g_NewGains = 0;
	}

	command_servo(1,3,g_servo1_period);
	command_servo(2,3,g_servo2_period);
	
	//Get Voltage in from photoresistors
	g_photo0 = volt_in(0);	//Pin hole resistor
	g_photo1 = volt_in(1);	//Ambient Light Resistor

	//Get Digital Input
	g_digitalia = (int) dig_in();