1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
|
#include "simplemotion.h"
#include "simplemotion_private.h"
#include "bufferedmotion.h"
#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 )
{
//value out of range
if(sampleRate<1 || sampleRate>2500)
return recordStatus(handle,SM_ERR_PARAMETER);
newAxis->initialized=smfalse;
newAxis->bushandle=handle;
newAxis->samplerate=sampleRate;
newAxis->deviceAddress=deviceAddress;
newAxis->readParamAddr=readParamAddr;
newAxis->readParamLength=readDataLength;
newAxis->readParamInitialized=smfalse;
newAxis->numberOfDiscardableReturnDataPackets=0;
newAxis->driveClock=0;
newAxis->bufferFill=0;
newAxis->numberOfPendingReadPackets=0;
//discard any existing data in buffer, and to get correct reading of device buffer size
smSetParameter( newAxis->bushandle, newAxis->deviceAddress, SMP_SYSTEM_CONTROL,SMP_SYSTEM_CONTROL_ABORTBUFFERED);
//after abort, we can read the maximum size of data in device buffer
smRead1Parameter(newAxis->bushandle,newAxis->deviceAddress,SMP_BUFFER_FREE_BYTES,&newAxis->bufferLength);
newAxis->bufferFreeBytes=newAxis->bufferLength;
//set input smoothing filter on [CIS] if samplerate is not maximum. with filter samplerates 250,500,750,1000,1250 etc run smooth.
if(sampleRate<2500)
{
if(smRead1Parameter(handle,deviceAddress,SMP_DRIVE_FLAGS,&newAxis->driveFlagsBeforeInit)!=SM_OK)
return getCumulativeStatus(handle);//if error happens in read, dont set the flags
smSetParameter(handle,deviceAddress,SMP_DRIVE_FLAGS,newAxis->driveFlagsBeforeInit|FLAG_USE_INPUT_LP_FILTER);
}
//set buffer execution rate to max so init commands go fast
smSetParameter(handle,deviceAddress,SMP_BUFFERED_CMD_PERIOD,10000/2500);
//set acceleration to "infinite" to avoid modification of user supplied trajectory inside drive
smRead1Parameter(handle,deviceAddress,SMP_TRAJ_PLANNER_ACCEL,&newAxis->driveAccelerationBeforeInit);//store original for restoration
smSetParameter(handle,deviceAddress,SMP_TRAJ_PLANNER_ACCEL,32767);
//set buffer execution rate
smSetParameter(handle,deviceAddress,SMP_BUFFERED_CMD_PERIOD,10000/sampleRate);
//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;
return getCumulativeStatus(handle);
}
/** uninitialize axis from buffered motion, recommended to call this before closing bus so drive's adjusted parameters are restored to originals*/
SM_STATUS smBufferedDeinit( BufferedMotionAxis *axis )
{
smBufferedAbort(axis);
//restore drive in pre-init state
if(axis->initialized==smtrue)
{
smSetParameter(axis->bushandle,axis->deviceAddress,SMP_TRAJ_PLANNER_ACCEL,axis->driveAccelerationBeforeInit);
smSetParameter(axis->bushandle,axis->deviceAddress,SMP_DRIVE_FLAGS,axis->driveFlagsBeforeInit);
}
axis->initialized=smfalse;
axis->readParamInitialized=smfalse;
return getCumulativeStatus(axis->bushandle);
}
/* this also starts buffered motion when it's not running*/
SM_STATUS smBufferedRunAndSyncClocks(BufferedMotionAxis *axis)
{
return smGetBufferClock( axis->bushandle, axis->deviceAddress, &axis->driveClock );
}
SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numBytesFree )
{
smint32 freebytes;
if(smRead1Parameter(axis->bushandle,axis->deviceAddress,SMP_BUFFER_FREE_BYTES,&freebytes)!=SM_OK)
{
*numBytesFree=0;//read has failed, assume 0
return getCumulativeStatus(axis->bushandle);
}
axis->bufferFreeBytes=freebytes;
axis->bufferFill=100*(axis->bufferLength-freebytes)/axis->bufferLength;//calc buffer fill 0-100%
*numBytesFree=freebytes;
return getCumulativeStatus(axis->bushandle);
}
smint32 smBufferedGetMaxFillSize(BufferedMotionAxis *axis, smint32 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)
//*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 )
{
//calculate number of bytes that the number of fill points will consume from buffer
if(axis->readParamInitialized==smtrue)
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 )
{
smint32 bytesUsed=0;
//if(freeBytesInDeviceBuffer>=cmdBufferSizeBytes)
// emit message(Warning,"Buffer underrun on axis "+QString::number(ax));
//freeBytesInDeviceBuffer-=8;
// if(drives[ax].bufferedStreamInitialized==false)
// cmdBufferSizeBytes=freeBytesInDeviceBuffer;//get empty buffer size
//first initialize the stream if not done yet
if(axis->readParamInitialized==smfalse)
{
//set acceleration to "infinite" to avoid modification of user supplied trajectory inside drive
smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_RETURN_PARAM_ADDR);
smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_24B,axis->readParamAddr);
smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_RETURN_PARAM_LEN);
smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_24B,axis->readParamLength);
smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_ABSOLUTE_SETPOINT);
bytesUsed+=2+3+2+3+2;
//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;
}
if(numFillPoints>=1)//send first fill data
{
smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_32B,fillPoints[0]);
bytesUsed+=4;
axis->numberOfPendingReadPackets+=1;
}
//send rest of data as increments
if(numFillPoints>=2)
{
int i;
//smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_ABSOLUTE_SETPOINT);
//smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_INCREMENTAL_SETPOINT);
//axis->numberOfPendingReadPackets++;//FIXME ei toimi ihan oikein tää koska skippaa äskeisen writevaluen
for(i=1;i<numFillPoints;i++)
{
//smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_24B, fillPoints[i]-fillPoints[i-1] );
smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_32B,fillPoints[i]);
bytesUsed+=4;
axis->numberOfPendingReadPackets++;
}
}
//send the commands that were added with smAppendSMCommandToQueue. this also reads all return packets that are available (executed already)
smUploadCommandQueueToDeviceBuffer(axis->bushandle,axis->deviceAddress);
//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;
int n=0;
//read return data buffer
smBytesReceived(axis->bushandle,&bufferedReturnBytesReceived);//get amount of data available
//qDebug()<<axis<<"return data bytes"<<bufferedReturnBytesReceived;
while(bufferedReturnBytesReceived>1)//loop until we have read it all
{
smGetQueuedSMCommandReturnValue(axis->bushandle, &readval);
smBytesReceived(axis->bushandle,&bufferedReturnBytesReceived);
if(axis->numberOfDiscardableReturnDataPackets>0)
{
//discard this return data as it's intialization return packets
axis->numberOfDiscardableReturnDataPackets--;
}
else//its read data that user expects
{
receivedPoints[n]=readval;
n++;
axis->numberOfPendingReadPackets--;
}
}
*numReceivedPoints=n;
}
*bytesFilled=bytesUsed;
return getCumulativeStatus(axis->bushandle);
}
/** 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.*/
SM_STATUS smBufferedAbort(BufferedMotionAxis *axis)
{
return smSetParameter( axis->bushandle, axis->deviceAddress, SMP_SYSTEM_CONTROL,SMP_SYSTEM_CONTROL_ABORTBUFFERED);
}
|
