stepmotor_orig.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 float 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_avg;
volatile float g_photo1_avg;
volatile float g_photo0_array[100];
volatile float g_photo1_array[100];
void			FunctionINIT();				//used to initialize arrays to zero

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	doflag = 0;					//used to skip or enter Mode switch case

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



// 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];
				
				MODE = 10;	//Begin LiSAR to check surroundings
				doflag = 0;

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

				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;
				// Of course I could have used a for loop here?????
				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;
			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 = (float) 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 = (float) read_chip1(); // read encoder chip # 1
	g_optical1 = (float) read_chip2();

	//Send out voltage?  ...nah
	//volt_out(0,uopen);  //uopen is the sent out voltage

	//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] = g_optical0;
		g_senddata[3] = g_optical1;
		g_senddata[4] = g_digitalia;
		g_senddata[5] = 0.0F;
		g_senddata[6] = 0.0F;
		g_senddata[7] = 0.0F;
		g_senddata[8] = 0.0F;
		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);

	if(doflag == 0){
		switch (MODE){
			case 10: //Check around - 1st step, look left
				countertime = 0;
				g_servo1_period = (float)0.0015 + delta;
				g_servo2_period = (float)0.0015 + delta;
				doflag = 1;
				break;
			case 11: //Check around - 2nd step, look right
				countertime = 0;
				g_servo1_period = (float)0.0015 - delta;
				g_servo2_period = (float)0.0015 - delta;
				doflag = 1;
				break;
			case 12: //Check around - 3rd step, look left again
				countertime = 0;
				g_servo1_period = (float)0.0015 + delta;
				g_servo2_period = (float)0.0015 + delta;
				doflag = 1;
				break;
			case 20: //Go Forward
				countertime = 0;
				g_servo1_period = (float)0.0015 + delta;
				g_servo2_period = (float)0.0015 - delta;
				doflag = 1;
				break;
			default:
				break;
		}
	}

	// Update Servo Period with new value when background task
	if(countertime == 0)
	{
		command_servo(1,3,g_servo1_period);
		command_servo(2,3,g_servo2_period);
	}

	//Increase Countertime!
	countertime++;

	//Check which mode it is in and see if it is done with its task, then switch modes
	if(doflag == 1){
		switch(MODE){
			case 10:
				if(countertime >= checktime)
				{
					MODE = 11;
					doflag = 0;
				}
				break;

			case 11:
				if(countertime >= 2*checktime)
				{
					MODE = 12;
					doflag = 0;
				}
				break;
			case 12:
				if(countertime >= checktime)
				{
					MODE = 20;
					doflag = 0;
				}
				break;
			case 20:
				if(countertime >= forwardtime)
				{
					MODE = 10;
					doflag = 0;
				}
				break;
			default:
				break;
		}
	}
					
	//----------------------------------------------------
	// Check Resistors - if anything special, change MODE
	//----------------------------------------------------

	//First Collect Data from analog in, shift everything up, put new value at end
	for(j = 0; j < 99; j++)
	{
		g_photo0_array[j] = g_photo0_array[j+1];
		g_photo1_array[j] = g_photo1_array[j+1];
	}
	g_photo0_array[99] = volt_in(0); //Pin hole resistor
	g_photo1_array[99] = volt_in(1); //Ambient Light Resistor

	//Now Compute an average - less shifty, will be easier to work with?
	g_photo0_avg = 0;
	g_photo1_avg = 0;
	for(j = 0; j < 100; j++)
	{
		g_photo0_avg += (float)0.01*g_photo0_array[j];
		g_photo1_avg += (float)0.01*g_photo1_array[j];
	}

	//----------------------------------------------------
	// Check Digital_In - if anything special, change MODE
	//----------------------------------------------------

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

	/*switch(digitalia){
		case 254:
			MODE = 40;
			break;
		case 254:
			MODE = 41;
			break;
		case 254:
			MODE = 42;
			break;
		case 254:
			MODE = 43;
			break;
		default:
			break;
	}*/

	//----------------------------------------------------
	// 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 = (float) read_chip1();
	g_optical1 = (float) read_chip2();

	//----------------------------------------------------
	// Send stuff to VB so we can take a look
	//----------------------------------------------------
	if (g_dontupdateflag == 0) {
		g_senddata[0] = g_photo0_avg;	//pinhole light
		g_senddata[1] = g_photo1_avg;	//ambient light
		g_senddata[2] = g_optical0;
		g_senddata[3] = g_optical1;
		g_senddata[4] = g_digitalia;
		g_senddata[5] = delta;
		g_senddata[6] = 0.0F;
		g_senddata[7] = 0.0F;
		g_senddata[8] = 0.0F;
		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] = g_servo1_period;
		g_senddata[17] = g_servo2_period;
		g_senddata[18] = 0.0F;
		g_senddata[19] = 0.0F;
	}

	//Send back to zero - only for testing purposes
	dig_out(0);

    return 0;
}

void FunctionINIT()	//Only here to initialize variables
{
	int j = 0;

	//Initialize photoresistor variables
	for(j = 0; j < 100; j++)
	{
		g_photo0_array[j] = 0.0F;
		g_photo1_array[j] = 0.0F;
	}

	//Can initialize more here???!?!?!?
}