Skip to content
Open
2 changes: 1 addition & 1 deletion Makefile
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ OBJECTS = $(SOURCES:%.c=%.o)
#user optionsare the ones starting with -D below
#be sure to check also user_options.h for more
CPPFLAGS = -I. -Iutils/ -DENABLE_BUILT_IN_DRIVERS
CFLAGS = -Wall -Wextra -DENABLE_BUILT_IN_DRIVERS -Iutils/
CFLAGS = -std=c99 -Wall -Wextra -DENABLE_BUILT_IN_DRIVERS -Iutils/

all: libsimplemotionv2.a

Expand Down
44 changes: 22 additions & 22 deletions bufferedmotion.c
Original file line number Diff line number Diff line change
Expand Up @@ -5,24 +5,24 @@
#include "sm485.h"

/** initialize buffered motion for one axis with address and samplerate (Hz) */
SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr deviceAddress, smint32 sampleRate, smint16 readParamAddr, smuint8 readDataLength )
SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr deviceAddress, int32_t sampleRate, int16_t readParamAddr, uint8_t readDataLength )
{
//value out of range
if(sampleRate<1 || sampleRate>2500)
return recordStatus(handle,SM_ERR_PARAMETER);

newAxis->initialized=smfalse;
newAxis->initialized=false;
newAxis->bushandle=handle;
newAxis->samplerate=sampleRate;
newAxis->deviceAddress=deviceAddress;
newAxis->readParamAddr=readParamAddr;
newAxis->readParamLength=readDataLength;
newAxis->readParamInitialized=smfalse;
newAxis->readParamInitialized=false;
newAxis->numberOfDiscardableReturnDataPackets=0;
newAxis->driveClock=0;
newAxis->bufferFill=0;
newAxis->numberOfPendingReadPackets=0;
newAxis->driveFlagsModifiedAtInit=smfalse;
newAxis->driveFlagsModifiedAtInit=false;
newAxis->deviceCapabilityFlags1=0;
newAxis->deviceCapabilityFlags2=0;

Expand Down Expand Up @@ -52,7 +52,7 @@ SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr devic
if(sampleRate<2500)
{
smSetParameter(handle,deviceAddress,SMP_DRIVE_FLAGS,newAxis->driveFlagsBeforeInit|FLAG_USE_INPUT_LP_FILTER);
newAxis->driveFlagsModifiedAtInit=smtrue;
newAxis->driveFlagsModifiedAtInit=true;
}
}

Expand All @@ -68,7 +68,7 @@ SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr devic

//FIXME can cause unnecessary initialized=false status if there was error flags in cumulative status before calling this func
if(getCumulativeStatus(handle)==SM_OK)
newAxis->initialized=smtrue;
newAxis->initialized=true;

return getCumulativeStatus(handle);
}
Expand All @@ -79,15 +79,15 @@ SM_STATUS smBufferedDeinit( BufferedMotionAxis *axis )
smBufferedAbort(axis);

//restore drive in pre-init state
if(axis->initialized==smtrue)
if(axis->initialized)
{
smSetParameter(axis->bushandle,axis->deviceAddress,SMP_TRAJ_PLANNER_ACCEL,axis->driveAccelerationBeforeInit);
if(axis->driveFlagsModifiedAtInit==smtrue)//if flags parameter modified, then restore the origianl value
if(axis->driveFlagsModifiedAtInit)//if flags parameter modified, then restore the origianl value
smSetParameter(axis->bushandle,axis->deviceAddress,SMP_DRIVE_FLAGS,axis->driveFlagsBeforeInit);
}

axis->initialized=smfalse;
axis->readParamInitialized=smfalse;
axis->initialized=false;
axis->readParamInitialized=false;

return getCumulativeStatus(axis->bushandle);
}
Expand All @@ -99,9 +99,9 @@ SM_STATUS smBufferedRunAndSyncClocks(BufferedMotionAxis *axis)
return smGetBufferClock( axis->bushandle, axis->deviceAddress, &axis->driveClock );
}

SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numBytesFree )
SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, int32_t *numBytesFree )
{
smint32 freebytes;
int32_t freebytes;

if(smRead1Parameter(axis->bushandle,axis->deviceAddress,SMP_BUFFER_FREE_BYTES,&freebytes)!=SM_OK)
{
Expand All @@ -117,44 +117,44 @@ SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numBytesFree )
return getCumulativeStatus(axis->bushandle);
}

