Skip to content. | Skip to navigation

Personal tools

dmc.cpp

dmc.cpp

dmc.cpp

/////////////////////////////////////////////////////////////////////////////
// DMC.CPP
//
//  Last updated: 3/17/1999
//
//  Part of the source code for the Vane End Actuator Control System
//
//  This file contains all constants/variables/routines that are dedicated to
//  motor control and communication with the Galil DMC cards.
/////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////
// DMCCharReady - Check for waiting DMC character
int DMCCharReady(int Port)
// Returns a 1 if the DMC on port Port has a character ready,
// otherwise it returns a zero
{
#ifdef ENABLEIO
	return(((inp(Port+1) & 0x20) == 0));
#else

	return(0);
#endif

}

/////////////////////////////////////////////////////////////////////////////
// DMCSend - Send a string to a DMC
void DMCSend(int Port, char *Data)
// Output a string (Data) to DMC on port Port.
// See the DMC manual for more info on communication with the DMC cards.
{
	int i;

	Log.Add("DMCSend(%d): Sending \"%s\" to Port %X",3,__LINE__,Data,Port);
#ifdef ENABLEIO
	// Clear the DMC's buffer
	while ((inp(Port+1) & 0x20) == 0)
		inp(Port);
	// Send the command
	for (i=0;i < strlen(Data);i++)
		outp(Port,Data[i]);
#endif

}

/////////////////////////////////////////////////////////////////////////////
// DMCReceive - Wait for a response from a DMC
int DMCReceive(int Port,char *Response,int Timeout)
// Put the DMC's (on port Port) response in Response, and wait for Timeout
// milliseconds before giving up.
// Returns a 0 on success, 1 on a failure, -1 on not done yet
// See the DMC manual for more info on communication with the DMC cards.
{
	static int i; // Index variable
	static int Done; // Flag: 1 when routine is done, otherwise 0

	if (DMCStep == 0)
	{
		i = 0;
		Done = 0;
		DMCTime = Timer; // Record the current time to prepare to wait
		DMCStep = 1;
		return(-1);
	}
	else if (DMCStep == 1)
	{
		// Wait for response
		while (DMCCharReady(Port) && (i < 79))
		{
			Response[i++] = (char)inp(Port);
			if (Response[i-1] == ':')
				Done = 1;
		}
		// Null-terminate the string
		Response[i] = 0;

		// Stop getting data when a ':' is received, or the timeout has been reached.
		// (Timer - savetime) gives the time in milliseconds since we started looking
		// for data.
		if (((Timer - DMCTime) < Timeout) && !Done)
			return(-1);  // Response not yet complete, timeout not reached

		if (Done)
		{
			Log.Add("DMCReceive(%d): Received \"%s\" from Port %X",3,__LINE__,Response,Port);
			if (Port == Axial)
			{
				if (AxialErrorCount >= MaxDMCErrors)
					EDSLog.Add(988,"Axial error messages resumed");
				AxialErrorCount = 0;
			}
			else
			{
				if (RadialErrorCount >= MaxDMCErrors)
					EDSLog.Add(988,"Radial error messages resumed");
				RadialErrorCount = 0;
			}
			DMCStep = 0;
			return(0);
		}
		// A communication failure means that a ':' was not received
		// before a Timeout occurred
		Log.Add("!DMCReceive(%d): Timeout!, Received \"%s\" from Port %X",VERBOSE,__LINE__,Response,Port);
		if (Port == Axial)
		{
			AxialErrorCount++;
			if (AxialErrorCount < MaxDMCErrors)
				EDSLog.Add(11,"Axial DMC com error");
			if (AxialErrorCount == MaxDMCErrors)
				EDSLog.Add(15,"Axial DMC error messages suspended");
		}
		else
		{
			RadialErrorCount++;
			if (RadialErrorCount < MaxDMCErrors)
				EDSLog.Add(11,"Radial DMC com error");
			if (RadialErrorCount == MaxDMCErrors)
				EDSLog.Add(15,"Radial DMC error messages suspended");
		}
		DMCStep = 0;
		return(1);
	}
	return(-1);
}

