0,0 → 1,901 |
#include <math.h> |
#include <stdio.h> |
#include <string.h> |
#include <fcntl.h> /* open */ |
#include <unistd.h> /* exit */ |
#include <sys/ioctl.h> /* ioctl */ |
#include "libusmc.h" |
#include "usmctypes.h" |
|
|
#define USB_ERROR(errCode) (errCode|0x00000000L) |
// Linux kernel errors or'ed with this value |
// to distinguish them from usb errors. |
#define LINKRN_ERROR(errCode) (errCode|0x10000000L) |
#define HIBYTE(w) ((w&0xff00)>>8) |
#define LOBYTE(w) (w&0x00ff) |
#define HIWORD(dw) ((dw&0xffff0000)>>16) |
#define LOWORD(dw) (dw&0x0000ffff) |
#define PACK_WORD(w) (HIBYTE(w)|(LOBYTE(w)<<8)) |
//#define PACK_WORD(w) (LOBYTE(w)|(HIBYTE(w)<<8)) |
#define PACK_DWORD(w) (HIBYTE(HIWORD(w))| \ |
(LOBYTE(HIWORD(w))<<8)| \ |
(HIBYTE(LOWORD(w))<<16)| \ |
(LOBYTE(LOWORD(w))<<24)) |
|
|
// DLL data: |
BOOL g_IsInitialized = FALSE; |
USMC_Devices g_devices; |
USMC_Mode g_deviceMode [32]; |
USMC_Parameters g_deviceParameters [32]; |
USMC_StartParameters g_deviceStartParameters [32]; |
DWORD g_devicesVersions [32]; |
char g_lastErrDesc [256]; // Null-terminated ASCII error description string. |
|
|
// Error string descriptions: |
static char * errDesc [] = { |
"0x???????? ( Unknown )", |
"0x00000000 ( Success )", |
/* "0x00000001 ( USB CRC )", |
"0x00000002 ( USB Bit stuffing )", |
"0x00000003 ( USB Data toggle mismatch )", |
|
"0x00000004 ( USB Stall )", |
"0x00000005 ( USB Device not responding )", |
"0x00000006 ( USB PID check failure )", |
"0x00000007 ( USB Unexpected PID )", |
"0x00000008 ( USB Data overrun )", |
|
"0x00000009 ( USB Data underrun )", |
"0x0000000C ( USB Buffer overrun )", |
"0x0000000D ( USB Buffer underrun )", |
"0x0000000E ( USB Not accessed )", |
"0x0000000F ( USB Not accessed alt )", |
|
"0x00000100 ( USB Isochronous )", |
"0x00000101 ( USB Canceled )", |
"0x00000103 ( USB Not complete )", |
"0x00000104 ( USB Client buffer )",*/ |
"0x14141414 ( USMC Not initialized )", |
//"0x00000006 ( USMC Reffered Device not connected )", |
//"0x00000012 ( DeviceManager error - driver function called with invalid arguments )", |
"0x14141415 ( USMC No devices connected )" |
}; |
|
|
// Internal-use functions prototypes: |
DWORD SetLastErr ( const char * str, DWORD dwErrCode ); |
char * ErrCode2Str ( DWORD dwErrCode ); |
int clamp ( int val, int min, int max ); |
float clampf ( float val, float min, float max ); |
DWORD IOCTRL ( DWORD Device, |
DWORD dwCode, |
PVOID pInBuf, |
DWORD dwInBufSize, |
PVOID pOutBuf, |
DWORD dwOutBufSize ); |
|
|
|
|
// Function: SetLastErr () |
// Argument list: |
// const char * str √ in √ Null-terminated ASCII error description. |
// DWORD dwErrCode - in - optional - error code supplied with description. |
// Return value: |
// DWORD - modified error code. |
// Remarks: |
// The SetLastErr () sets description of last occured error. |
DWORD SetLastErr ( const char * str, DWORD dwErrCode ) |
{ |
if ( dwErrCode == -USMC_ERROR_REF_DEV_DISCONNECTED ) |
sprintf ( ( char * ) g_lastErrDesc, "Error: %s. Code: %s", str, "Reffered device disconnected" ); |
else if ( dwErrCode != 1 ) |
sprintf ( ( char * ) g_lastErrDesc, "Error: %s. Code: %d", str, ( signed int ) dwErrCode ); |
|
//if ( dwErrCode != 1 ) |
// sprintf ( ( char * ) g_lastErrDesc, "Error: %s. Code: %s", str, ErrCode2Str ( dwErrCode ) ); |
//else |
// sprintf ( ( char * ) g_lastErrDesc, "Error: %s.", str ); |
|
|
return dwErrCode; |
} |
|
|
|
|
// Function: ErrCode2Str () |
// Argument list: |
// DWORD dwErrCode √ in √ error code returned by any operation. |
// Return value: |
// char * - string equivalent for specified numerical error code. |
// Remarks: |
// There is no need to free error string obtained with this function: |
// return value points to static dll variable. |
char * ErrCode2Str ( DWORD dwErrCode ) |
{ |
switch ( dwErrCode ) |
{ |
/* case USB_NO_ERROR: |
return errDesc [1]; break; |
case USB_CRC_ERROR: |
return errDesc [2]; break; |
case USB_BIT_STUFFING_ERROR: |
return errDesc [3]; break; |
case USB_DATA_TOGGLE_MISMATCH_ERROR: |
return errDesc [4]; break; |
case USB_STALL_ERROR: |
return errDesc [5]; break; |
case USB_DEVICE_NOT_RESPONDING_ERROR: |
return errDesc [6]; break; |
case USB_PID_CHECK_FAILURE_ERROR: |
return errDesc [7]; break; |
case USB_UNEXPECTED_PID_ERROR: |
return errDesc [8]; break; |
case USB_DATA_OVERRUN_ERROR: |
return errDesc [9]; break; |
case USB_DATA_UNDERRUN_ERROR: |
return errDesc [10]; break; |
case USB_BUFFER_OVERRUN_ERROR: |
return errDesc [11]; break; |
case USB_BUFFER_UNDERRUN_ERROR: |
return errDesc [12]; break; |
case USB_NOT_ACCESSED_ERROR: |
return errDesc [13]; break; |
case USB_NOT_ACCESSED_ALT: |
return errDesc [14]; break; |
case USB_ISOCH_ERROR: |
return errDesc [15]; break; |
case USB_CANCELED_ERROR: |
return errDesc [16]; break; |
case USB_NOT_COMPLETE_ERROR: |
return errDesc [17]; break; |
case USB_CLIENT_BUFFER_ERROR: |
return errDesc [18]; break; |
case USMC_ERROR_NOT_INITIALIZED: |
return errDesc [19]; break; |
case USMC_ERROR_REF_DEV_DISCONNECTED: |
return errDesc [20]; break; |
case LINKRN_ERROR ( ERROR_BAD_ARGUMENTS ): |
return errDesc [21]; break; |
case USMC_ERROR_NO_DEVICES_CONNECTED: |
return errDesc [22]; break;*/ |
default: |
return errDesc [0]; break; |
} |
} |
|
|
|
|
int clamp ( int val, int min, int max ) |
{ |
return val > max ? max : ( val < min ? min : val ); |
} |
|
|
|
|
float clampf ( float val, float min, float max ) |
{ |
return val > max ? max : ( val < min ? min : val ); |
} |
|
|
|
// Function: InitDefaultValues () |
// Initializes structures with default values. |
// Return Value: |
// The InitDefaultValues function has no return values. |
void InitDefaultValues () |
{ |
int i; |
|
for ( i = 0 ; i < 32 ; i++ ) |
{ |
memset ( &g_deviceMode [i], 0, sizeof ( USMC_Mode ) ); |
memset ( &g_deviceParameters [i], 0, sizeof ( USMC_Parameters ) ); |
memset ( &g_deviceStartParameters [i], 0, sizeof ( USMC_StartParameters ) ); |
|
// USMC_Mode defaults: |
g_deviceMode [i].PReg = TRUE; |
g_deviceMode [i].Tr1En = TRUE; |
g_deviceMode [i].Tr2En = TRUE; |
g_deviceMode [i].RotTrOp = TRUE; |
g_deviceMode [i].SyncOUTEn = TRUE; |
g_deviceMode [i].SyncINOp = TRUE; |
g_deviceMode [i].SyncCount = 4; |
|
// USMC_Parameters defaults: |
g_deviceParameters [i].MaxTemp = 70.0f; |
g_deviceParameters [i].AccelT = 200.0f; |
g_deviceParameters [i].DecelT = 200.0f; |
g_deviceParameters [i].BTimeout1 = 500.0f; |
g_deviceParameters [i].BTimeout2 = 500.0f; |
g_deviceParameters [i].BTimeout3 = 500.0f; |
g_deviceParameters [i].BTimeout4 = 500.0f; |
g_deviceParameters [i].BTO1P = 200.0f; |
g_deviceParameters [i].BTO2P = 300.0f; |
g_deviceParameters [i].BTO3P = 400.0f; |
g_deviceParameters [i].BTO4P = 500.0f; |
g_deviceParameters [i].MinP = 500.0f; |
g_deviceParameters [i].BTimeoutR = 500.0f; |
g_deviceParameters [i].LoftPeriod = 500.0f; |
g_deviceParameters [i].RTDelta = 200; |
g_deviceParameters [i].RTMinError = 15; |
g_deviceParameters [i].EncMult = 2.5f; |
g_deviceParameters [i].MaxLoft = 32; |
g_deviceParameters [i].PTimeout = 100.0f; |
g_deviceParameters [i].SynOUTP = 1; |
g_deviceParameters [i].StartPos = 0; |
|
// USMC_StartParameters defaults: |
g_deviceStartParameters [i].SDivisor = 8; |
g_deviceStartParameters [i].LoftEn = TRUE; |
g_deviceStartParameters [i].SlStart = TRUE; |
} |
} |
|
|
|
|
// Function: IOCTRL () |
// Encapsulates DeviceIoControl call ( internal use ). |
// Parameters: |
// [in] Device: device number to call DeviceIoControl on. |
// [in] dwCode: IO Control code for operation. |
// [out] pInBuf: buffer pointer to store returned data in. |
// [in] dwInBufSize: size of in buffer. |
// [in] pOutBuf: buffer pointer to send data from. |
// [in] dwOutBufSize: size of out buffer. |
// Return Value: |
// DWORD; zero means succes, other value - error code. |
// Remarks: |
// Call this function with Device == -1 when devices wasn't enumerated yet. |
// The IOCTRL () function DOES NOT sets error description. This routine must |
// be completed by caller function to specify most appropriate information: where |
// error has occured and with which operation it associated. |
DWORD IOCTRL ( DWORD Device, |
DWORD dwCode, |
PVOID pInBuf, |
DWORD dwInBufSize, |
PVOID pOutBuf, |
DWORD dwOutBufSize ) |
{ |
DWORD deviceNumber; |
char deviceName [128]; // Device name in form Prefix + DeviceNumber + ":\0". |
DWORD dwErr; |
int hFile; |
char * buf; |
|
dwErr = 0; |
|
deviceNumber = Device == -1 ? 0 : Device; |
sprintf ( deviceName, "/dev/usmc%d", deviceNumber ); |
|
hFile = open ( deviceName, 0 ); |
|
if ( hFile == -1 ) |
return -USMC_ERROR_REF_DEV_DISCONNECTED; |
|
|
if ( pInBuf && dwInBufSize ) { |
buf = pInBuf; |
} else if ( pOutBuf && dwOutBufSize ) { |
buf = pOutBuf; |
} |
|
|
dwErr = ioctl ( hFile, |
dwCode, |
buf ); |
|
close ( hFile ); |
|
return dwErr; |
} |
|
|
|
|
DWORD USMC_Init ( USMC_Devices * Str ) |
{ |
int nod; |
DWORD i; |
|
nod = 0; |
Str -> NOD = 0; |
Str -> Serial = Str -> Version = NULL; |
|
if ( !g_IsInitialized ) |
{ |
//InitializeCriticalSection ( &g_Lock ); |
g_IsInitialized = TRUE; |
} |
|
//EnterCriticalSection ( &g_Lock ); |
DWORD dwRes = IOCTRL ( -1, |
IOCTL_GET_NOD, |
( PVOID ) &nod, |
1, NULL, 0 ); |
|
if ( ( int ) dwRes < 0 ) |
return USMC_SUCCESS; // No devices connected, so return NOD = 0. |
else |
Str -> NOD = nod; |
|
|
// Enumerate connected devices: |
Str -> Serial = ( char ** ) malloc ( ( ssize_t ) ( sizeof ( char * ) * 32 ) ); |
Str -> Version = ( char ** ) malloc ( ( ssize_t ) ( sizeof ( char * ) * 32 ) ); |
|
for ( i = 0 ; i < Str -> NOD; i++ ) |
{ |
char * serial = ( char * ) malloc ( ( ssize_t ) 16 ); |
char * version = ( char * ) malloc ( ( ssize_t ) 6 ); |
|
dwRes = IOCTRL ( i, |
IOCTL_GET_SERIAL, |
( PVOID ) serial, |
16, NULL, 0 ); |
|
if ( ( int ) dwRes < 0 ) |
{ |
dwRes = SetLastErr ( "USMC_Init () failed during IOCTRL ( IOCTL_GET_SERIAL ) call", dwRes ); |
free ( serial ); |
free ( version ); |
Str -> NOD = 0; |
Str -> Serial = Str -> Version = NULL; |
|
return dwRes; |
} |
|
|
dwRes = IOCTRL ( i, |
IOCTL_GET_VERSION, |
( PVOID ) version, |
6, NULL, 0 ); |
|
if ( ( int ) dwRes < 0 ) |
{ |
dwRes = SetLastErr ( "USMC_Init () failed during IOCTRL ( IOCTL_GET_VERSION ) call", dwRes ); |
free ( serial ); |
free ( version ); |
Str -> NOD = 0; |
Str -> Serial = Str -> Version = NULL; |
|
return dwRes; |
} |
|
|
Str -> Serial [i] = serial; |
|
( Str -> Version [i] ) = ( char * ) malloc ( ( ssize_t ) 4 ); |
memcpy ( Str -> Version [i], version + 2, ( ssize_t ) 4 ); |
|
//strupr ( Str -> Version [i] ); |
sscanf ( Str -> Version [i], "%X", &g_devicesVersions [i] ); |
} |
|
|
g_devices = *Str; |
InitDefaultValues (); |
|
//LeaveCriticalSection ( &g_Lock ); |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_GetState ( DWORD Device, USMC_State * Str ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_GetState () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
|
STATE_PACKET getStateData; |
|
//EnterCriticalSection ( &g_Lock ); |
DWORD dwRes = IOCTRL ( Device, |
IOCTL_GET_STATE, |
( PVOID ) &getStateData, |
11, NULL, 0 ); |
//LeaveCriticalSection ( &g_Lock ); |
|
if ( ( int ) dwRes < 0 ) |
return SetLastErr ( "USMC_GetState () failed during IOCTRL ( IOCTL_GET_STATE ) call", dwRes ); |
|
|
Str -> AReset = getStateData.AFTRESET; |
Str -> CurPos = ( ( signed int ) getStateData.CurPos ) / 8; |
Str -> CW_CCW = getStateData.CW_CCW; |
Str -> EmReset = getStateData.EMRESET; |
Str -> FullPower = getStateData.REFIN; |
Str -> FullSpeed = getStateData.FULLSPEED; |
Str -> Loft = getStateData.LOFT; |
Str -> Power = getStateData.RESET; |
Str -> RotTr = getStateData.ROTTR; |
Str -> RotTrErr = getStateData.ROTTRERR; |
Str -> RUN = getStateData.RUN; |
/*Str -> SDivisor= See below;*/ |
Str -> SyncIN = getStateData.SYNCIN; |
Str -> SyncOUT = getStateData.SYNCOUT; |
/*Str -> Temp = See below;*/ |
Str -> Trailer1 = getStateData.TRAILER1; |
Str -> Trailer2 = getStateData.TRAILER2; |
/*Str -> Voltage = See below;*/ |
|
Str -> SDivisor = ( BYTE ) ( 1 << ( getStateData.M2 << 1 | getStateData.M1 ) ); |
double t = ( double ) getStateData.Temp; |
|
if ( g_devicesVersions [Device] < 0x2400 ) |
{ |
t = t * 3.3 / 65536.0; |
t = t * 10.0 / ( 5.0 - t ); |
t = ( 1.0 / 298.0 ) + ( 1.0 / 3950.0 ) * log ( t / 10.0 ); |
t = 1.0 / t - 273.0; |
} |
else |
{ |
t = ( ( t * 3.3 * 100.0 / 65536.0 ) - 50.0 ); |
} |
|
|
Str -> Temp = ( float ) t; |
Str -> Voltage = ( float ) ( ( ( double ) getStateData.Voltage ) / 65536.0 * 3.3 * 20.0 ); |
Str -> Voltage = Str -> Voltage < 5.0f ? 0.0f : Str -> Voltage; |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_SaveParametersToFlash ( DWORD Device ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_SaveParametersToFlash () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
//EnterCriticalSection ( &g_Lock ); |
DWORD dwRes = IOCTRL ( Device, |
IOCTL_SAVE_PARAMETERS, |
NULL, 0, NULL, 0 ); |
//LeaveCriticalSection ( &g_Lock ); |
|
|
if ( ( int ) dwRes < 0 ) |
return SetLastErr ( "USMC_SaveParametersToFlash () failed during IOCTRL ( IOCTL_SAVE_PARAMETERS ) call", dwRes ); |
|
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_GetMode ( DWORD Device, USMC_Mode * Str ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_GetMode () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
*Str = g_deviceMode [Device]; |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_SetMode ( DWORD Device, USMC_Mode * Str ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_SetMode () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
MODE_PACKET setModeData; |
// Byte 0: |
setModeData.PMODE = Str -> PMode; |
setModeData.REFINEN = Str -> PReg; |
setModeData.RESETD = Str -> ResetD; |
setModeData.EMRESET = Str -> EMReset; |
setModeData.TR1T = Str -> Tr1T; |
setModeData.TR2T = Str -> Tr2T; |
setModeData.ROTTRT = Str -> RotTrT; |
setModeData.TRSWAP = Str -> TrSwap; |
// Byte 1: |
setModeData.TR1EN = Str -> Tr1En; |
setModeData.TR2EN = Str -> Tr2En; |
setModeData.ROTTREN = Str -> RotTeEn; |
setModeData.ROTTROP = Str -> RotTrOp; |
setModeData.BUTT1T = Str -> Butt1T; |
setModeData.BUTT2T = Str -> Butt2T; |
/*setModeData.BUTSWAP = ...;*/ |
setModeData.RESETRT = Str -> ResetRT; |
// Byte 2: |
setModeData.SNCOUTEN = Str -> SyncOUTEn; |
setModeData.SYNCOUTR = Str -> SyncOUTR; |
setModeData.SYNCINOP = Str -> SyncINOp; |
setModeData.SYNCOPOL = Str -> SyncInvert; |
setModeData.ENCODER = Str -> EncoderEn; |
setModeData.INVENC = Str -> EncoderInv; |
setModeData.RESBENC = Str -> ResBEnc; |
setModeData.RESENC = Str -> ResEnc; |
|
setModeData.SYNCCOUNT = PACK_DWORD ( Str -> SyncCount ); |
|
// Save parameters within DLL for further USMC_GetMode () calls: |
//EnterCriticalSection ( &g_Lock ); |
g_deviceMode [Device] = *Str; |
|
DWORD dwRes = IOCTRL ( Device, |
IOCTL_SET_MODE, |
NULL, 0, |
( PVOID ) &setModeData, 7 ); |
//LeaveCriticalSection ( &g_Lock ); |
|
|
if ( ( int ) dwRes < 0 ) |
return SetLastErr ( "USMC_SetMode () failed during IOCTRL ( IOCTL_SET_MODE ) call", dwRes ); |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_GetParameters ( DWORD Device, USMC_Parameters * Str ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_GetParameters () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
*Str = g_deviceParameters [Device]; |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_SetParameters ( DWORD Device, USMC_Parameters * Str ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_SetParameters () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
PARAMETERS_PACKET setParametersData; |
|
/*=====================*/ |
/* ----Conversion:---- */ |
/*=====================*/ |
setParametersData.DELAY1 = ( BYTE ) clamp ( ( int ) ( Str -> AccelT / 98.0f + 0.5f ), 1, 15 ); |
setParametersData.DELAY2 = ( BYTE ) clamp ( ( int ) ( Str -> DecelT / 98.0f + 0.5f ), 1, 15 ); |
setParametersData.RefINTimeout = ( WORD ) ( clampf ( Str -> PTimeout , 1.0f, 9961.0f ) / 0.152f + 0.5f ); |
setParametersData.BTIMEOUT1 = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout1, 1.0f, 9961.0f ) / 0.152f + 0.5f ) ); |
setParametersData.BTIMEOUT2 = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout2, 1.0f, 9961.0f ) / 0.152f + 0.5f ) ); |
setParametersData.BTIMEOUT3 = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout3, 1.0f, 9961.0f ) / 0.152f + 0.5f ) ); |
setParametersData.BTIMEOUT4 = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeout4, 1.0f, 9961.0f ) / 0.152f + 0.5f ) ); |
setParametersData.BTIMEOUTR = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeoutR, 1.0f, 9961.0f ) / 0.152f + 0.5f ) ); |
setParametersData.BTIMEOUTD = PACK_WORD ( ( WORD ) ( clampf ( Str -> BTimeoutD, 1.0f, 9961.0f ) / 0.152f + 0.5f ) ); |
setParametersData.MINPERIOD = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> MinP , 2.0f, 625.0f ) ) + 0.5f ) ); |
setParametersData.BTO1P = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO1P, 2.0f, 625.0f ) ) + 0.5f ) ); |
setParametersData.BTO2P = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO2P, 2.0f, 625.0f ) ) + 0.5f ) ); |
setParametersData.BTO3P = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO3P, 2.0f, 625.0f ) ) + 0.5f ) ); |
setParametersData.BTO4P = PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> BTO4P, 2.0f, 625.0f ) ) + 0.5f ) ); |
setParametersData.MAX_LOFT = PACK_WORD ( ( WORD ) ( clamp ( Str -> MaxLoft, 1, 1023 ) * 64 ) ); |
|
if ( g_devicesVersions [Device] < 0x2407 ) |
{ |
setParametersData.STARTPOS = 0x00000000L; |
} |
else |
{ |
setParametersData.STARTPOS = PACK_DWORD ( Str -> StartPos * 8 & 0xFFFFFF00 ); |
} |
|
|
setParametersData.RTDelta = PACK_WORD ( ( WORD ) ( clamp ( Str -> RTDelta , 4, 1023 ) * 64 ) ); |
setParametersData.RTMinError = PACK_WORD ( ( WORD ) ( clamp ( Str -> RTMinError, 4, 1023 ) * 64 ) ); |
|
double t = ( double ) clampf ( Str -> MaxTemp, 0.0f, 100.0f ); |
|
if ( g_devicesVersions [Device] < 0x2400 ) |
{ |
t = 10.0 * exp ( 3950.0 * ( 1.0 / ( t + 273.0 ) - 1.0 / 298.0 ) ); |
t = ( ( 5 * t / ( 10 + t ) ) * 65536.0 / 3.3 + 0.5 ); |
} |
else |
{ |
t = ( t + 50.0 ) / 330.0 * 65536.0; |
t = ( t + 0.5f ); |
} |
|
|
setParametersData.MaxTemp = PACK_WORD ( ( WORD ) t ); |
setParametersData.SynOUTP = Str -> SynOUTP; |
setParametersData.LoftPeriod = Str -> LoftPeriod == 0.0f ? 0 : |
PACK_WORD ( ( WORD ) ( 65536.0f - ( 125000.0f / clampf ( Str -> LoftPeriod, 16.0f, 5000.0f ) ) + 0.5f ) ); |
setParametersData.EncVSCP = ( BYTE ) ( Str -> EncMult * 4.0f + 0.5f ); |
//setParametersData.EncVSCP = 1; /////////// |
|
memset ( setParametersData.Reserved, 0, 15 ); |
//unsigned char testBuf [sizeof ( PARAMETERS_PACKET )]; |
//memcpy ( ( void * ) testBuf, ( const void * ) &setParametersData, ( size_t ) sizeof ( PARAMETERS_PACKET ) ); |
/*=======================*/ |
/* ----Deconversion:---- */ |
/*=======================*/ |
Str -> AccelT = setParametersData.DELAY1 * 98.0f; |
Str -> DecelT = setParametersData.DELAY2 * 98.0f; |
Str -> PTimeout = ( float ) ( setParametersData.RefINTimeout * 0.152f ); |
Str -> BTimeout1 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT1 ) * 0.152f ); |
Str -> BTimeout2 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT2 ) * 0.152f ); |
Str -> BTimeout3 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT3 ) * 0.152f ); |
Str -> BTimeout4 = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUT4 ) * 0.152f ); |
Str -> BTimeoutR = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUTR ) * 0.152f ); |
Str -> BTimeoutD = ( float ) ( PACK_WORD ( setParametersData.BTIMEOUTD ) * 0.152f ); |
Str -> MinP = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.MINPERIOD ) ) ); |
Str -> BTO1P = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO1P ) ) ); |
Str -> BTO2P = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO2P ) ) ); |
Str -> BTO3P = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO3P ) ) ); |
Str -> BTO4P = 125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.BTO4P ) ) ); |
Str -> MaxLoft = PACK_WORD ( setParametersData.MAX_LOFT ) / 64; |
|
if( g_devicesVersions [Device] < 0x2407 ) |
{ |
Str -> StartPos = 0x00000000L; |
} |
else |
{ |
Str -> StartPos = PACK_DWORD ( setParametersData.STARTPOS ) / 8; |
} |
|
|
Str -> RTDelta = PACK_WORD ( setParametersData.RTDelta ) / 64; |
Str -> RTMinError = PACK_WORD ( setParametersData.RTMinError ) / 64; |
|
if ( g_devicesVersions [Device] < 0x2400 ) |
{ |
t = ( double )( PACK_WORD ( setParametersData.MaxTemp ) ); |
t = t * 3.3 / 65536.0; |
t = 10.0 * t / ( 5.0 - t ); |
t = ( 1.0 / 298.0 ) + ( 1.0 / 3950.0 ) * log ( t / 10.0 ); |
t = 1.0 / t - 273.0; |
} |
else |
{ |
t = ( double ) ( PACK_WORD ( setParametersData.MaxTemp ) ); |
t = ( t * 3.3 * 100.0 / 65536.0 ) - 50.0; |
} |
|
|
Str -> MaxTemp = ( float ) t; |
|
Str -> SynOUTP = setParametersData.SynOUTP; |
Str -> LoftPeriod = PACK_WORD ( setParametersData.LoftPeriod ) == 0 ? 0.0f : |
125000.0f / ( 65536.0f - ( float ) ( PACK_WORD ( setParametersData.LoftPeriod ) ) ); |
Str -> EncMult = ( ( float ) setParametersData.EncVSCP ) / 4.0f; |
|
// Save parameters within DLL for further USMC_GetParameters () calls: |
//EnterCriticalSection ( &g_Lock ); |
g_deviceParameters [Device] = *Str; |
|
DWORD dwRes = IOCTRL ( Device, |
IOCTL_SET_PARAMETERS, |
NULL, 0, |
( PVOID ) &setParametersData, 57 ); |
//LeaveCriticalSection ( &g_Lock ); |
|
|
if ( ( int ) dwRes < 0 ) |
return SetLastErr ( "USMC_SetParameters () failed during IOCTRL ( IOCTL_SET_PARAMETERS ) call", dwRes ); |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_GetStartParameters ( DWORD Device, USMC_StartParameters * Str ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_GetStartParameters () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
*Str = g_deviceStartParameters [Device]; |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_Start ( DWORD Device, int DestPos, |
float * Speed, |
USMC_StartParameters * Str ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_Start () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
GO_TO_PACKET goToData; |
|
/*=====================*/ |
/* ----Conversion:---- */ |
/*=====================*/ |
goToData.DestPos = ( DWORD ) ( DestPos * 8 ); |
goToData.TimerPeriod = PACK_WORD ( ( WORD ) ( 65536.0f - ( 1000000.0f / clampf ( *Speed, 16.0f, 5000.0f ) ) + 0.5f ) ); |
switch ( Str -> SDivisor ) |
{ |
case 1: |
goToData.M1 = goToData.M2 = 0; |
break; |
case 2: |
goToData.M1 = 1; |
goToData.M2 = 0; |
break; |
case 4: |
goToData.M1 = 0; |
goToData.M2 = 1; |
break; |
case 8: |
goToData.M1 = 1; |
goToData.M2 = 1; |
break; |
} |
//goToData.M1 = Str -> SDivisor && 0x01; |
//goToData.M2 = Str -> SDivisor && 0x02; |
goToData.DEFDIR = Str -> DefDir; |
goToData.LOFTEN = Str -> LoftEn; |
goToData.SLSTRT = Str -> SlStart; |
goToData.WSYNCIN = Str -> WSyncIN; |
goToData.SYNCOUTR = Str -> SyncOUTR; |
goToData.FORCELOFT = Str -> ForceLoft; |
|
|
/*=======================*/ |
/* ----Deconversion:---- */ |
/*=======================*/ |
*Speed = 1000000.0f / ( 65536.0f - ( float ) PACK_WORD ( goToData.TimerPeriod ) ); |
Str -> SDivisor = 1 << ( ( goToData.M2 << 1 ) | goToData.M1 ); |
|
// Save start parameters within DLL for further USMC_GetStartParameters () calls: |
//EnterCriticalSection ( &g_Lock ); |
g_deviceStartParameters [Device] = *Str; |
|
DWORD dwRes = IOCTRL ( Device, |
IOCTL_GO_TO, |
NULL, 0, |
( PVOID ) &goToData, 7 ); |
//LeaveCriticalSection ( &g_Lock ); |
|
|
if ( ( int ) dwRes < 0 ) |
return SetLastErr ( "USMC_Start () failed during IOCTRL ( IOCTL_GO_TO ) call", dwRes ); |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_Stop ( DWORD Device ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_Stop () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
//EnterCriticalSection ( &g_Lock ); |
DWORD dwRes = IOCTRL ( Device, |
IOCTL_STOP_STEP_MOTOR, |
NULL, 0, NULL, 0 ); |
//LeaveCriticalSection ( &g_Lock ); |
|
|
if ( ( int ) dwRes < 0 ) |
return SetLastErr ( "USMC_Stop () failed during IOCTRL ( IOCTL_STOP_STEP_MOTOR ) call", dwRes ); |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_SetCurrentPosition ( DWORD Device, int Position ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_SetCurrentPosition () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
Position *= 8; |
Position &= 0xFFFFFFE0; |
//EnterCriticalSection ( &g_Lock ); |
DWORD dwRes = IOCTRL ( Device, |
IOCTL_SET_CURRENT_POSITION, |
NULL, 0, |
( PVOID ) &Position, 4 ); |
//LeaveCriticalSection ( &g_Lock ); |
|
|
if ( ( int ) dwRes < 0 ) |
return SetLastErr ( "USMC_SetCurrentPosition () failed during IOCTRL ( IOCTL_SET_CURRENT_POSITION ) call", dwRes ); |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
DWORD USMC_GetEncoderState ( DWORD Device, USMC_EncoderState * Str ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_GetEncoderState () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
if ( g_devicesVersions [Device] < 0x2410 ) |
return USMC_SUCCESS; |
|
|
//EnterCriticalSection ( &g_Lock ); |
ENCODER_STATE_PACKET getEncoderStateData; |
|
DWORD dwRes = IOCTRL ( Device, |
IOCTL_GET_ENCODER_STATE, |
( PVOID ) &getEncoderStateData, |
8, NULL, 0 ); |
//LeaveCriticalSection ( &g_Lock ); |
|
|
if ( ( int ) dwRes < 0 ) |
return SetLastErr ( "USMC_GetEncoderState () failed during IOCTRL ( IOCTL_GET_STATE ) call", dwRes ); |
|
|
Str -> ECurPos = getEncoderStateData.ECurPos; |
Str -> EncoderPos = getEncoderStateData.EncPos; |
|
|
return USMC_SUCCESS; |
} |
|
|
|
|
void USMC_GetLastErr ( char * str, ssize_t len ) |
{ |
//EnterCriticalSection ( &g_Lock ); |
if ( strlen ( g_lastErrDesc ) > len ) |
memcpy ( str, g_lastErrDesc, len ); |
else |
strcpy ( str, ( const char * ) g_lastErrDesc ); |
//LeaveCriticalSection ( &g_Lock ); |
} |
|
|
|
|
DWORD USMC_Close ( void ) |
{ |
if ( !g_IsInitialized ) |
return SetLastErr ( "USMC_Close () failed", -USMC_ERROR_NOT_INITIALIZED ); |
|
|
return USMC_SUCCESS; |
} |