Arcus Ethernet Motor Class based on QT

DMX_ETH_17_Motor.cpp

/**
* Constructor. Miscellaneous initilization of some variables.
*/
DMX_ETH_17_Motor::DMX_ETH_17_Motor() {
	isConnected = false;
	mSocket = new QTcpSocket();
}

/**
* Returns whether or not the motor is connected.
* @return bool, true if the motor is connected, false otherwise.
*/
bool DMX_ETH_17_Motor::getIsConnected() {
	return isConnected;
}

/**
* Connect to the DMX ETH 17-3 motor.
* Usese standard sockets (TCP-IP) to connect to the motor over ethernet.
* The IP address and port number does not change.
* @return, result code from trying to connect, -1 if no connection made, not -1
*		   otherwise, note sure what it will be when connected.
*/
int DMX_ETH_17_Motor::connectMotor() {
	if (!isConnected) {
		mSocket->connectToHost(DMX_ETH_IP_ADDRESS_STRING, DMX_ETH_PORT);
		if (mSocket->waitForConnected(5000)) {
			isConnected = true;
			return 1;
		}
		else {
			return -1;
		}
	}
	else {
		return 0;
	}

}

/**
* Disconnects the motor.
*/
void DMX_ETH_17_Motor::disconnectMotor() {
	if (isConnected) {
		mSocket->disconnectFromHost();
		if (mSocket->waitForDisconnected()) {
			isConnected = false;
		}
	}
}

/**
* Sends a command to the motor.
* @param command, the command to the motor, without null termination.
* @return int, basically true(1) or false(0) based on if it send the command
successfully, this does not indicate that the command was receieve by the
motor.
*/
int DMX_ETH_17_Motor::sendCommand(std::string command) {
	//add null terminator to command. 
	command += std::string(COMMAND_END);
	char sendBuffer[50];
	//check for connection. 
	if (isConnected) {

		//put command in buffer. 
		strcpy(sendBuffer, command.c_str());

		int sendResult, bytesSent = 0;
		int bufferLength = strlen(sendBuffer);

		mSocket->write(sendBuffer);
		bool sent = mSocket->waitForBytesWritten(2000);
		return sent == true ? 1 : 0;
	}
	return 0;
}

/**
* Listens for a command from the motor. Has a timeout defined by DMX_ETH_RECEIEVE_TIMEOUT_SECONDS.
* @return std::string representing the receieved response from the motor.
**/
std::string DMX_ETH_17_Motor::receiveResponse() {
	//byte array buffer for data. 
	QByteArray *data = new QByteArray();
	//wait to be ready to read, up to 2 seconds. 
	bool ready = mSocket->waitForReadyRead(2000);
	//if we're ready read the socket. 
	if (ready) {
		while (mSocket->bytesAvailable() > 0) {
			//append the data to the buffer. 
			data->append(mSocket->readAll());
		}
	}
	//if we receieved data write it to a string and return it. 
	if (data->size() > 0) {
		std::string strData(data->constData());
		return strData;
	}
	else {
		//otherwise return an empty string. 
		return std::string("");
	}


}

/**
* Sends and receieves a message. This blocks until both functions are done.
* @param command, the command to send the motors.
* @return std::string the reply from the motor.
*/
std::string DMX_ETH_17_Motor::sendReceive(std::string command) {
	if (sendCommand(command)) {
		return receiveResponse();
	}
	else {
		//couldn't send command. 
		return std::string("FAIL");
	}
}