/////////////////////////////////////////////////////////////////////////////
// DMCCommand - Send a DMC a command and wait for a response
int DMCCommand(int Port, char *Data,char *Response,int Timeout)
{
	// This routine sends the command in string Data to the DMC on port Port,
	// and puts the response from the DMC in string Response.  The routine will
	// timeout after Timeout milliseconds.
	// Returns a 0 on success, 1 on failure, -1 on not done yet.
	// A 1 means that the DMC timed-out (didn't give a ':' prompt back
	// before Timeout milliseconds)

	char temp[255];
	int e;

	if (DMCStep == 0)
		DMCSend(Port,Data);
	e = DMCReceive(Port,Response,Timeout);
	if ((e > 0) && (Response[0] == '?'))
	{
		DMCSend(Port,"TC1\r");
		BDMCReceive(Port,temp,DMCFastCommand);
		Log.Add("!DMCCommand(%d): DMC Error message: %s",VERBOSE,__LINE__,temp);
	}
	return(e);
}

/////////////////////////////////////////////////////////////////////////////
// CheckDMCs - Verifies initial power-up state of DMCs
int CheckDMCs()
// CheckDMCs exists just to make sure that on boot-up, the outputs
// of the Radial controller are low (0), which disables the emergency stop
// card, and that the Axial controller outputs are high (1), turning OFF
// pressure and vacuum.  These outputs are stored using the burn command (BN),
// but this procedure checks them just in case, and re-burns them if necessary.
{
	char line[80];

	Log.Add("CheckDMCs(%d): Resetting DMCs",VERBOSE,__LINE__);
	// Reset DMCs to simulate power-up
	if (BDMCCommand(Radial,"RS\r",line,DMCSlowCommand)) return(1);

//// Axial controller won't perform a reset, it claims an EEPROM checksum error
// *******
//	if (BDMCCommand(Axial,"RS\r",line,DMCSlowCommand)) return(2);
// *******
	// Turn echo off (it messes up the atof function if its on)
	if (BDMCCommand(Radial,"EO 0\r",line,DMCSlowCommand)) return(3);
	if (BDMCCommand(Axial,"EO 0\r",line,DMCSlowCommand)) return(4);
	Log.Add("CheckDMCs(%d): Verifying DMC boot-up outputs",VERBOSE,__LINE__);
	// Check the radial outputs, and verify that it is 0
	if (BDMCCommand(Radial,"MG _OP\r",line,DMCSlowCommand))
		return(5);
	if (atof(line) != 0)
	{
		Log.Add("!CheckDMCs(%d): Radial boot-up output wrong, correcting",VERBOSE,__LINE__);
		// If it was wrong, set the outputs to zero and burn
		if (BDMCCommand(Radial,"OP0\r",line,DMCSlowCommand))
			return(6);
		if (BDMCCommand(Radial,"BN\r",line,2000))
			return(7);
	}
	// Check the axial outputs, and verify that it is 255
	if (BDMCCommand(Axial,"MG _OP\r",line,DMCSlowCommand))
		return(8);
	if (atof(line) != 255)
	{
		Log.Add("!CheckDMCs(%d): Axial boot-up output wrong, correcting",VERBOSE,__LINE__);
		// If it was wrong, set the outputs to 255 and burn
		if (BDMCCommand(Axial,"OP255\r",line,DMCSlowCommand))
			return(9);
		if (BDMCCommand(Axial,"BN\r",line,2000))
			return(10);
	}
	return(0);
}

