package org.reprap.devices;
import java.io.IOException;
import org.reprap.utilities.Debug;
import org.reprap.Device;
import org.reprap.Preferences;
import org.reprap.ReprapException;
import org.reprap.comms.Address;
import org.reprap.comms.Communicator;
import org.reprap.comms.IncomingContext;
import org.reprap.comms.OutgoingMessage;
import org.reprap.comms.IncomingMessage;
import org.reprap.comms.IncomingMessage.InvalidPayloadException;
import org.reprap.comms.messages.IncomingIntMessage;
import org.reprap.comms.messages.OutgoingAddressMessage;
import org.reprap.comms.messages.OutgoingBlankMessage;
import org.reprap.comms.messages.OutgoingByteMessage;
import org.reprap.comms.messages.OutgoingIntMessage;
/**
*
*/
public class GenericStepperMotor extends Device {
/**
* API for firmware
* Activate the stepper motor in forward direction
*/
public static final byte MSG_SetForward = 1;
/**
* Activate the stepper motor in reverse direction
*/
public static final byte MSG_SetReverse = 2;
/**
* Set the stepper motor position (how?)
*/
public static final byte MSG_SetPosition = 3;
/**
* Get the current stepper motor position
*/
public static final byte MSG_GetPosition = 4;
/**
*
*/
public static final byte MSG_Seek = 5;
/**
* Set the motor to idle; this turns the torque off whereas speed = 0 keeps torque on
*/
public static final byte MSG_SetIdle = 6;
/**
* Set notification (?)
*/
public static final byte MSG_SetNotification = 7;
/**
* Set the sync mode (?)
*/
public static final byte MSG_SetSyncMode = 8;
/**
* Calibrate (?)
*/
public static final byte MSG_Calibrate = 9;
/**
* Get the range (?)
*/
public static final byte MSG_GetRange = 10;
/**
* DDAMaster ?
*/
public static final byte MSG_DDAMaster = 11;
/**
* Move on step in forward direction
*/
public static final byte MSG_StepForward = 12;
/**
* Move on step in backward direction
*/
public static final byte MSG_StepBackward = 13;
/**
* Set the power to the stepper motor
*/
public static final byte MSG_SetPower = 14;
/**
* Homereset(??
*/
public static final byte MSG_HomeReset = 16;
/**
*
*/
public static final byte SYNC_NONE = 0;
public static final byte SYNC_SEEK = 1;
public static final byte SYNC_INC = 2;
public static final byte SYNC_DEC = 3;
/**
*
*/
private boolean haveInitialised = false;
private boolean haveSetNotification = false;
private boolean haveCalibrated = false;
/**
* Useful to know what we're called
*/
private String axis;
/**
* Power output limiting (0-100 percent)
*/
private int maxTorque;
/**
* @param communicator
* @param address
* @param prefs
* @param motorId
*/
public GenericStepperMotor(Communicator communicator, Address address, Preferences prefs, int motorId) {
//Address address, int maxTorque
super(communicator, address);
switch(motorId)
{
case 1:
axis = "X";
break;
case 2:
axis = "Y";
break;
case 3:
axis = "Z";
break;
default:
axis = "X";
System.err.println("GenericStepperMotor - dud axis id: " + motorId);
}
this.maxTorque = prefs.loadInt(axis + "Axis" + "Torque(%)");
if(!isAvailable())
return;
}
/**
* @throws IOException
*/
private void initialiseIfNeeded() throws IOException {
if (!haveInitialised) {
haveInitialised = true;
setMaxTorque(maxTorque);
}
}
/**
* Dispose of this object
*/
public void dispose() {
}
/**
* Set the motor speed (or turn it off)
* @param speed A value between -255 and 255.
* @throws ReprapException
* @throws IOException
*/
public void setSpeed(int speed) throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded();
lock();
Debug.d(axis + " axis - setting speed: " + speed);
try {
OutgoingMessage request = new RequestSetSpeed(speed);
sendMessage(request);
}
finally {
unlock();
}
}
/**
* @throws IOException
*/
public void setIdle() throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded();
lock();
Debug.d(axis + " axis - going idle.");
try {
OutgoingMessage request = new RequestSetSpeed();
sendMessage(request);
}
finally {
unlock();
}
}
/**
* @throws IOException
*/
public void stepForward() throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded();
lock();
Debug.d(axis + " axis - stepping forward.");
try {
OutgoingMessage request = new RequestOneStep(true);
sendMessage(request);
}
finally {
unlock();
}
}
/**
* @throws IOException
*/
public void stepBackward() throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded();
lock();
Debug.d(axis + " axis - stepping backward.");
try {
OutgoingMessage request = new RequestOneStep(false);
sendMessage(request);
}
finally {
unlock();
}
}
/**
* @throws IOException
*/
public void resetPosition() throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
setPosition(0);
}
/**
* @param position
* @throws IOException
*/
public void setPosition(int position) throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded();
lock();
Debug.d(axis + " axis - setting position to: " + position);
try {
sendMessage(new RequestSetPosition(position));
}
finally {
unlock();
}
}
/**
* @return current position of the motor
* @throws IOException
*/
public int getPosition() throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return 0;
}
int value;
initialiseIfNeeded();
lock();
Debug.d(axis + " axis - getting position. It is... ");
try {
IncomingContext replyContext = sendMessage(
new OutgoingBlankMessage(MSG_GetPosition));
IncomingIntMessage reply = new RequestPositionResponse(replyContext);
try {
value = reply.getValue();
}
catch (IncomingMessage.InvalidPayloadException ex) {
throw new IOException(ex.getMessage());
}
}
finally {
unlock();
}
Debug.d("..." + value);
return value;
}
/**
* @param speed
* @param position
* @throws IOException
*/
public void seek(int speed, int position) throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded() ;
lock();
Debug.d(axis + " axis - seeking position " + position + " at speed " + speed);
try {
sendMessage(new RequestSeekPosition(speed, position));
}
finally {
unlock();
}
}
/**
* @param speed
* @param position
* @throws IOException
*/
public void seekBlocking(int speed, int position) throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded();
lock();
Debug.d(axis + " axis - seeking-blocking position " + position + " at speed " + speed);
try {
setNotification();
new RequestSeekResponse(this, new RequestSeekPosition(speed, position), 50000);
setNotificationOff();
} catch (Exception e) {
// TODO: Nasty error. But WTF do we do about it?
e.printStackTrace();
} finally {
unlock();
}
}
/**
* @param speed
* @return range of the motor
* @throws IOException
* @throws InvalidPayloadException
*/
public Range getRange(int speed) throws IOException, InvalidPayloadException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return new Range();
}
initialiseIfNeeded() ;
lock();
Debug.d(axis + " axis - getting range.");
try {
if (haveCalibrated) {
IncomingContext replyContext = sendMessage(
new OutgoingBlankMessage(MSG_GetRange));
RequestRangeResponse response = new RequestRangeResponse(replyContext);
return response.getRange();
} else {
setNotification();
IncomingContext replyContext = sendMessage(
new OutgoingByteMessage(MSG_Calibrate, (byte)speed));
RequestRangeResponse response = new RequestRangeResponse(replyContext);
setNotificationOff();
return response.getRange();
}
}
finally {
unlock();
}
}
/**
* @param speed
* @throws IOException
* @throws InvalidPayloadException
*/
public void homeReset(int speed) throws IOException, InvalidPayloadException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded() ;
lock();
Debug.d(axis + " axis - home reset at speed " + speed);
try {
setNotification();
new RequestHomeResetResponse(this, new OutgoingByteMessage(MSG_HomeReset, (byte)speed), 60000);
setNotificationOff();
} finally {
unlock();
}
}
/**
* @param syncType
* @throws IOException
*/
public void setSync(byte syncType) throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded() ;
lock();
Debug.d(axis + " axis - setting sync to " + syncType);
try {
sendMessage(
new OutgoingByteMessage(MSG_SetSyncMode, syncType));
}
finally {
unlock();
}
}
/**
* @param speed
* @param x1
* @param deltaY
* @throws IOException
*/
public void dda(int speed, int x1, int deltaY) throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded() ;
lock();
Debug.d(axis + " axis - dda at speed " + speed + ". x1 = " + x1 + ", deltaY = " + deltaY);
try {
setNotification();
new RequestDDAMasterResponse(this, new RequestDDAMaster(speed, x1, deltaY), 60000);
setNotificationOff();
}
finally {
unlock();
}
}
/**
* @throws IOException
*/
private void setNotification() throws IOException {
initialiseIfNeeded() ;
Debug.d(axis + " axis - setting notification on.");
if (!haveSetNotification) {
sendMessage(new OutgoingAddressMessage(MSG_SetNotification,
getCommunicator().getAddress()));
haveSetNotification = true;
}
}
/**
* @throws IOException
*/
private void setNotificationOff() throws IOException {
initialiseIfNeeded() ;
Debug.d(axis + " axis - setting notification off.");
if (haveSetNotification) {
sendMessage(new OutgoingAddressMessage(MSG_SetNotification, getAddress().getNullAddress()));
haveSetNotification = false;
}
}
/**
*
* @param maxTorque An integer value 0 to 100 representing the maximum torque percentage
* @throws IOException
*/
public void setMaxTorque(int maxTorque) throws IOException {
if(!wasAvailable())
{
Debug.d("Attempting to control or interrogate non-existent axis drive for " + axis);
return;
}
initialiseIfNeeded() ;
if (maxTorque > 100) maxTorque = 100;
double power = maxTorque * 68.0 / 100.0;
byte scaledPower = (byte)power;
lock();
Debug.d(axis + " axis - setting maximum torque to: " + maxTorque);
try {
sendMessage(
new OutgoingByteMessage(MSG_SetPower, scaledPower));
}
finally {
unlock();
}
}
/**
*
*/
protected class RequestPositionResponse extends IncomingIntMessage {
/**
* @param incomingContext
* @throws IOException
*/
public RequestPositionResponse(IncomingContext incomingContext) throws IOException {
super(incomingContext);
}
/* (non-Javadoc)
* @see org.reprap.comms.messages.IncomingIntMessage#isExpectedPacketType(byte)
*/
protected boolean isExpectedPacketType(byte packetType) {
return packetType == MSG_GetPosition;
}
}
/**
*
*/
protected class RequestSeekResponse extends IncomingIntMessage {
/**
* @param device
* @param message
* @param timeout
* @throws IOException
*/
public RequestSeekResponse(Device device, OutgoingMessage message, long timeout) throws IOException {
super(device, message, timeout);
}
/* (non-Javadoc)
* @see org.reprap.comms.messages.IncomingIntMessage#isExpectedPacketType(byte)
*/
protected boolean isExpectedPacketType(byte packetType) {
return packetType == MSG_Seek;
}
}
/**
*
*/
protected class RequestSetPosition extends OutgoingIntMessage {
/**
* @param position
*/
public RequestSetPosition(int position) {
super(MSG_SetPosition, position);
}
}
protected class RequestSetSpeed extends OutgoingMessage {
/**
*
*/
byte [] message;
/**
* The empty constructor will create a message to idle the motor
*/
RequestSetSpeed() {
message = new byte [] { MSG_SetIdle };
}
/**
* Create a message for setting the motor speed.
* @param speed The speed to set the motor to. Note that specifying
* 0 will stop the motor and hold it at 0 and thus still draws
* high current. To idle the motor, use the message created by the
* empty constructor.
*/
RequestSetSpeed(int speed) {
byte command;
if (speed >= 0) {
command = MSG_SetForward;
} else {
command = MSG_SetReverse;
speed = -speed;
}
if (speed > 255) speed = 255;
message = new byte[] { command, (byte)speed };
}
/* (non-Javadoc)
* @see org.reprap.comms.OutgoingMessage#getBinary()
*/
public byte[] getBinary() {
return message;
}
}
/**
*
*/
protected class RequestOneStep extends OutgoingMessage
{
/**
*
*/
byte [] message;
/**
* @param forward
*/
RequestOneStep(boolean forward) {
byte command;
if (forward)
command = MSG_StepForward;
else
command = MSG_StepBackward;
message = new byte[] { command };
}
/* (non-Javadoc)
* @see org.reprap.comms.OutgoingMessage#getBinary()
*/
public byte[] getBinary() {
return message;
}
}
/**
*
*/
protected class RequestSeekPosition extends OutgoingMessage {
/**
*
*/
byte [] message;
/**
* @param speed
* @param position
*/
RequestSeekPosition(int speed, int position) {
message = new byte[] { MSG_Seek,
(byte)speed,
(byte)(position & 0xff),
(byte)((position >> 8) & 0xff)};
}
/* (non-Javadoc)
* @see org.reprap.comms.OutgoingMessage#getBinary()
*/
public byte[] getBinary() {
return message;
}
}
/**
*
*/
protected class RequestDDAMaster extends OutgoingMessage {
byte [] message;
/**
* @param speed
* @param x1
* @param deltaY
*/
RequestDDAMaster(int speed, int x1, int deltaY) {
message = new byte[] { MSG_DDAMaster,
(byte)speed,
(byte)(x1 & 0xff),
(byte)((x1 >> 8) & 0xff),
(byte)(deltaY & 0xff),
(byte)((deltaY >> 8) & 0xff)
};
}
/* (non-Javadoc)
* @see org.reprap.comms.OutgoingMessage#getBinary()
*/
public byte[] getBinary() {
return message;
}
}
/**
*
*/
protected class RequestDDAMasterResponse extends IncomingIntMessage {
/**
* @param device
* @param message
* @param timeout
* @throws IOException
*/
public RequestDDAMasterResponse(Device device, OutgoingMessage message, long timeout) throws IOException {
super(device, message, timeout);
}
/* (non-Javadoc)
* @see org.reprap.comms.messages.IncomingIntMessage#isExpectedPacketType(byte)
*/
protected boolean isExpectedPacketType(byte packetType) {
return packetType == MSG_DDAMaster;
}
}
/**
*
*/
protected class RequestRangeResponse extends IncomingIntMessage {
/**
* @param incomingContext
* @throws IOException
*/
public RequestRangeResponse(IncomingContext incomingContext) throws IOException {
super(incomingContext);
}
/* (non-Javadoc)
* @see org.reprap.comms.messages.IncomingIntMessage#isExpectedPacketType(byte)
*/
protected boolean isExpectedPacketType(byte packetType) {
// We could get this either as an asynchronous notification
// from calibration or by explicit request
return packetType == MSG_GetRange || packetType == MSG_Calibrate;
}
/**
* @return
* @throws InvalidPayloadException
*/
public Range getRange() throws InvalidPayloadException {
byte [] reply = getPayload();
if (reply == null || reply.length != 3)
throw new InvalidPayloadException("Unexpected payload getting range");
Range r = new Range();
r.minimum = 0;
r.maximum = IncomingIntMessage.ConvertBytesToInt(reply[1], reply[2]);
return r;
}
}
/**
*
*/
protected class RequestHomeResetResponse extends IncomingMessage {
/**
* @param device
* @param message
* @param timeout
* @throws IOException
*/
public RequestHomeResetResponse(Device device, OutgoingMessage message, long timeout) throws IOException {
super(device, message, timeout);
}
/* (non-Javadoc)
* @see org.reprap.comms.IncomingMessage#isExpectedPacketType(byte)
*/
protected boolean isExpectedPacketType(byte packetType) {
return packetType == MSG_HomeReset;
}
}
/**
*
*/
public class Range {
/**
*
*/
public int minimum;
/**
*
*/
public int maximum;
}
}