/**
* Gets the expected return type for the response for the motor, based on the given
* command.
* @param command, the string representing the command, see the define statements in
the motor header file.
* @return Return_Type, and enumeration of response types, either OK, BINARY, TWENTY_EIGHT_BIT,
THIRTY_TWO_BIT, or DMX_LONG (a long/int)
*/
Return_Type DMX_ETH_17_Motor::getCommandReturnType(std::string command) {
	//commands that return OK.
	if (command == STOP_IMMEDIATELY || command == ABSOLUTE_MOVE || command == SET_ACCELERATION_MILLIS
		|| command == SET_DECELERATION_MILLIS || command == SET_DRIVER_DIRECTION_POLARITY
		|| command == WRITE_DIGITAL_OUTPUT_ONE || command == WRITE_DIGITAL_OUTPUT_TWO
		|| command == SET_DRIVER_IDLE_CURRENT_SETTING || command == SET_DRIVER_MICROSTEP_SETTING
		|| command == SET_DRIVER_RUN_CURRENT_SETTING || command == SET_UNIQUE_DECELERATION_ENABLE
		|| command == SET_POWER_DISABLED || command == SET_POWER_ENABLED || command == SET_ENCODER_VALUE
		|| command == SET_HIGH_SPEED_SETTING || command == HOME_NEGATIVE || command == HOME_NEGATIVE
		|| command == SET_HOME_CORRECTION_AMOUNT || command == HOME_NEGATIVE_SLOW || command == HOME_POSITIVE_SLOW
		|| command == SET_MOVE_MODE_INCREMENTAL || command == JOG_MOTOR_NEGATIVE || command == JOG_MOTOR_POSITIVE
		|| command == LIMIT_HOMING_IN_NEG_DIR || command == LIMIT_HOMING_IN_POS_DIR
		|| command == SET_LIMIT_CORRECTION_AMOUNT || command == SET_LOW_SPEED_SETTING
		|| command == SET_POLARITY || command == SET_POSITION || command == READ_DRIVER_PARAMETERS
		|| command == SET_RETURN_ZERO_ENABLE || command == WRITE_DRIVER_PARAMETERS
		|| command == SET_S_CURVE || command == STOP_MOTOR_WITH_DECEL || command == STORE_SETTINGS_TO_FLASH
		|| command == ON_THE_FLY_TARGET_CHANGE || command == MOVE_ABSOLUTE
		|| command == GET_STEP_N_LOOP_MAX_CONTROL_ATTEMPT || command == GET_STEP_N_LOOP_CORRECTION
		|| command == GET_STEP_N_LOOP_RATIO || command == GET_STEP_N_LOOP_TOLERANCE) {

		return DMX_OK;
	}
	//Binary return commands
	else if (command == READ_DIGITAL_INPUT_ONE || command == READ_DIGITAL_INPUT_TWO
		|| command == GET_DRIVER_DIRECTION_POLARITY || command == GET_UNIQUE_DECELERATION_ENABLE
		|| command == GET_POWER_ENABLED || command == GET_RETURN_ZERO_ENABLE
		|| command == GET_STEP_N_LOOP_ENABLED) {
		return BINARY;
	}
	//28 bit return commands
	else if (command == READ_DELTA || command == GET_ENCODER_VALUE || command == GET_HOME_CORRECTION_AMOUNT
		|| command == GET_LIMIT_CORRECTION_AMOUNT || command == GET_POSITION) {
		return TWENTY_EIGHT_BIT;
	}
	//long return commands
	else if (command == GET_DRIVER_IDLE_CURRENT_SETTING || command == GET_DRIVER_MICROSTEP_SETTING
		|| command == GET_DRIVER_RUN_CURRENT_SETTING) {
		return DMX_LONG;
	}
	//default, return ok. 
	else {
		return DMX_OTHER;
	}
}

/**
* Sends a message with a given input and receieves the response. This blocks until
* both the sending and receiving has completed.
* @param command, the command to send the motor.
* @param input, the input to add to the command.
* @return std::string the reply from the motor.
*/
std::string DMX_ETH_17_Motor::sendReceiveWithInput(std::string command, std::string input) {
	std::string sendString = command + input;
	if (sendCommand(sendString)) {
		return receiveResponse();
	}
	else {
		return std::string("FAIL");
	}
}

/**
* Homes the motor.
*/
void DMX_ETH_17_Motor::home() {
	//TODO: Home the motor. 
}

/**
* Moves the motor to an absolute position based on the position counter.
* @param absPosition, the target absolute position.
*/
std::string DMX_ETH_17_Motor::moveTo(int absPosition) {
	if (isConnected) {
		char buff[50];
		sprintf(buff, "%d", absPosition);
		return sendReceiveWithInput(MOVE_ABSOLUTE, buff);
	}
	return std::string("FAIL");
}