/////////////////////////////////////////////////////////////////////////////
// GetEncoders - Returns current axial and radial encoder values
int GetEncoders(int AE[4],int RE[4])
// Reads all 8 encoder positions into AE (Axial encoders)
// and RE (Radial encoders).
// Returns a 0 on success, 1 on a failure, -1 on not done yet
//
// AE[0] = NW Axial,AE[1] = SW Axial,AE[2] = SE Axial,AE[3] = NE Axial
// RE[0] = NW Radial,RE[1] = SW Radial,RE[2] = SE Radial,RE[3] = NE Radial
{
	int i,temp;
	static int Error;
	static char line[80]; /* Buffer */

	if (GetEncodersStep == 0)
	{
		Error = 0;
		for (i=0;i<=3;i++)
		{
			RE[i] = -999999;
			AE[i] = -999999;
		}
		Log.Add("GetEncoders(%d): Reading radial encoders",VERBOSE,__LINE__);
		GetEncodersStep = 1;
		return(-1);
	}
	else if (GetEncodersStep == 1)
	{
		// First read radial encoders with the Tell Position (TP) command
		temp = DMCCommand(Radial,"TP\r",line,DMCFastCommand);
		if (temp == -1) return(-1);
		if (temp != 0)
		{
			Log.Add("!GetEncoders(%d): Error reading radial encoders",VERBOSE,__LINE__);
			Error = 1;
		}
		else if (sscanf(line," %d, %d, %d, %d",&RE[0],&RE[3],&RE[1],&RE[2]) != 4)
		{
			Log.Add("!GetEncoders(%d): Error reading radial encoders",VERBOSE,__LINE__);
			Error = 2;
		}
		Log.Add("GetEncoders(%d): Reading axial encoders",VERBOSE,__LINE__);
		GetEncodersStep = 2;
		return(-1);
	}
	else if (GetEncodersStep == 2)
	{
		// Then read axial encoders
		temp = DMCCommand(Axial,"TP\r",line,DMCFastCommand);
		if (temp == -1) return(-1);
		if (temp != 0)
		{
			Log.Add("!GetEncoders(%d): Error reading axial encoders",VERBOSE,__LINE__);
			Error = 3;
		}
		else if (sscanf(line," %d, %d, %d, %d",&AE[0],&AE[3],&AE[1],&AE[2]) != 4)
		{
			Log.Add("!GetEncoders(%d): Error reading axial encoders",VERBOSE,__LINE__);
			Error = 4;
		}
		// Encoders will always read negative, so invert
		for (i=0;i<=3;i++)
		{
			AE[i] = -AE[i];
			RE[i] = -RE[i];
		}
	}
	GetEncodersStep = 0;
	return(Error);
}

/////////////////////////////////////////////////////////////////////////////
// GetSwitches - Returns current axial and radial switch values
int GetSwitches(int AS[4],int RS[4])
// Reads all 8 switch values into AS (Axial switches)
// and RS (Radial switches).
// Returns a 0 on success, 1 on a failure, -1 on not done yet
//
// AS[0] = NW Axial,AS[1] = SW Axial,AS[2] = SE Axial,AS[3] = NE Axial
// RS[0] = NW Radial,RS[1] = SW Radial,RS[2] = SE Radial,RS[3] = NE Radial
{
	static int Error;
	static char line[80]; /* Buffer */
	int i,temp;

	if (GetSwitchesStep == 0)
	{
		Error = 0;
		for (i=0;i<=3;i++)
		{
			AS[i] = 0;
			RS[i] = 0;
		}
		Log.Add("GetSwitches(%d): Reading radial switches",VERBOSE,__LINE__);
		GetSwitchesStep = 1;
		return(-1);
	}
	else if (GetSwitchesStep == 1)
	{
		// First read radial switches with the Tell Switches (TS) command
		temp = DMCCommand(Radial,"TS\r",line,DMCFastCommand);
		if (temp == -1) return(-1);
		if (temp != 0)
		{
			Log.Add("!GetSwitches(%d): Error reading radial switches",VERBOSE,__LINE__);
			Error = 5;
		}
		else if (sscanf(line," %d, %d, %d, %d",&RS[0],&RS[3],&RS[1],&RS[2]) != 4)
		{
			Log.Add("!GetSwitches(%d): Error reading radial switches",VERBOSE,__LINE__);
			Error = 6;
		}
		Log.Add("GetSwitches(%d): Reading axial switches",VERBOSE,__LINE__);
		GetSwitchesStep = 2;
		return(-1);
	}
	else if (GetSwitchesStep == 2)
	{
		// Then read axial switches
		temp = DMCCommand(Axial,"TS\r",line,DMCFastCommand);
		if (temp == -1) return(-1);
		if (temp != 0)
		{
			Log.Add("!GetSwitches(%d): Error reading axial switches",VERBOSE,__LINE__);
			Error = 7;
		}
		else if (sscanf(line," %d, %d, %d, %d",&AS[0],&AS[3],&AS[1],&AS[2]) != 4)
		{
			Log.Add("!GetSwitches(%d): Error reading axial switches",VERBOSE,__LINE__);
			Error = 8;
		}
	}
	GetSwitchesStep = 0;
	return(Error);
}