smint32 smBufferedGetMaxFillSize(BufferedMotionAxis *axis, smint32 numBytesFree )
int32_t smBufferedGetMaxFillSize(BufferedMotionAxis *axis, int32_t numBytesFree )
{
//even if we have lots of free space in buffer, we can only send up to SM485_MAX_PAYLOAD_BYTES bytes at once in one SM transmission
if(numBytesFree>SM485_MAX_PAYLOAD_BYTES)
numBytesFree=SM485_MAX_PAYLOAD_BYTES;

//calculate number of points that can be uploaded to buffer (max size SM485_MAX_PAYLOAD_BYTES bytes and fill consumption is 2+4+2+3*(n-1) bytes)
if(axis->readParamInitialized==smtrue)
if(axis->readParamInitialized)
//*numPoints=(freebytes-2-4-2)/3+1;
return numBytesFree/4;
else
//*numPoints=(freebytes-2-4-2 -2-2-2-2)/3+1;//if read data uninitialized, it takes extra 8 bytes to init on next fill, so reduce it here
return (numBytesFree-2-3-2-3-2)/4;//if read data uninitialized, it takes extra n bytes to init on next fill, so reduce it here
}

smint32 smBufferedGetBytesConsumed(BufferedMotionAxis *axis, smint32 numFillPoints )
int32_t smBufferedGetBytesConsumed(BufferedMotionAxis *axis, int32_t numFillPoints )
{
//calculate number of bytes that the number of fill points will consume from buffer
if(axis->readParamInitialized==smtrue)
if(axis->readParamInitialized)
return numFillPoints*4;
else
return numFillPoints*4 +2+3+2+3+2;//if read data uninitialized, it takes extra n bytes to init on next fill, so reduce it here
}


SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoints, smint32 *fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints, smint32 *bytesFilled )
SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, int32_t numFillPoints, int32_t *fillPoints, int32_t *numReceivedPoints, int32_t *receivedPoints, int32_t *bytesFilled )
{
smint32 bytesUsed=0;
int32_t bytesUsed=0;

//if(freeBytesInDeviceBuffer>=cmdBufferSizeBytes)
// emit message(Warning,"Buffer underrun on axis "+QString::number(ax));

//freeBytesInDeviceBuffer-=8;
// if(drives[ax].bufferedStreamInitialized==false)
// if(!drives[ax].bufferedStreamInitialized)
// cmdBufferSizeBytes=freeBytesInDeviceBuffer;//get empty buffer size

//first initialize the stream if not done yet
if(axis->readParamInitialized==smfalse)
if(!axis->readParamInitialized)
{
//set acceleration to "infinite" to avoid modification of user supplied trajectory inside drive
smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_RETURN_PARAM_ADDR);
Expand All @@ -166,7 +166,7 @@ SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoin

//next time we read return data, we discard first 4 return packets to avoid unexpected read data to user
axis->numberOfDiscardableReturnDataPackets+=5;
axis->readParamInitialized=smtrue;
axis->readParamInitialized=true;
}