/**
* Moves the motor and blocks until the motor has stopped moving. In other words,
* this function would return until the motor has stopped moving.
* @param absPosition the absolute position to move the motor to.
* @return std::string the response from the motor.
*/
std::string DMX_ETH_17_Motor::moveToAndWait(int absPosition) {
	if (isConnected) {
		char buff[50];
		sprintf(buff, "%d", absPosition);
		std::string resp = sendReceiveWithInput(MOVE_ABSOLUTE, buff);
		int* status = readMotorStatus();
		while (status[0] == 1 || status[1] == 1 || status[2] == 1) {
			status = readMotorStatus();
		}

		return resp;
	}
	else {
		return std::string("FAIL");
	}
}

/**
* Move the motor to an absolute degree position (for reference with the mirror
* assembly, 0 degrees is having the mirror assembly perfectly vertical)
* @param degrees, the absolute degree position to move the motor to.
*/
std::string DMX_ETH_17_Motor::moveToDegrees(int degrees) {
	//TODO: Impletement absolute degree movement. Should be limited to +- 90 or 0 to 
	// 180 degrees. 
	if (isConnected) {
		//power up first. 
		powerUp();
		//calculate how many steps we need to move. 
		double fullRotationRatio = (double)degrees / 360.0f;
		double steps = DMX_PULLEY_RATIO * fullRotationRatio * 10000.0f;
		//move and wait.
		std::string moveResp = moveToAndWait((int)steps);
		return moveResp;
	}
	else {
		return std::string("FAIL");
	}
}

/**
* Move the motor by a certain number of degrees. This will just cause the motor to
* move by a certain amount, corresponding to the degree number. Direction is indicated
* by the sign of the number.
* @param degrees, the number of degrees to move.
*/
std::string DMX_ETH_17_Motor::moveDegrees(int degrees) {
	//check if we're connected first. 
	if (isConnected) {
		//check for power. 
		powerUp();
		//convert to steps. 
		double fullRotationRatio = (double)degrees / 360.0f;
		double steps = DMX_PULLEY_RATIO * fullRotationRatio * 10000.0f;
		//set move mode to incremental, not absolute. 
		std::string resp = sendReceive(SET_MOVE_MODE_INCREMENTAL);
		//execute the movement. 
		std::string moveResp = moveToAndWait((int)steps);
		//set movement mode back to absolute. 
		std::string setMovementResp = sendReceive(ABSOLUTE_MOVE);

		return moveResp;
	}
	else {
		return std::string("FAIL");
	}
}

/**
* Returns the position of the motor. This is from the position counter, not the encoder.
* @return int the position.
*/
int DMX_ETH_17_Motor::getPosition() {
	std::string resp = sendReceive(GET_POSITION);
	int position = stoi(resp);
	return position;
}

/**
* Returns the degree position of the pulley. This should refer to the angle of the
* mirror assembly.
* @return int the degrees with respect to vertical.
*/
int DMX_ETH_17_Motor::getPositionDegrees() {
	int position = getPosition();
	int degrees = (position * 360) / (DMX_PULLEY_RATIO * 10000);
	return degrees;
}
/**
* Checks to see if the motor is powered. This function will return false if the
* motor is not powered or if the motor is not connected to.
* @return bool, true if powered, false otherwise.
*/
bool DMX_ETH_17_Motor::isPowered() {
	if (isConnected) {
		std::string binary = sendReceive(GET_POWER_ENABLED);
		int bin = atoi(binary.c_str());
		return bin == 1;
	}
	else {
		return false;
	}
}

/**
* Powers up the motor if it is not powered up already.
*/
void DMX_ETH_17_Motor::powerUp() {
	if (isConnected) {
		std::string binary = sendReceive(GET_POWER_ENABLED);
		int bin = atoi(binary.c_str());
		if (bin == 0) {
			//not powered up. 
			std::string resp = sendReceive(SET_POWER_ENABLED);
			//TODO: Check response to see if successful. 
		}
	}
}