/////////////////////////////////////////////////////////////////////////////
// ConfigDMCs - Sets up the DMCs
int ConfigDMCs()
// Configures the two DMC cards
// Returns a 0 on success, 1 on failure.
{
	char line[80];

	// First configure the DMCs
	Log.Add("ConfigDMCs(%d): Configuring the DMCs",VERBOSE,__LINE__);
#ifdef ENABLEIO
	// Empty and configure DMC FIFO buffers

	// Set up DMC fifos for 128 bytes in both directions. Again, check the
	// manual for info.
	outp(Radial+1,0x05);
	outp(Radial+1,0xF0);
	outp(Axial+1,0x05);
	outp(Axial+1,0xF0);
	// Clear DMC buffers. Check the DMC manual for information
	// on the weird codes you have to send it.
	inp(Radial+1);
	outp(Radial+1,0x01);
	outp(Radial+1,0x80);
	outp(Radial+1,0x01);
	outp(Radial+1,0x80);
	outp(Axial+1,0x01);
	outp(Axial+1,0x80);
	outp(Axial+1,0x01);
	outp(Axial+1,0x80);
	inp(Radial+1);
#endif

	// Initialize Radial DMC
	// Turn echo off and motors off
	if (BDMCCommand(Radial,"EO 0\r",line,DMCSlowCommand)) return(1);
	if (BDMCCommand(Radial,"MO\r",line,DMCSlowCommand)) return(2);
	// Set motor types to stepper
	if (BDMCCommand(Radial,"MT-2,-2,-2,-2\r",line,DMCSlowCommand)) return(3);
	// Configure limit,home,and latch inputs active low, and stepper pulse = 25.36us
	if (BDMCCommand(Radial,"CN1,-1,-1,3\r",line,DMCSlowCommand)) return(4);
	// Set accelerations
	if (BDMCCommand(Radial,"AC5000,5000,5000,5000\r",line,DMCFastCommand)) return(5);
	// Set decelerations
	if (BDMCCommand(Radial,"DC10000,10000,10000,10000\r",line,DMCFastCommand)) return(6);
	// Set maximum speeds at about 0.1mm/sec
	if (BDMCCommand(Radial,"SP380,380,380,380\r",line,DMCFastCommand)) return(7);
	// Repeat for Axial DMC
	// Turn echo off and motors off
	if (BDMCCommand(Axial,"EO 0\r",line,DMCSlowCommand)) return(8);
	if (BDMCCommand(Axial,"MO\r",line,DMCSlowCommand)) return(9);
	// Set motor types to stepper
	if (BDMCCommand(Axial,"MT2,2,2,2\r",line,DMCSlowCommand)) return(10);
	// Configure limit,home,and latch inputs active low, and stepper pulse = 25.36us
	if (BDMCCommand(Axial,"CN1,-1,-1,3\r",line,DMCSlowCommand)) return(11);
	// Set accelerations
	if (BDMCCommand(Axial,"AC5000,5000,5000,5000\r",line,DMCFastCommand)) return(12);
	// Set decelerations
	if (BDMCCommand(Axial,"DC10000,10000,10000,10000\r",line,DMCFastCommand)) return(13);
	// Set maximum speeds at about 0.1mm/sec
	if (BDMCCommand(Axial,"SP160,160,160,160\r",line,DMCFastCommand)) return(14);
	return(0);
}

