stepmotor.c

RTX是当前非常流行的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 int g_optical0;	
volatile int g_optical1;	

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	g_map_path[1000][2];				//used to map path
volatile float	g_map_obst[1000][2];				//used to log obstacles?
volatile int	pathcount = 0;

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 float	gx = 0;						//global x position
volatile float	gy = 0;						//global y position
volatile float	gx_old = 0;					//global old x position
volatile float	gy_old = 0;					//global old y position
volatile int	path_curr = 0;				//total path length taken
volatile int	path_old = 0;				//previous path length taken
volatile double heading = 0;				//heading from Global +Y direction
volatile double phi = 0;					//difference to turn from gx and gy to origin

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	CheckFlag = 0;

volatile int	mod_digit = 0;				//digitalia mod (10)
volatile float	percentlight = (float)500;	//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

#define		THETA_SCALE 631

//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;
				}

				// Initialize Encoder
				init_7166();

				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;
				CheckFlag = 0;

				pathcount = 0;
				countertime = 0;

				gx = 0;
				gy = 0;
				gx_old = 0;
				gy_old = 0;
				path_curr = 0;
				path_old = 0;
				heading = 0;
				phi = 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();

	//Read optical encoder
	hard_latch();	// This command latches all four encoder chip's count value
					// hard_latch MUST be called before any calls to read_chip functions
	g_optical0 = (int) read_chip1(); // read encoder chip # 1
	g_optical1 = (int) read_chip2();

	//Test in manual mode for heading
	heading = (double)(-g_optical0 % (THETA_SCALE*4))*3.14159/(2*THETA_SCALE);
	
	//Correct for negative g_optical readings
	if(heading < 0)
	{
		heading = heading + 3.14159*2;
	}

	//Send data to VB just so we can see what's up
	if (g_dontupdateflag == 0) {
		g_senddata[0] = g_photo0;	//pinhole light
		g_senddata[1] = g_photo1;	//ambient light
		g_senddata[2] = (float)g_optical0;
		g_senddata[3] = (float)g_optical1;
		g_senddata[4] = (float)g_digitalia;
		g_senddata[5] = 0.0F;
		g_senddata[6] = 0.0F;
		g_senddata[7] = 0.0F;
		g_senddata[8] = (float)heading*180/3.14159;
		g_senddata[9] = 0.0F;
		g_senddata[10] = 0.0F;
		g_senddata[11] = 0.0F;
		g_senddata[12] = 0.0F;
		g_senddata[13] = 0.0F;
		g_senddata[14] = 0.0F;
		g_senddata[15] = 0.0F;
		g_senddata[16] = 0.0F;
		g_senddata[17] = 0.0F;
		g_senddata[18] = 0.0F;
		g_senddata[19] = 0.0F;
	}

    return 0;
}



int RTFCNDCL TmrAuto(PVOID unused) { // AUTONOMOUS!

	int j = 0;			//counter for various things
	
	//Send Digout 4, for testing purposes only
	dig_out(4);

	//----------------------------------------------------
	// Use Optical Encoder to Map area
	//----------------------------------------------------

	//Read optical encoder
	hard_latch();	// hard_latch MUST be called before any calls to read_chip functions
					
	g_optical0 = (int) read_chip1();	//X-Direction.. -X is CW
	g_optical1 = (int) read_chip2();	//Y-Direction.. forward is negative Y
	
	//			-Y(forward)
	//			 _
	//			/ \
	//	+X	   |   |	-X (turning right) ->
	//			\_/
	//
	//			+Y


	//---------------------------------------------------------------
	// Use the path array to log position
	//
	// To convert X, which is in 'counts', use the following conversion:
	//	656 counts = 90 degrees
	//	so, multiply counts by 3.14159/(2*656)
	//
	// Statistical Data:
	//	10 revolutions: 26049+216 counts	May 7, 2002 3:45 AM
	//  20 revolutions: 26304+26049 counts	May 7, 2002 3:47 AM
	//  10 revolutions: 17+26304 counts		May 7, 2002 3:48 AM
	//
	//---------------------------------------------------------------

	//heading from +Y in radians

	//656 is not right, try 530
	heading = (double)(-g_optical0 % (THETA_SCALE*4))*3.14159/(2*THETA_SCALE);

	//Correct for negative g_optical readings
	if(heading < 0)
	{
		heading = heading + 3.14159*2;
	}

	if((DoFlag == 0)||(MODE == 64))
	{
		//Correct for positive or negative heading
		/*if(heading > 1.5708)
		{
			heading = heading - 3.14159;
		}*/

		path_curr = (int)(-g_optical1);

		gx = (float)(path_curr - path_old)*sin(heading) + gx_old;
		gy = (float)(path_curr - path_old)*cos(heading) + gy_old;

		path_old = path_curr;
		gx_old = gx;
		gy_old = gy;
	}


	//Path Optimization
	if((FoundFlag == 0)&&(countertime == 1))
	{
		if((MODE == 20)||(MODE == 40)||(MODE == 51)||(MODE == 53)||(MODE == 55)||(MODE == 57))
		{
			g_map_path[pathcount][1] = gx;	//X-pos
			g_map_path[pathcount][2] = gy;	//Y-pos
			pathcount++;
		}
	}

	//----------------------------------------------------
	// MODE:
	// 	10 - Checking  area: from forward positiion, looks right
	// 	11 - Checking  area: looks left
	// 	12 - Checking  area: returns to forward position
	//
	// 	20 - Goes forward
	// 
	//	40 - Photoresistor senses close agreement to ambient light
	//
	//	50 - Tactile sensorr has gone off: F - right, backup
	//	51 - F-right, turn left
	//	52 - F-left, backup
	//	53 - f-left, turn right
	//	54 - R-right, go forward
	//	55 - R-right, turn right
	//	56 - R-left, go forward
	//	57 - R-left, turn left
	//
	//	60 - Target found, turn on solenoid