/**
* Powers down the motor if it is powered up.
*/
void DMX_ETH_17_Motor::powerDown() {
	if (isConnected) {
		std::string binary = sendReceive(GET_POWER_ENABLED);
		int bin = atoi(binary.c_str());
		if (bin == 1) {
			//powered so shut down. 
			std::string resp = sendReceive(SET_POWER_DISABLED);
			//TODO: Check response to see if successful. 
		}
	}
}

/**
* Enables Step N Loop movement algorithm.
*/
void DMX_ETH_17_Motor::enableStepNLoop() {
	if (isConnected) {
		std::string resp = sendReceive(GET_STEP_N_LOOP_ENABLED);
		int bin = atoi(resp.c_str());
		if (bin == 0) {
			//not enabled.
			std::string success = sendReceiveWithInput(SET_STEP_N_LOOP_ENABLED, std::string("1"));
			//TODO: Check response. 
		}
	}
}

/**
* Configures the step n loop algorithm to have the proper ratio value. This value is
* calculated by setting the position and encoder values to 0 and then moving the motor
* 1000 pulses. Then 1000 is divided by the new econder value and this is set as the
* Step N Loop ratio. This is based on page 35 of the DMX ETH Manual.
*/
void DMX_ETH_17_Motor::configureStepNLoop() {
	if (isConnected) {
		//we are connected so enable stepNloop just in case it isn't enabled. 
		enableStepNLoop();
		//set encoder and position values to 0.
		std::string succ = sendReceiveWithInput(SET_ENCODER_VALUE, std::string("0"));
		std::string succTwo = sendReceiveWithInput(SET_POSITION, std::string("0"));

		//move 1000 pulses.
		std::string succThree = sendReceiveWithInput(MOVE_ABSOLUTE, std::string("1000"));

		//Get the motor status. 
		std::string strStatus = sendReceive(GET_STEP_N_LOOP_STATUS);
		//Get the int status. 
		int status = atoi(strStatus.c_str());
		/*
		Status can be between 0 and 13. Only 0 means the motor is idle.
		1 = Moving
		2 = Correcting
		3 = Stopping
		4 = Aborting
		5 = Jogging
		6 = Homing
		7 = Z-Homing
		8 = Correction Range Error
		9 = Correction Attempt Error
		10 = Stall Error
		11 = Limit Error
		12 = N/A, Step N Loop not enabled
		13 = Limit homing.
		*/
		while (status != 0) {
			//update the status. 
			std::string strStatus = sendReceive(GET_STEP_N_LOOP_STATUS);
			status = atoi(strStatus.c_str());
		}

		//read the encoder values. 
		std::string encoderVal = sendReceive(GET_ENCODER_VALUE);
		int val = atoi(encoderVal.c_str());
		int ratio = 1000 / abs(val);
		//put the ratio in a Cstring.
		char input[50];
		sprintf(input, "%d", ratio);
		//set the loop ratio value. 
		std::string succFour = sendReceiveWithInput(SET_STEP_N_LOOP_RATIO, input);
		//TODO: Check for successes with OK response. 
	}
}

/**
* Reads the motor status and puts the bits into an array.
* @return int*, pointer to an array of bits, with length 10. See page 32 of
the motor user manual for more info.
Bit 0 = Motor running at constant speed.
Bit 1 = Motor in acceleration.
Bit 2 = Motor in deceleration.
Bit 3 = Home input switch status.
Bit 4 = Minus limit input switch status.
Bit 5 = Plus limit input switch status.
Bit 6 = Minus limit error.
Bit 7 = Plus limit error.
Bit 8 = Latch input status.
Bit 9 = Z-index status
Bit 10 = TOC time-out status.
*/
int* DMX_ETH_17_Motor::readMotorStatus() {
	MotorStatus status;
	std::string resp = sendReceive(GET_MOTOR_STATUS);
	int num = atoi(resp.c_str());
	return intToBits(num, 11);
}