/////////////////////////////////////////////////////////////////////////////
// ResetEmergency - Reset the emergency stop card
int ResetEmergency()
{
	// Reset emergency stop card
	int ResetOK = 0;
	int i = 0;
	int Check;
	char line[80],s[255];

	Log.Add("ResetEmergency(%d): Resetting the Emergency Stop Card",VERBOSE,__LINE__);
	// Sometimes the card doesn't reset on the first try, so I give it
	// 5 chances (Two is the most I've seen, 5 should be way more than necessary) */
	do
	{
		i++;
		// Watchdog must be serviced before reset
		Watchdog();
		// Set PC Emergency Stop Output bit
		BDMCCommand(Radial,"SB2\r",line,DMCFastCommand);
		// Set Emergency Reset Output
		BDMCCommand(Radial,"SB1\r",line,DMCFastCommand);
		// Wait for Reset to occur
		SafeDelay(900);
		Watchdog();
		BDMCCommand(Radial,"CB1\r",line,DMCFastCommand);
		if (!BDMCCommand(Radial,"TI\r",line,DMCSlowCommand))
			if (sscanf(line,"%d",&Check) == 1)
			{
				Check ^= 255;
				if (!(Check & 16))
					Flags.SetFatal(FFEmergencyStop);
				else
				{
					Flags.ClearFatal(FFEmergencyStop);
					ResetOK = 1;
				}
			}
	} while (!ResetOK && (i < 5));
	if (!ResetOK)
	{
		Log.Add("!ResetEmergency(%d): Error resetting the Emergency Stop Card",VERBOSE,__LINE__);
		return(1);
	}
	return(0);
}