if(numFillPoints>=1)//send first fill data
Expand Down Expand Up @@ -199,7 +199,7 @@ SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoin
//read all available return data from stream (commands that have been axecuted in drive so far)
//return data works like FIFO for all sent commands (each sent stream command will produce return data packet that we fetch here)
{
smint32 bufferedReturnBytesReceived,readval;
int32_t bufferedReturnBytesReceived,readval;
int n=0;

//read return data buffer
Expand Down
44 changes: 22 additions & 22 deletions bufferedmotion.h
Original file line number Diff line number Diff line change
Expand Up @@ -10,25 +10,25 @@ extern "C"{
//typedef enum _smBufferedState {BufferedStop=0,BufferedRun=1} smBufferedState;

typedef struct _BufferedMotionAxis {
smbool initialized;
smbool readParamInitialized;
smint32 numberOfDiscardableReturnDataPackets;
smint32 numberOfPendingReadPackets;//number of read data packets that should be arriving from device (to read rest of pending data, use smBufferedFillAndReceive(numFillPoints=0) until this variable this goes to zero)
bool initialized;
bool readParamInitialized;
int32_t numberOfDiscardableReturnDataPackets;
int32_t numberOfPendingReadPackets;//number of read data packets that should be arriving from device (to read rest of pending data, use smBufferedFillAndReceive(numFillPoints=0) until this variable this goes to zero)
smbus bushandle;
smaddr deviceAddress;
smint32 samplerate;
smint16 readParamAddr;
smuint8 readParamLength;
smint32 driveFlagsBeforeInit;
smbool driveFlagsModifiedAtInit;//true if deInit should restore driveFlagsBeforeInit
smint32 driveAccelerationBeforeInit;
smuint16 driveClock;//clock counter is updated at smBufferedRunAndSyncClocks only for the one axis that is used with that func. clock is running up at 10kHz count rate, meaning that it rolls over every 6.5536 secs
smint32 bufferLength;//buffer lenght in bytes of the device. note this may be different in different devices types. so call smBufferedGetFree on the device that has the smallest buffer. however as of 2.2016 all GD drives have 2048 bytes buffers.
smint32 bufferFreeBytes;//number of bytes free in buffer, updated at smBufferedGetFree
smint32 bufferFill;//percentage of buffer fill, updated at smBufferedGetFree. this should stay above 50% to ensure gapless motion. if gaps occur, check SMV2USB adpater COM port latency setting (set to 1ms) or try lower samplerate.
smint32 smProtocolVersion;//version of SM protocol of the target device. some internal functionality of API may use this info.
smint32 deviceCapabilityFlags1;//value of SMP_DEVICE_CAPABILITIES1 if target device has SM protocol version 28 or later (if SM version<28, then value is 0)
smint32 deviceCapabilityFlags2;//value of SMP_DEVICE_CAPABILITIES2 if target device has SM protocol version 28 or later (if SM version<28, then value is 0)
int32_t samplerate;
int16_t readParamAddr;
uint8_t readParamLength;
int32_t driveFlagsBeforeInit;
bool driveFlagsModifiedAtInit;//true if deInit should restore driveFlagsBeforeInit
int32_t driveAccelerationBeforeInit;
uint16_t driveClock;//clock counter is updated at smBufferedRunAndSyncClocks only for the one axis that is used with that func. clock is running up at 10kHz count rate, meaning that it rolls over every 6.5536 secs
int32_t bufferLength;//buffer lenght in bytes of the device. note this may be different in different devices types. so call smBufferedGetFree on the device that has the smallest buffer. however as of 2.2016 all GD drives have 2048 bytes buffers.
int32_t bufferFreeBytes;//number of bytes free in buffer, updated at smBufferedGetFree
int32_t bufferFill;//percentage of buffer fill, updated at smBufferedGetFree. this should stay above 50% to ensure gapless motion. if gaps occur, check SMV2USB adpater COM port latency setting (set to 1ms) or try lower samplerate.
int32_t smProtocolVersion;//version of SM protocol of the target device. some internal functionality of API may use this info.
int32_t deviceCapabilityFlags1;//value of SMP_DEVICE_CAPABILITIES1 if target device has SM protocol version 28 or later (if SM version<28, then value is 0)
int32_t deviceCapabilityFlags2;//value of SMP_DEVICE_CAPABILITIES2 if target device has SM protocol version 28 or later (if SM version<28, then value is 0)
} BufferedMotionAxis;

/** initialize buffered motion for one axis with address and samplerate (Hz) */
Expand All @@ -47,17 +47,17 @@ typedef struct _BufferedMotionAxis {
Note return data per one FillAndReceive must not exceed 120 bytes. So max allowed numFillPoints will depend on returnDataLength.
numFillPoints must be equal or below 30 for 32B, 40 for 24B and 60 for 16B.
*/
LIB SM_STATUS smBufferedInit( BufferedMotionAxis *newAxis, smbus handle, smaddr deviceAddress, smint32 sampleRate, smint16 readParamAddr, smuint8 readDataLength );
LIB SM_STATUS smBufferedInit( BufferedMotionAxis *newAxis, smbus handle, smaddr deviceAddress, int32_t sampleRate, int16_t readParamAddr, uint8_t readDataLength );

/** uninitialize axis from buffered motion, recommended to call this before closing bus so drive's adjusted parameters are restored to originals*/
LIB SM_STATUS smBufferedDeinit( BufferedMotionAxis *axis );

/* this also starts buffered motion when it's not running*/
LIB SM_STATUS smBufferedRunAndSyncClocks( BufferedMotionAxis *axis );
LIB SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numBytesFree );
LIB smint32 smBufferedGetMaxFillSize(BufferedMotionAxis *axis, smint32 numBytesFree );
LIB smint32 smBufferedGetBytesConsumed(BufferedMotionAxis *axis, smint32 numFillPoints );
LIB SM_STATUS smBufferedFillAndReceive( BufferedMotionAxis *axis, smint32 numFillPoints, smint32 *fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints, smint32 *bytesFilled );
LIB SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, int32_t *numBytesFree );
LIB int32_t smBufferedGetMaxFillSize(BufferedMotionAxis *axis, int32_t numBytesFree );
LIB int32_t smBufferedGetBytesConsumed(BufferedMotionAxis *axis, int32_t numFillPoints );
LIB SM_STATUS smBufferedFillAndReceive( BufferedMotionAxis *axis, int32_t numFillPoints, int32_t *fillPoints, int32_t *numReceivedPoints, int32_t *receivedPoints, int32_t *bytesFilled );
/** This will stop executing buffered motion immediately and discard rest of already filled buffer on a given axis. May cause drive fault state such as tracking error if done at high speed because stop happens without deceleration.
Note: this will not stop motion, but just stop executing the sent buffered commands. The last executed motion point will be still followed by drive. So this is bad function
for quick stopping stopping, for stop to the actual place consider using disable drive instead (prefferably phsyical input disable).
Expand Down
Loading