/**
* Reads the polarity from the motor. This is the polarity for many signals from the
* motor.
* @return int*, pointer to an array of bits, with length 14. See page 34 of the
user manual for more info.
Bit 0 = Reserved
Bit 1 = Direction
Bit 2 = Reserved
Bit 3 = Reserved
Bit 4 = Limit
Bit 5 = Home
Bit 6 = Latch
Bit 7 = Z-Channel Index
Bit 8, 9 = Encoder decoding
00 -> 1X
01 -> 2X
10 -> 4X
Bit 10 = Digital Output
Bit 11 = Digital Input
Bit 12 = Jump to line 0 on error
Bit 13 = Enable Output
*/
int* DMX_ETH_17_Motor::readPolarity() {
	std::string resp = sendReceive(GET_POLARITY);
	int pol = atoi(resp.c_str());
	return intToBits(pol, 14);
}

/**
* Switches the direction of the motor.
*/
void DMX_ETH_17_Motor::switchMotorDirection() {
	int* bits = readPolarity();
	//need to change bit 1.
	bits[1] = bits[1] == 0 ? 1 : 0;
	int num = bitsToInt(bits, 14);
	char input[50];
	sprintf(input, "%d", num);
	std::string succ = sendReceiveWithInput(SET_POLARITY, input);
	//TODO: Check for success. 
}

/**
* Reads a bit from an integer.
* @param number, the number to read from.
* @param byteToRead, the bit position to read from, 0 LSB based.
*/
int DMX_ETH_17_Motor::readBit(int number, int byteToRead) {
	return (number >> byteToRead) & 1;
}

/**
* Turns an integer into an array of bits. Reads the integer bit wise and puts these
* values into an array.
* @param number, the integer number to read from.
* @param numberOfBits, the number of bits that make up the number.
* @return int*, a pointer to the created array of bits, the length of this array will
correspond to {@param numberOfBits}
*/
int* DMX_ETH_17_Motor::intToBits(int number, int numberOfBits) {
	int * arrayOfBits = new int[numberOfBits];
	for (int i = 0; i < numberOfBits; i++) {
		arrayOfBits[i] = readBit(number, i);
	}
	return arrayOfBits;
}

/**
* Converts a array of bits to an integer.
* @param bits, the array of bits (0's and 1's)
* @param numBits, the total number of bits that make up the integer.
* @return int, the integer.
*/
int DMX_ETH_17_Motor::bitsToInt(int bits[], int numBits) {
	int answer = 0;
	for (int i = 0; i < numBits; i++) {
		if (bits[i] != 0) {
			answer += (int)pow((double)2.0, (double)i);
		}
	}
	return answer;
}

/**
* Helper function to get a human readable string of an error.
* @param error, the error from WSAGetLastError()
* @return CString, string representation of error. Cast from LPSTR.
*/
std::string DMX_ETH_17_Motor::getErrorMessage(int error) {

	return std::string("error");
}

DMX_ETH_17_Motor.h

/**
* This class encapsulates the DMX ETH 17-3 Series motor and handles the
* socket management and the sending and receieving of commands.
*
* Example:
*	DMX_ETH_17_Motor mMotor();
*	int main() {
*
*		mMotor.connectMotor();
*		mMotor.sendCommand(JOG_MOTOR_POSITIVE);
*		std::string response = mMotor.receieveResponse();
*
*	}
*/
class DMX_ETH_17_Motor {
public:
	DMX_ETH_17_Motor();
	int connectMotor();
	void disconnectMotor();
	int sendCommand(std::string command);
	std::string receiveResponse();
	std::string sendReceive(std::string command);
	std::string sendReceiveWithInput(std::string command, std::string input);
	std::string getErrorMessage(int error);
	Return_Type getCommandReturnType(std::string command);
	bool getIsConnected();
	void home();
	std::string moveTo(int absPosition);
	std::string moveToAndWait(int absPosition);
	std::string moveToDegrees(int degrees);
	std::string moveDegrees(int degrees);
	int getPosition();
	int getPositionDegrees();
	bool isPowered();
	void powerUp();
	void powerDown();
	void enableStepNLoop();
	void configureStepNLoop();
	int* readMotorStatus();
	int* readPolarity();
	void switchMotorDirection();

private:
	bool isConnected;
	QTcpSocket *mSocket;
	int nPacket;
	char recvBuffer[50];
	void noConnection();
	int readBit(int number, int byteToRead);
	int* intToBits(int number, int numberOfBits);
	int bitsToInt(int * bits, int numBits);
};