/////////////////////////////////////////////////////////////////////////////
// MoveAllMotors - Moves all motors the specified number of steps
int MoveAllMotors(int TA[4],int TR[4],int MoveTime,int Timeout,int MoveDelay)
// This routine sends commands to the DMCs to accomplish a move
// of TA[] (target axial) or TR[] (target radial) steps in
// MoveTime milliseconds, allowing a timeout of
// Timeout milliseconds before failure and waiting MoveDelay ms after the move
// Returns a 0 on success, -1 on not done yet, > 0 on error
//
// TA[0] = NW Axial,TA[1] = SW Axial,TA[2] = SE Axial,TA[3] = NE Axial
// TR[0] = NW Radial,TR[1] = SW Radial,TR[2] = SE Radial,TR[3] = NE Radial
{
	int SPA[4],SPR[4]; // Calculated motor speeds
	int i,z[4] = {0,0,0,0};
	char line[80],temp[80];
	static int t;
	static int AxialDone,RadialDone;

	if (MoveAllMotorsStep == 0)
	{
		AxialDone  = 0;
		RadialDone = 0;
		// Setup motor move speeds, so that the move is accomplished
		// in MoveTime milliseconds. The minimum speed is 2.
		for (i=0;i<=3;i++)
		{
			SPA[i] = abs(TA[i]) * 1000 / (float)MoveTime;
			if (SPA[i] < 2)
				SPA[i] = 2;
			SPR[i] = abs(TR[i]) * 1000 / (float)MoveTime;
			if (SPR[i] < 2)
				SPR[i] = 2;
		}
		// Turn on axial motor, set destination location and speed
		sprintf(line,"SH\r");
		if (BDMCCommand(Axial,line,temp,DMCSlowCommand))
		{
			Log.Add("!MoveAllMotors(%d): Communication error with Axial DMC",VERBOSE,__LINE__);
			return(1);
		}
		sprintf(line,"PR%ld,%ld,%ld,%ld\r",AxialSign*TA[0],AxialSign*TA[3],
						AxialSign*TA[1],AxialSign*TA[2]);
		Cell.DisplayAxialMove(TA);
		if (BDMCCommand(Axial,line,temp,DMCSlowCommand))
		{
			Log.Add("!MoveAllMotors(%d): Communication error with Axial DMC",VERBOSE,__LINE__);
			return(2);
		}
		sprintf(line,"SP%ld,%ld,%ld,%ld\r",SPA[0],SPA[3],SPA[1],SPA[2]);
		if (BDMCCommand(Axial,line,temp,DMCSlowCommand))
		{
			Log.Add("!MoveAllMotors(%d): Communication error with Axial DMC",VERBOSE,__LINE__);
			return(3);
		}
		// Turn on radial motor, set destination location and speed
		sprintf(line,"SH\r");
		if (BDMCCommand(Radial,line,temp,DMCSlowCommand))
		{
			Log.Add("!MoveAllMotors(%d): Communication error with Radial DMC",VERBOSE,__LINE__);
			return(4);
		}
		sprintf(line,"PR%ld,%ld,%ld,%ld\r",RadialSign*TR[0],RadialSign*TR[3],
						RadialSign*TR[1],RadialSign*TR[2]);
		Cell.DisplayRadialMove(TR);
		if (BDMCCommand(Radial,line,temp,DMCSlowCommand))
		{
			Log.Add("!MoveAllMotors(%d): Communication error with Radial DMC",VERBOSE,__LINE__);
			return(5);
		}
		sprintf(line,"SP%ld,%ld,%ld,%ld\r",SPR[0],SPR[3],SPR[1],SPR[2]);
		if (BDMCCommand(Radial,line,temp,DMCSlowCommand))
		{
			Log.Add("!MoveAllMotors(%d): Communication error with Radial DMC",VERBOSE,__LINE__);
			return(6);
		}
		// Now begin the move on all axes which require it
		// Only execute an axial move if movement is required (TA[x] != 0)
		if (TA[0] || TA[1] || TA[2] || TA[3])
		{
			sprintf(line,"BG ");
			if (TA[0]) strcat(line,"X");
			if (TA[3]) strcat(line,"Y");
			if (TA[1]) strcat(line,"Z");
			if (TA[2]) strcat(line,"W");
			strcat(line,"\r");
			if (BDMCCommand(Axial,line,temp,DMCSlowCommand))
			{
				Log.Add("!MoveAllMotors(%d): Communication error with Axial DMC",VERBOSE,__LINE__);
				return(7);
			}
		}
		else
		// If all of the TA[] variables are 0, no more axial moves are required
			AxialDone = 1;
		// Only execute a radial move if movement is required (TR[x] != 0)
		if (TR[0] || TR[1] || TR[2] || TR[3])
		{
			sprintf(line,"BG ");
			if (TR[0]) strcat(line,"X");
			if (TR[3]) strcat(line,"Y");
			if (TR[1]) strcat(line,"Z");
			if (TR[2]) strcat(line,"W");
			strcat(line,"\r");
			if (BDMCCommand(Radial,line,temp,DMCSlowCommand))
			{
				Log.Add("!MoveAllMotors(%d): Communication error with Radial DMC",VERBOSE,__LINE__);
				return(8);
			}
		}
		else
		// If all of the TR[] variables are 0, no more radial moves are required
			RadialDone = 1;
		// Now wait for the move to finish
		t = Timer;
		MoveAllMotorsStep = 1;
		return(-1);
	}
	if (MoveAllMotorsStep == 1)
	{
		// AxialDone and RadialDone are set to 1 when their respective moves
		// are complete.
		if (!((Cell.AS[0] & 128) || (Cell.AS[1] & 128) ||
					(Cell.AS[2] & 128) || (Cell.AS[3] & 128)))
			AxialDone = 1;
		if (!((Cell.RS[0] & 128) || (Cell.RS[1] & 128) ||
					(Cell.RS[2] & 128) || (Cell.RS[3] & 128)))
			RadialDone = 1;
		// Movement is finished when the DMCs have responded or the timeout is reached
		if (((Timer - t) < Timeout) && (!AxialDone || !RadialDone)) return(-1);
		if (!AxialDone)
			EDSLog.Add(12,"Axial motion timeout");
		if (!RadialDone)
			EDSLog.Add(12,"Radial motion timeout");
		MoveAllMotorsStep = 2;
		Cell.DisplayAxialMove(z);
		Cell.DisplayRadialMove(z);
		t = Timer;
		return(-1);
	}
	if (MoveAllMotorsStep == 2)
	{
		// Allow the motors to come to a stop, and let the DMCs get ready for
		// a new command.
		if ((Timer - t) > MoveDelay)
		{
			MoveAllMotorsStep = 0;
			return(0);
		}
	}
	return(-1);
}

/////////////////////////////////////////////////////////////////////////////
// DMCTerminal - Starts a simple terminal session with a DMC
void DMCTerminal(int Port)
// This routine provides a small terminal emulation
// for direct communication with the DMC cards.
// The DMC to be communicated with is passed in Port.
//
// Hitting ESC closes the terminal.
{
	char line[80];
	int c;

	// Turn echo mode on
	BDMCCommand(Port,"EO 1\r",line,DMCSlowCommand);
	CloseVideo();
	if (Port == Radial)
		cprintf("Radial DMC Terminal on port %X (ESC to end)\r\n",Port);
	else
		cprintf("Axial DMC Terminal on port %X (ESC to end)\r\n",Port);
	do
	{
		/* Check for outgoing characters */
		if (kbhit())
		{
			c = getch();
			outp(Port,c);
		}
		/* Check for incoming characters */
		if (DMCCharReady(Port))
			cprintf("%c",inp(Port));
		Watchdog();
	} while (c != 27);
	InitVideo();
	InitDisplay();
	Cell.DisplayAll();
	// Turn echo mode back off
	DMCSend(Port,"\rEO 0\r");
  SafeDelay(250);
	// Clear the DMC's buffer
	while ((inp(Port+1) & 0x20) == 0)
		inp(Port);
	// Make sure further DMC communication starts off on the right foot.
	DMCStep = 0;
}

/////////////////////////////////////////////////////////////////////////////
// MoveIndividualMotor - Moves a single motor a given number of microns
int MoveIndividualMotor(int Port,int MotorNum,int Dist)
// This routine moves a single motor Dist microns at the encoder.
// Pass either Axial or Radial as the Port value, and an integer
// from 1 to 4 for MotorNum.
//
// Returns a 0 on success, 1 on failure
{
	int i,c,e,t;
	int Done = 0;
	int TE[4]; // Store the relative encoder values to move to
	int MS[4]; // Store the number of steps we want the motors to move
	int SP[4]; // Store the speeds the motors will move at
	char temp[80],line[80],s[255];
	int TooClose = 0; // Flag set to 1 if the encoders are close to their stops
	int MoveTime;

	// Set the number of steps to give the motors to achieve the move.
	// Set all speeds to twice the required distance, so that a move takes
	// about half a second.
	for (i=0;i<=3;i++)
	{
		// Set the reqired motors distance, and all other motors to 0
		if (MotorNum == i+1)
			TE[i] = Dist;
		else
			TE[i] = 0;
		// Check for a possible collision with the encoder's stop.
		// (We don't want to damage the encoder, and it is possible!!!)
		if (Port == Axial)
		{
			if (TE[i] + Cell.AE[i] < 100) TooClose = 1;
			MS[i] = TE[i] / AxialEncoderMicsPerStep;
		}
		else
		{
			if (TE[i] + Cell.RE[i] < 100) TooClose = 1;
			MS[i] = TE[i] / RadialEncoderMicsPerStep;
		}
		// Set the motor speeds
		// The speeds must be at least 2 or you get an error from DMC
		if (abs(MS[i]) < 2)
			SP[i] = 2;
		else
		{
			SP[i] = 2 * abs(MS[i]);
			if (SP[i] > 500) SP[i] = 500;
		}
		if (TE[i] != 0)
		{
			MoveTime = 2 * abs(MS[i] / SP[i]) * 1000;
			if (MoveTime < 2000) MoveTime = 2000;
		}
	}
	if (TooClose)
	{
		InputLog.Add("Encoders may be too",BRIEF);
		InputLog.Add("close to stops!",BRIEF);
		InputLog.Add("Move anyway (Y/N)?",BRIEF);
		InputLog.DisplayAll();
		while (!kbhit())
			Watchdog();
		c = toupper(getch());
		InputLog.Add("* %c",BRIEF,c);
		if (c != 'Y') return(1);
	}
	Log.Add("MoveIndividualMotor(%d): Beginning move",VERBOSE,__LINE__);
	// Turn on motors, set destination location and speed
	sprintf(line,"SH\r");
	if (BDMCCommand(Port,line,temp,DMCSlowCommand)) return(2);
	if (Port == Radial)
		sprintf(line,"PR%ld,%ld,%ld,%ld\r",RadialSign*MS[0],RadialSign*MS[3],
						RadialSign*MS[1],RadialSign*MS[2]);
	else
		sprintf(line,"PR%ld,%ld,%ld,%ld\r",AxialSign*MS[0],AxialSign*MS[3],
						AxialSign*MS[1],AxialSign*MS[2]);
	if (BDMCCommand(Port,line,temp,DMCSlowCommand)) return(3);
	sprintf(line,"SP%ld,%ld,%ld,%ld\r",SP[0],SP[3],SP[1],SP[2]);
	if (BDMCCommand(Port,line,temp,DMCSlowCommand)) return(4);
	// Now begin the move
	if (MS[0] || MS[1] || MS[2] || MS[3])
	{
		sprintf(line,"BG ");
		if (MS[0]) strcat(line,"X");
		if (MS[3]) strcat(line,"Y");
		if (MS[1]) strcat(line,"Z");
		if (MS[2]) strcat(line,"W");
		strcat(line,"\r");
		if (BDMCCommand(Port,line,temp,DMCSlowCommand)) return(5);
	}
	// Now wait for the move to finish
	t = Timer;
	do
	{
		// Don't let watchdog go off
		Watchdog();
		if (kbhit())
		{
			c = getch();
			if (c == 27)
			{
				BDMCCommand(Radial,"CB2\r",temp,DMCFastCommand);
				BDMCCommand(Radial,"CB2\r",temp,DMCFastCommand);
			}
		}
		// Done is set to 1 when the move is complete
		e = Cell.UpdateEncoders();
		if (e == 0)
		{
			if (Port == Axial)
				if (!((Cell.AS[0] & 128) || (Cell.AS[1] & 128) || (Cell.AS[2] & 128) || (Cell.AS[3] & 128)))
					Done = 1;
			if (Port == Radial)
				if (!((Cell.RS[0] & 128) || (Cell.RS[1] & 128) || (Cell.RS[2] & 128) || (Cell.RS[3] & 128)))
					Done = 1;
		}
		Cell.UpdateTensions();
	} while (((Timer - t) < MoveTime) && (!Done));
	if (!Done)
		EDSLog.Add(12,"Motion timeout",12);
	// Allow the motors to come to a stop, and let the DMCs get ready for
	// a new command.
	SafeDelay(DMCMoveCommand);
	// Turn the motors off to avoid heating
	if (BDMCCommand(Port,"MO\r",temp,DMCSlowCommand)) return(6);
	return(0);
}

/////////////////////////////////////////////////////////////////////////////
// AdjustTension - Moves all radial motors to adjust the tensions
int AdjustTension(int Dist)
// This routine moves all radial motors to Dist microns in from their zeroed
// location, tensioning the vanes.  The new TensionAmount is written to
// VANE.INI.
//
// Returns a 0 on success, 1 on abort
{
	int i,c,e;
	char s[255];
	int t;
	int Done = 0;
	int TE[4]; // Store the relative encoder values to move to
	int MS[4]; // Store the number of steps we want the motors to move
	int SP[4]; // Store the speeds the motors will move at
	char temp[80],line[80];
	int TooClose = 0; // Flag set to 1 if the encoders are close to their stops
	int MoveTime;

	InputLog.Add("About to adjust",BRIEF);
	InputLog.Add("tensions! Are",BRIEF);
	InputLog.Add("you sure (Y/N)?",BRIEF);
	InputLog.DisplayAll();
	while (!kbhit())
		Watchdog();
	c = toupper(getch());
	InputLog.Add("* %c",BRIEF,c);
	if (c != 'Y') return(1);

	// Write the new tension value to VANE.INI
	WriteTension(Dist);

	// Start moving to the new, tensioned location
	Flags.SetStatus(SFMoveInProgress);
	Flags.SetMoveStage(MSBacklash);
	Cell.dx = Cell.x;
	Cell.dy = Cell.y;
	Cell.dz = Cell.z;
	Cell.dh = Cell.h;
	Cell.dv = Cell.v;
	Cell.DisplayDestination();
	return(0);
}


Generated by GNU Enscript 1.6.5.2.
Document Actions