caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Got simple package compiling with both ros and simplemotion included.

Browse Source
This commit is contained in:
Corwin Perren
2018-04-21 19:45:37 -07:00
parent acbe86bd07
commit a2ad3c2c9a
15 changed files with 3033 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
software/ros_packages/rover_arm/CMakeLists.txt
View File

@@ -9,7 +9,7 @@ add_compile_options(-std=c++11)
## is used, also find other catkin packages ## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS find_package(catkin REQUIRED COMPONENTS
roscpp roscpp
message_generation #message_generation
std_msgs std_msgs
) )
@@ -47,9 +47,9 @@ find_package(catkin REQUIRED COMPONENTS
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder ## Generate messages in the 'msg' folder
add_message_files( #add_message_files(
FILES # FILES
) #)
## Generate services in the 'srv' folder ## Generate services in the 'srv' folder
# add_service_files( # add_service_files(
@@ -66,10 +66,10 @@ find_package(catkin REQUIRED COMPONENTS
# ) # )
## Generate added messages and services with any dependencies listed here ## Generate added messages and services with any dependencies listed here
generate_messages( #generate_messages(
DEPENDENCIES # DEPENDENCIES
std_msgs # std_msgs
) #)
################################################ ################################################
## Declare ROS dynamic reconfigure parameters ## ## Declare ROS dynamic reconfigure parameters ##
@@ -120,12 +120,11 @@ catkin_package(
include_directories( include_directories(
# include # include
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
simplemotion
) )
## Declare a C++ library ## Declare a C++ library
# add_library(${PROJECT_NAME} add_subdirectory(src/simplemotion)
# src/${PROJECT_NAME}/rover_arm.cpp
# )
## Add cmake target dependencies of the library ## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries ## as an example, code may need to be generated before libraries
@@ -148,9 +147,11 @@ add_executable(${PROJECT_NAME} src/rover_arm.cpp)
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against ## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node target_link_libraries(
# ${catkin_LIBRARIES} ${PROJECT_NAME}
# ) ${catkin_LIBRARIES}
simplemotion
)
############# #############
## Install ## ## Install ##

23
software/ros_packages/rover_arm/src/rover_arm.cpp
View File

@@ -4,6 +4,27 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <ros/console.h> #include <ros/console.h>
#include "simplemotion/simplemotion.h"
using namespace std;
int device_address = 1;
int main(int argc, char** argv){ int main(int argc, char** argv){
std::cout << "Test" << std::endl; smbus busHandle = smOpenBus("/dev/ttyUSB0");
if (busHandle >= 0) {
cout << "Successfully connected bus" << endl;
// deviceAddress=ui->deviceAddress->value();
} else
cout << "Couldn't connect to bus";
while(1){
smSetParameter(busHandle, device_address, SMP_FAULTS, 0);
smSetParameter(busHandle, device_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
smSetParameter(busHandle, device_address, SMP_ABSOLUTE_SETPOINT, 1000);
cout << "Test" << endl;
}
} }

3
software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt Normal file
View File

@@ -0,0 +1,3 @@
add_library(simplemotion rs232.c simplemotion.c busdevice.c sm_consts.c)
target_include_directories (simplemotion PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

71
software/ros_packages/rover_arm/src/simplemotion/bufferedmotion.h Normal file
View File

@@ -0,0 +1,71 @@
#ifndef BUFFEREDMOTION_H
#define BUFFEREDMOTION_H
#ifdef __cplusplus
extern "C"{
#endif
#include "simplemotion.h"
//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)
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)
} BufferedMotionAxis;
/** initialize buffered motion for one axis with address and samplerate (Hz) */
/* returnDataLenght must be one of following:
#define SM_RETURN_STATUS 3
//return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR
//consumes 2 byte from payload buffer. can contain value up to 14 bits. value greater than 14 bits is clipped (padded with 0s)
#define SM_RETURN_VALUE_16B 2
//return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR
//consumes 3 byte from payload buffer. can contain value up to 14 bits. value greater than 22 bits is clipped (padded with 0s)
#define SM_RETURN_VALUE_24B 1
//return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR
//consumes 4 byte from payload buffer. can contain value up to 30 bits. value greater than 14 bits is clipped (padded with 0s)
#define SM_RETURN_VALUE_32B 0
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 );
/** 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 );
/** 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).
*/
LIB SM_STATUS smBufferedAbort(BufferedMotionAxis *axis);
#ifdef __cplusplus
}
#endif
#endif // BUFFEREDMOTION_H

173
software/ros_packages/rover_arm/src/simplemotion/busdevice.c Normal file
View File

@@ -0,0 +1,173 @@
#include "busdevice.h"
#include "rs232.h"
#include <string.h>
#define BD_NONE 0
#define BD_RS 1
#define BD_FTDI 2
//how much bytes available in transmit buffer
#define TANSMIT_BUFFER_LENGTH 128
typedef struct _SMBusDevice
{
//common
smint8 bdType;//bus device type (such as rs232 or ftdi lib or mcu UART etc). 1=rs232 lib
smbool opened;
SM_STATUS cumulativeSmStatus;
//used for rs232 lib only
int comPort;
smuint8 txBuffer[TANSMIT_BUFFER_LENGTH];
smint32 txBufferUsed;//how many bytes in buffer currently
//used for FTDI lib only
} SMBusDevice;
//init on first open
smbool bdInitialized=smfalse;
SMBusDevice BusDevice[SM_MAX_BUSES];
//init device struct table
void smBDinit()
{
int i;
for(i=0;i<SM_MAX_BUSES;i++)
{
BusDevice[i].bdType=BD_NONE;
BusDevice[i].opened=smfalse;
BusDevice[i].txBufferUsed=0;
}
bdInitialized=smtrue;
}
//ie "COM1" "VSD2USB"
//return -1 if fails, otherwise handle number
smbusdevicehandle smBDOpen( const char *devicename )
{
int handle;
//true on first call
if(bdInitialized==smfalse)
smBDinit();
//find free handle
for(handle=0;handle<SM_MAX_BUSES;handle++)
{
if(BusDevice[handle].opened==smfalse) break;//choose this
}
//all handles in use
if(handle>=SM_MAX_BUSES) return -1;
if(strncmp(devicename,"COM",3) == 0 || strncmp(devicename,"/dev/tty",8) == 0) //use rs232 lib
{
BusDevice[handle].comPort=OpenComport( devicename, SM_BAUDRATE );
if( BusDevice[handle].comPort == -1 )
{
return -1; //failed to open
}
BusDevice[handle].bdType=BD_RS;
BusDevice[handle].txBufferUsed=0;
}
else//no other bus types supproted yet
{
return -1;
}
//success
BusDevice[handle].cumulativeSmStatus=0;
BusDevice[handle].opened=smtrue;
return handle;
}
smbool smIsBDHandleOpen( const smbusdevicehandle handle )
{
if(handle<0) return smfalse;
if(handle>=SM_MAX_BUSES) return smfalse;
return BusDevice[handle].opened;
}
//return true if ok
smbool smBDClose( const smbusdevicehandle handle )
{
//check if handle valid & open
if( smIsBDHandleOpen(handle)==smfalse ) return smfalse;
if( BusDevice[handle].bdType==BD_RS )
{
CloseComport( BusDevice[handle].comPort );
BusDevice[handle].opened=smfalse;
return smtrue;
}
return smfalse;
}
//write one byte to buffer and send later with smBDTransmit()
//returns true on success
smbool smBDWrite(const smbusdevicehandle handle, const smuint8 byte )
{
//check if handle valid & open
if( smIsBDHandleOpen(handle)==smfalse ) return smfalse;
if( BusDevice[handle].bdType==BD_RS )
{
if(BusDevice[handle].txBufferUsed<TANSMIT_BUFFER_LENGTH)
{
//append to buffer
BusDevice[handle].txBuffer[BusDevice[handle].txBufferUsed]=byte;
BusDevice[handle].txBufferUsed++;
return smtrue;
}
else
return smfalse;
}
return smfalse;
}
smbool smBDTransmit(const smbusdevicehandle handle)
{
//check if handle valid & open
if( smIsBDHandleOpen(handle)==smfalse ) return smfalse;
if( BusDevice[handle].bdType==BD_RS )
{
if(SendBuf(BusDevice[handle].comPort,BusDevice[handle].txBuffer, BusDevice[handle].txBufferUsed)==BusDevice[handle].txBufferUsed)
{
BusDevice[handle].txBufferUsed=0;
return smtrue;
}
else
{
BusDevice[handle].txBufferUsed=0;
return smfalse;
}
}
return smfalse;
}
//read one byte from bus. if byte not immediately available, block return up to SM_READ_TIMEOUT millisecs to wait data
//returns true if byte read sucessfully
smbool smBDRead( const smbusdevicehandle handle, smuint8 *byte )
{
//check if handle valid & open
if( smIsBDHandleOpen(handle)==smfalse ) return smfalse;
if( BusDevice[handle].bdType==BD_RS )
{
int n;
n=PollComport(BusDevice[handle].comPort, byte, 1);
if( n!=1 ) return smfalse;
else return smtrue;
}
return smfalse;
}

35
software/ros_packages/rover_arm/src/simplemotion/busdevice.h Normal file
View File

@@ -0,0 +1,35 @@
//HW interface to phyiscal bus device
#ifndef SM_BUSDEVICE
#define SM_BUSDEVICE
#include "simplemotion.h"
#include "simplemotion_private.h"
typedef smint8 smbusdevicehandle;
#define SM_BAUDRATE 460800
//ie "COM1" "VSD2USB"
//return 0-1 if fails, otherwise handle number
smbusdevicehandle smBDOpen( const char *devicename );
//return true if ok
smbool smBDClose( const smbusdevicehandle handle );
//write one byte to trasmit buffer. send data with smBDTransmit()
//returns smtrue on success. smfalse could mean buffer full error if forgot to call smBDTransmit
smbool smBDWrite( const smbusdevicehandle handle , const smuint8 byte );
//write transmit buffer to physical device
//returns true on success
smbool smBDTransmit(const smbusdevicehandle handle);
//read one byte from bus. if byte not immediately available, block return up to SM_READ_TIMEOUT millisecs to wait data
//returns true if byte read sucessfully
smbool smBDRead( const smbusdevicehandle handle , smuint8 *byte );
#endif

123
software/ros_packages/rover_arm/src/simplemotion/devicedeployment.h Normal file
View File

@@ -0,0 +1,123 @@
/* Device deployment library based on SimpleMotionV2 lib. Features:
*
* - install .gdf format firmware to drive
* - load .drc settings file to drive
* - read installed firmware binary checksum from device - this may be used to verify that exact correct FW build is installed in the drive
*
* TODO:
* - Support Argon. Currently tested only on IONI/ATOMI series drives. Currently FW upgrade on Argon will fail, but settings load may work.
* - Add some way of reading binary checksum from .gdf file or settings file. Now user must read it from device and store that number somewhere manually.
*/
#ifndef SMDEPLOYMENTTOOL_H
#define SMDEPLOYMENTTOOL_H
#ifdef WIN32
//dll specs
#ifdef BUILD_DLL
#define LIB __declspec(dllexport)
#else
// #define LIB __declspec(dllimport)
#define LIB
#endif
#else
#define LIB
#endif
#include "simplemotion.h"
#ifdef __cplusplus
extern "C"{
#endif
typedef enum
{
FWComplete=100,
FWInvalidFile=-1,
FWConnectionError=-2,
FWIncompatibleFW=-3,
FWConnectionLoss=-4,
FWUnsupportedTargetDevice=-5,
FWFileNotReadable=-6,
FWConnectingDFUModeFailed=-7
} FirmwareUploadStatus;
/**
* @brief smFirmwareUpload Sets drive in firmware upgrade mode if necessary and uploads a new firmware. Call this many until it returns value 100 (complete) or a negative value (error).
* @param smhandle SM bus handle, must be opened before call
* @param smaddress Target SM device address
* @param filename .gdf file name
* @return Enum FirmwareUploadStatus that indicates errors or Complete status. Typecast to integer to get progress value 0-100.
*/
LIB FirmwareUploadStatus smFirmwareUpload(const smbus smhandle, const int smaddress, const char *firmware_filename );
/**
* @brief smFirmwareUpload Sets drive in firmware upgrade mode if necessary and uploads a new firmware. Call this many until it returns value 100 (complete) or a negative value (error).
* @param smhandle SM bus handle, must be opened before call
* @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255.
* @param fwData pointer to memory address where .gdf file contents are loaded. Note: on some architectures (such as ARM Cortex M) fwData must be aligned to nearest 4 byte boundary to avoid illegal machine instructions.
* @param fwDataLenght number of bytes in fwData
* @return Enum FirmwareUploadStatus that indicates errors or Complete status. Typecast to integer to get progress value 0-100.
*/
FirmwareUploadStatus smFirmwareUploadFromBuffer( const smbus smhandle, const int smaddress, smuint8 *fwData, const int fwDataLength );
typedef enum
{
CFGComplete=100,
CFGInvalidFile=-1,
CFGCommunicationError=-2,
CFGIncompatibleFW=-4,
CFGUnsupportedTargetDevice=-5,
CFGUnableToOpenFile=-6
} LoadConfigurationStatus;
//TODO implement: #define CONFIGMODE_REQUIRE_SAME_FW 1 //will return IncompatibleFW if firmware checksum does not match the one in .drc files. if this error is returned, perform smFirmwareUpload and perform smLoadConfiguration again. Requires DRC file version 111 or later (if not met, returns InvalidFile).
#define CONFIGMODE_ALWAYS_RESTART_TARGET 2 //will perform device restart after setup even when it's not required
#define CONFIGMODE_DISABLE_DURING_CONFIG 4 //will set device in disabled state during configuration
#define CONFIGMODE_CLEAR_FAULTS_AFTER_CONFIG 8 //will perform clear faults command after configuration
/**
* @brief smConfigureParameters Configures all target device parameters from file and performs device restart if necessary. This can take few seconds to complete. This may take 2-5 seconds to call.
* @param smhandle SM bus handle, must be opened before call
* @param smaddress Target SM device address
* @param filename .DRC file name
* @param mode Combined from CONFIGMODE_ define bits (can logic OR mutliple values).
* @return Enum LoadConfigurationStatus
*
* Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW.
*/
LIB LoadConfigurationStatus smLoadConfiguration( const smbus smhandle, const int smaddress, const char *filename, unsigned int mode, int *skippedCount, int *errorCount );
/**
* @brief smConfigureParametersFromBuffer Same as smConfigureParameters but reads data from user specified memory address instead of file. Configures all target device parameters from file and performs device restart if necessary. This can take few seconds to complete. This may take 2-5 seconds to call.
* @param smhandle SM bus handle, must be opened before call
* @param smaddress Target SM device address
* @param drcData Pointer to to a memory where .drc file is loaded
* @param drcDataLen Number of bytes available in the drcData buffer
* @param mode Combined from CONFIGMODE_ define bits (can logic OR mutliple values).
* @return Enum LoadConfigurationStatus
*
* Requires DRC file version 111 or later to use CONFIGMODE_REQUIRE_SAME_FW.
*/
LIB LoadConfigurationStatus smLoadConfigurationFromBuffer(const smbus smhandle, const int smaddress, const smuint8 *drcData, const int drcDataLength, unsigned int mode, int *skippedCount, int *errorCount );
/**
* @brief smGetDeviceFirmwareUniqueID Reads installed firmware binary checksum that can be used to verify whether a wanted FW version is installed
* @param smhandle SM bus handle, must be opened before call
* @param smaddress Target SM device address. Can be device in DFU mode or main operating mode. For Argon, one device in a bus must be started into DFU mode by DIP switches and smaddress must be set to 255.
* @param UID result will be written to this pointer
* @return smtrue if success, smfalse if failed (if communication otherwise works, then probably UID feature not present in this firmware version)
*/
smbool smGetDeviceFirmwareUniqueID( smbus smhandle, int deviceaddress, smuint32 *UID );
#ifdef __cplusplus
}
#endif
#endif // SMDEPLOYMENTTOOL_H

366
software/ros_packages/rover_arm/src/simplemotion/rs232.c Normal file
View File

@@ -0,0 +1,366 @@
/*
***************************************************************************
*
* Author: Teunis van Beelen
*
* Copyright (C) 2005, 2006, 2007, 2008, 2009 Teunis van Beelen
*
* teuniz@gmail.com
*
***************************************************************************
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation version 2 of the License.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*
***************************************************************************
*
* This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
*
***************************************************************************
*/
#include "rs232.h"
#include "simplemotion_private.h"
#ifdef __linux__ /* Linux */
int OpenComport(const char * comport_name, int baudrate)
{
int error;
int handle;
int baudr;
struct termios new_port_settings;
switch(baudrate)
{
case 50 : baudr = B50;
break;
case 75 : baudr = B75;
break;
case 110 : baudr = B110;
break;
case 134 : baudr = B134;
break;
case 150 : baudr = B150;
break;
case 200 : baudr = B200;
break;
case 300 : baudr = B300;
break;
case 600 : baudr = B600;
break;
case 1200 : baudr = B1200;
break;
case 1800 : baudr = B1800;
break;
case 2400 : baudr = B2400;
break;
case 4800 : baudr = B4800;
break;
case 9600 : baudr = B9600;
break;
case 19200 : baudr = B19200;
break;
case 38400 : baudr = B38400;
break;
case 57600 : baudr = B57600;
break;
case 115200 : baudr = B115200;
break;
case 230400 : baudr = B230400;
break;
case 460800 : baudr = B460800;
break;
case 500000 : baudr = B500000;
break;
case 576000 : baudr = B576000;
break;
case 921600 : baudr = B921600;
break;
case 1000000 : baudr = B1000000;
break;
default : printf("invalid baudrate\n");
return(1);
break;
}
//Cport[comport_number] = open(comports[comport_number], O_RDWR | O_NOCTTY | O_NDELAY);
handle = open(comport_name, O_RDWR | O_NOCTTY );
if(handle==-1)
{
perror("unable to open comport ");
return(handle);
}
memset(&new_port_settings, 0, sizeof(new_port_settings)); /* clear the new struct */
new_port_settings.c_cflag = baudr | CS8 | CLOCAL | CREAD;
new_port_settings.c_iflag = IGNPAR;
new_port_settings.c_oflag = 0;
new_port_settings.c_lflag = 0;
new_port_settings.c_cc[VMIN] = 0; /* block untill n bytes are received */
new_port_settings.c_cc[VTIME] = readTimeoutMs/100; /* block untill a timer expires (n * 100 mSec.) */
error = tcsetattr(handle, TCSANOW, &new_port_settings);
if(error==-1)
{
close(handle);
perror("unable to adjust portsettings ");
return(-1);
}
return(handle);
}
int PollComport(int comport_number, unsigned char *buf, int size)
{
int n;
#ifndef __STRICT_ANSI__ /* __STRICT_ANSI__ is defined when the -ansi option is used for gcc */
if(size>SSIZE_MAX) size = (int)SSIZE_MAX; /* SSIZE_MAX is defined in limits.h */
#else
if(size>4096) size = 4096;
#endif
n = read(comport_number, buf, size);
return(n);
}
int SendByte(int comport_number, unsigned char byte)
{
int n;
n = write(comport_number, &byte, 1);
if(n<0) return(1);
return(0);
}
int SendBuf(int comport_number, unsigned char *buf, int size)
{
return(write(comport_number, buf, size));
}
void CloseComport(int comport_number)
{
close(comport_number);
//feature removed, not restorint old settings :
//tcsetattr(comport_number, TCSANOW, old_port_settings + comport_number);
}
/*
Constant Description
TIOCM_LE DSR (data set ready/line enable)
TIOCM_DTR DTR (data terminal ready)
TIOCM_RTS RTS (request to send)
TIOCM_ST Secondary TXD (transmit)
TIOCM_SR Secondary RXD (receive)
TIOCM_CTS CTS (clear to send)
TIOCM_CAR DCD (data carrier detect)
TIOCM_CD Synonym for TIOCM_CAR
TIOCM_RNG RNG (ring)
TIOCM_RI Synonym for TIOCM_RNG
TIOCM_DSR DSR (data set ready)
*/
int IsCTSEnabled(int comport_number)
{
int status;
status = ioctl(comport_number, TIOCMGET, &status);
if(status&TIOCM_CTS) return(1);
else return(0);
}
#else /* windows */
#ifdef DEBUG_COMPORT
//for finding problem with windows calls
void print_win32_system_error(char *name) {
// Retrieve, format, and print out a message from the last error. The
// `name' that's passed should be in the form of a present tense noun
// (phrase) such as "opening file".
//
char *ptr = NULL;
FormatMessage(
FORMAT_MESSAGE_ALLOCATE_BUFFER |
FORMAT_MESSAGE_FROM_SYSTEM,
0,
GetLastError(),
0,
(char *)&ptr,
1024,
NULL);
fprintf(stderr, "\nError %s: %ls\n", name, ptr);
LocalFree(ptr);
}
#endif
int OpenComport(const char * comport_name, int baudrate)
{
char baudr[64], portname[64];
HANDLE handle;
switch(baudrate)
{
case 110 : strcpy(baudr, "baud=110 data=8 parity=N stop=1");
break;
case 300 : strcpy(baudr, "baud=300 data=8 parity=N stop=1");
break;
case 600 : strcpy(baudr, "baud=600 data=8 parity=N stop=1");
break;
case 1200 : strcpy(baudr, "baud=1200 data=8 parity=N stop=1");
break;
case 2400 : strcpy(baudr, "baud=2400 data=8 parity=N stop=1");
break;
case 4800 : strcpy(baudr, "baud=4800 data=8 parity=N stop=1");
break;
case 9600 : strcpy(baudr, "baud=9600 data=8 parity=N stop=1");
break;
case 19200 : strcpy(baudr, "baud=19200 data=8 parity=N stop=1");
break;
case 38400 : strcpy(baudr, "baud=38400 data=8 parity=N stop=1");
break;
case 57600 : strcpy(baudr, "baud=57600 data=8 parity=N stop=1");
break;
case 115200 : strcpy(baudr, "baud=115200 data=8 parity=N stop=1");
break;
case 128000 : strcpy(baudr, "baud=128000 data=8 parity=N stop=1");
break;
case 256000 : strcpy(baudr, "baud=256000 data=8 parity=N stop=1");
break;
case 460800 : strcpy(baudr, "baud=460800 data=8 parity=N stop=1");
break;
default : printf("invalid baudrate\n");
return(-1);
break;
}
strcpy(portname,"\\\\.\\");
strcat(portname,comport_name);
handle = CreateFileA(portname,
GENERIC_READ|GENERIC_WRITE,
0, /* no share */
NULL, /* no security */
OPEN_EXISTING,
0, /* no threads */
NULL); /* no templates */
if(handle==INVALID_HANDLE_VALUE)
{
printf("unable to open comport\n");
return(-1);
}
DCB port_settings;
memset(&port_settings, 0, sizeof(port_settings)); /* clear the new struct */
port_settings.DCBlength = sizeof(port_settings);
if(!BuildCommDCBA(baudr, &port_settings))
{
printf("unable to set comport dcb settings\n");
CloseHandle(handle);
return(-1);
}
if(!SetCommState(handle, &port_settings))
{
printf("unable to set comport cfg settings\n");
CloseHandle(handle);
return(-1);
}
COMMTIMEOUTS Cptimeouts;
Cptimeouts.ReadIntervalTimeout = 0;
Cptimeouts.ReadTotalTimeoutMultiplier = 0;
Cptimeouts.ReadTotalTimeoutConstant = readTimeoutMs;
Cptimeouts.WriteTotalTimeoutMultiplier = 50;
Cptimeouts.WriteTotalTimeoutConstant = 50;
if(!SetCommTimeouts(handle, &Cptimeouts))
{
printf("unable to set comport time-out settings\n");
CloseHandle(handle);
return(-1);
}
return( (int)handle);
}
int PollComport(int comport_number, unsigned char *buf, int size)
{
int n;
if(size>4096) size = 4096;
/* added the void pointer cast, otherwise gcc will complain about */
/* "warning: dereferencing type-punned pointer will break strict aliasing rules" */
ReadFile((HANDLE)comport_number, buf, size, (LPDWORD)((void *)&n), NULL);
return(n);
}
int SendByte(int comport_number, unsigned char byte)
{
int n;
WriteFile((HANDLE)comport_number, &byte, 1, (LPDWORD)((void *)&n), NULL);
if(n<0) return(1);
return(0);
}
int SendBuf(int comport_number, unsigned char *buf, int size)
{
int n;
if(WriteFile((HANDLE)comport_number, buf, size, (LPDWORD)((void *)&n), NULL))
{
return(n);
}
return(-1);
}
void CloseComport(int comport_number)
{
CloseHandle((HANDLE)comport_number);
}
#endif

81
software/ros_packages/rover_arm/src/simplemotion/rs232.h Normal file
View File

@@ -0,0 +1,81 @@
/*
***************************************************************************
*
* Author: Teunis van Beelen
*
* Copyright (C) 2005, 2006, 2007, 2008, 2009 Teunis van Beelen
*
* teuniz@gmail.com
*
***************************************************************************
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation version 2 of the License.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*
***************************************************************************
*
* This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
*
***************************************************************************
*/
/* libraray modified for Simplemotion, major changes:
-OpenComport now returns actual file handle that must be passed to rx/tx functions, or -1 if fails
Todo:
-Restore port settings at CloseComport
*/
#ifndef rs232_INCLUDED
#define rs232_INCLUDED
#ifdef __cplusplus
extern "C" {
#endif
#include <stdio.h>
#include <string.h>
#ifdef __linux__
#include <termios.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <limits.h>
#else
#include <windows.h>
#endif
//return port handle or -1 if fails
int OpenComport(const char * comport_name, int baudrate);
int PollComport(int, unsigned char *, int);
int SendByte(int, unsigned char);
int SendBuf(int, unsigned char *, int);
void CloseComport(int);
#ifdef __cplusplus
} /* extern "C" */
#endif
#endif

947
software/ros_packages/rover_arm/src/simplemotion/simplemotion.c Normal file
View File

@@ -0,0 +1,947 @@
//Copyright (c) Granite Devices Oy
/*
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; version 2 of the License.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
*/
#include <stdio.h>
#include <string.h>
#include "busdevice.h"
#include "sm485.h"
#include <stdarg.h>
#include "simplemotion_private.h"
SM_STATUS smParseReturnData( smbus handle, smuint8 data );
#define HANDLE_STAT(stat) if(stat!=SM_OK)return (stat);
#define HANDLE_STAT_AND_RET(stat,returndata) { if(returndata==RET_INVALID_CMD||returndata==RET_INVALID_PARAM) return SM_ERR_PARAMETER; if(stat!=SM_OK) return (stat); }
enum RecvState {WaitCmdId,WaitAddr,WaitPayloadSize,WaitPayload,WaitCrcHi,WaitCrcLo};
FILE *smDebugOut=NULL;
//useful macros from extracting/storing multibyte values from/to byte buffer
#define bufput32bit(buf, pos, val) *((smuint32*)(smuint8*)((buf)+(pos)))=((smuint32)(val))
#define bufput16bit(buf, pos, val) *((smuint16*)(smuint8*)((buf)+(pos)))=((smuint16)(val))
#define bufput8bit(buf, pos, val) *((smuint8*)(smuint8*)((buf)+(pos)))=((smuint8)(val))
#define bufget32bit(buf, pos) (*((smuint32*)(smuint8*)((buf)+(pos))))
#define bufget16bit(buf, pos) (*((smuint16*)(smuint8*)((buf)+(pos))))
#define bufget8bit(buf, pos) (*((smuint8*)(smuint8*)((buf)+(pos))))
smbool smIsHandleOpen( const smbus handle );
SM_STATUS smReceiveReturnPacket( smbus bushandle );
typedef struct SM_BUS_
{
smint8 bdHandle;
smbool opened;
enum RecvState recv_state,recv_state_next;
smint16 recv_payloadsize;
smint16 recv_storepos;
smuint8 recv_rsbuf[SM485_RSBUFSIZE];
smuint8 recv_cmdid;
smuint8 recv_addr;
smuint16 recv_crc;
smuint16 recv_read_crc_hi;
smbool receiveComplete;
smbool transmitBufFull;//set true if user uploads too much commands in one SM transaction. if true, on execute commands, nothing will be sent to bus to prevent unvanted clipped commands and buffer will be cleared
char busDeviceName[SM_BUSDEVICENAME_LEN];
smint16 cmd_send_queue_bytes;//for queued device commands
smint16 cmd_recv_queue_bytes;//recv_queue_bytes counted upwards at every smGetQueued.. and compared to payload size
SM_STATUS cumulativeSmStatus;
} SM_BUS;
SM_BUS smBus[SM_MAX_BUSES];
smuint16 readTimeoutMs=SM_READ_TIMEOUT;
//init on first smOpenBus call
smbool smInitialized=smfalse;
//if debug message has priority this or above will be printed to debug stream
smVerbosityLevel smDebugThreshold=Trace;
#ifdef ENABLE_DEBUG_PRINTS
void smDebug( smbus handle, smVerbosityLevel verbositylevel, char *format, ...)
{
va_list fmtargs;
char buffer[1024];
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return;
if(smDebugOut!=NULL && verbositylevel <= smDebugThreshold )
{
va_start(fmtargs,format);
vsnprintf(buffer,sizeof(buffer)-1,format,fmtargs);
va_end(fmtargs);
fprintf(smDebugOut,"%s: %s",smBus[handle].busDeviceName, buffer);
}
}
#else
#define smDebug(...)
#endif
void smResetSM485variables(smbus handle)
{
smBus[handle].recv_state=WaitCmdId;
smBus[handle].recv_state_next=WaitCmdId;
smBus[handle].recv_payloadsize=-1;
smBus[handle].recv_storepos=0;//number of bytes to expect data in cmd, -1=wait cmd header
smBus[handle].recv_cmdid=0;// cmdid=0 kun ed komento suoritettu
smBus[handle].recv_addr=255;
smBus[handle].recv_crc=SM485_CRCINIT;
smBus[handle].recv_read_crc_hi=0xffff;//bottom bits will be contains only 1 byte when read
smBus[handle].receiveComplete=smfalse;
smBus[handle].transmitBufFull=smfalse;
smBus[handle].cmd_send_queue_bytes=0;
smBus[handle].cmd_recv_queue_bytes=0;
}
smuint16 calcCRC16(smuint8 data, smuint16 crc)
{
unsigned int i; /* will index into CRC lookup */
i = (crc>>8) ^ data; /* calculate the CRC */
crc = (((crc&0xff) ^ table_crc16_hi[i])<<8) | table_crc16_lo[i];
return crc;
}
smuint16 calcCRC16Buf(const char *buffer, smuint16 buffer_length)
{
smuint8 crc_hi = 0xFF; /* high CRC byte initialized */
smuint8 crc_lo = 0xFF; /* low CRC byte initialized */
unsigned int i; /* will index into CRC lookup */
/* pass through message buffer */
while (buffer_length--) {
i = crc_hi ^ *buffer++; /* calculate the CRC */
crc_hi = crc_lo ^ table_crc16_hi[i];
crc_lo = table_crc16_lo[i];
}
return (crc_hi << 8 | crc_lo);
}
smuint8 calcCRC8Buf( smuint8 *buf, int len, int crcinit )
{
int i;
smuint8 crc=crcinit;
for(i=0;i<len;i++)
{
crc=table_crc8[crc^buf[i]];
}
return crc;
}
SM_STATUS smSetTimeout( smuint16 millsecs )
{
if(millsecs<=5000)
{
readTimeoutMs=millsecs;
return SM_OK;
}
return SM_ERR_PARAMETER;
}
unsigned long smGetVersion()
{
return SM_VERSION;
}
//init bus struct table
void smBusesInit()
{
int i;
for(i=0;i<SM_MAX_BUSES;i++)
{
smBus[i].opened=smfalse;;
smResetSM485variables(i);
}
smInitialized=smtrue;
}
smbool smIsHandleOpen( const smbus handle )
{
if(handle<0) return smfalse;
if(handle>=SM_MAX_BUSES) return smfalse;
return smBus[handle].opened;
}
/** Open SM RS485 communication bus. Parameters:
-devicename: on Windows COM port as "COMx" or on unix /dev/ttySx or /dev/ttyUSBx where x=port number
-return value: integer handle to be used with all other commands, -1 if fails
*/
smbus smOpenBus( const char * devicename )
{
int handle;
//true on first call
if(smInitialized==smfalse)
smBusesInit();
//find free handle
for(handle=0;handle<SM_MAX_BUSES;handle++)
{
if(smBus[handle].opened==smfalse) break;//choose this
}
//all handles in use
if(handle>=SM_MAX_BUSES) return -1;
//open bus device
smBus[handle].bdHandle=smBDOpen(devicename);
if(smBus[handle].bdHandle==-1) return -1;
//success
strncpy( smBus[handle].busDeviceName, devicename, SM_BUSDEVICENAME_LEN );
smBus[handle].busDeviceName[SM_BUSDEVICENAME_LEN-1]=0;//null terminate string
smBus[handle].opened=smtrue;
return handle;
}
/** Close connection to given bus handle number. This frees communication link therefore makes it available for other apps for opening.
-return value: a SM_STATUS value, i.e. SM_OK if command succeed
*/
LIB SM_STATUS smCloseBus( const smbus bushandle )
{
//check if bus handle is valid & opened
if(smIsHandleOpen(bushandle)==smfalse) return recordStatus(bushandle,SM_ERR_NODEVICE);
smBus[bushandle].opened=smfalse;
if( smBDClose(smBus[bushandle].bdHandle) == smfalse ) return recordStatus(bushandle,SM_ERR_BUS);
return SM_OK;
}
char *cmdidToStr(smuint8 cmdid )
{
char *str;
switch(cmdid)
{
case SMCMD_INSTANT_CMD : str="SMCMD_INSTANT_CMD";break;
case SMCMD_INSTANT_CMD_RET :str="SMCMD_INSTANT_CMD_RET";break;
case SMCMD_BUFFERED_CMD :str="SMCMD_BUFFERED_CMD";break;
case SMCMD_BUFFERED_RETURN_DATA :str="SMCMD_BUFFERED_RETURN_DATA";break;
case SMCMD_BUFFERED_RETURN_DATA_RET :str="SMCMD_BUFFERED_RETURN_DATA_RET";break;
case SMCMD_BUFFERED_CMD_RET:str="SMCMD_BUFFERED_CMD_RET";break;
#ifdef PROCESS_IMAGE_SUPPORT
case SMCMD_PROCESS_IMAGE:str="SMCMD_PROCESS_IMAGE";break;
case SMCMD_PROCESS_IMAGE_RET: str="SMCMD_PROCESS_IMAGE_RET";break;
#endif
case SMCMD_GET_CLOCK: str="SMCMD_GET_CLOCK";break;
case SMCMD_GET_CLOCK_RET :str="SMCMD_GET_CLOCK_RET";break;
case SMCMD_FAST_UPDATE_CYCLE:str="SMCMD_FAST_UPDATE_CYCLE";break;
case SMCMD_FAST_UPDATE_CYCLE_RET:str="SMCMD_FAST_UPDATE_CYCLE_RET";break;
default: str="unknown cmdid";break;
}
//puts(str);
return str;
}
//write one byte to tx buffer
//returns true on success
smbool smWriteByte( const smbus handle, const smuint8 byte, smuint16 *crc )
{
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
smbool success=smBDWrite(smBus[handle].bdHandle,byte);
if(crc!=NULL)
*crc = calcCRC16(byte,*crc);
if(success==smtrue)
smDebug(handle, High, " sent byte %02x\n",byte);
else
smDebug(handle, High, " sending byte %02x failed\n",byte);
return success;
}
//write tx buffer to bus
//returns true on success
smbool smTransmitBuffer( const smbus handle )
{
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
smbool success=smBDTransmit(smBus[handle].bdHandle);
return success;
}
SM_STATUS smSendSMCMD( smbus handle, smuint8 cmdid, smuint8 addr, smuint8 datalen, smuint8 *cmddata )
{
int i;
smuint8 data;
smuint16 sendcrc;
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
sendcrc=SM485_CRCINIT;
smDebug(handle, Mid, "> %s (id=%d, addr=%d, payload=%d)\n",cmdidToStr(cmdid),cmdid,
addr,
datalen);
smDebug(handle,High,"ID ");
if( smWriteByte(handle,cmdid, &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);
if(cmdid&SMCMD_MASK_N_PARAMS)
{
smDebug(handle,High,"Nparams ");
if( smWriteByte(handle,datalen, &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);
}
smDebug(handle,High,"ADDR ");
if( smWriteByte(handle,addr, &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);
for(i=0;i<datalen;i++)
{
smDebug(handle,High,"DATA ");
if( smWriteByte(handle,cmddata[i], &sendcrc) != smtrue ) return recordStatus(handle,SM_ERR_BUS);
}
smDebug(handle,High,"CRC ");
if( smWriteByte(handle,sendcrc>>8, NULL) != smtrue ) return recordStatus(handle,SM_ERR_BUS);
smDebug(handle,High,"CRC ");
if( smWriteByte(handle,sendcrc&0xff,NULL) != smtrue ) return recordStatus(handle,SM_ERR_BUS);
//transmit bytes to bus that were written in buffer by smWriteByte calls
if( smTransmitBuffer(handle) != smtrue ) return recordStatus(handle,SM_ERR_BUS);
return recordStatus(handle,SM_OK);
}
SM_STATUS smFastUpdateCycle( smbus handle, smuint8 nodeAddress, smuint16 write1, smuint16 write2, smuint16 *read1, smuint16 *read2)
{
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
smDebug(handle, Mid, "> %s (addr=%d, w1=%d, w2=%d)\n",cmdidToStr(SMCMD_FAST_UPDATE_CYCLE),
nodeAddress,
write1,write2);
//form the tx packet
smuint8 cmd[8];
int i;
cmd[0]=SMCMD_FAST_UPDATE_CYCLE;
cmd[1]=nodeAddress;
bufput16bit(cmd,2,write1);
bufput16bit(cmd,4,write2);
cmd[6]=calcCRC8Buf(cmd,6,0x52);
//send
for(i=0;i<7;i++)
{
if( smWriteByte(handle,cmd[i], NULL) != smtrue )
return recordStatus(handle,SM_ERR_BUS);
}
smTransmitBuffer(handle);//this sends the bytes entered with smWriteByte
smDebug(handle, High, " Reading reply packet\n");
for(i=0;i<6;i++)
{
smbool success;
smuint8 rx;
success=smBDRead(smBus[handle].bdHandle,&rx);
cmd[i]=rx;
if(success!=smtrue)
return recordStatus(handle,SM_ERR_BUS|SM_ERR_LENGTH);//no enough data received
}
//parse
if( cmd[5]!=calcCRC8Buf(cmd,5,0x52) || cmd[0]!=SMCMD_FAST_UPDATE_CYCLE_RET )
{
return recordStatus(handle,SM_ERR_COMMUNICATION);//packet error
}
if(read1!=NULL)
*read1=bufget16bit(cmd,1);
if(read2!=NULL)
*read2=bufget16bit(cmd,3);
//return data read complete
smDebug(handle,Mid, "< %s (id=%d, r1=%d, r2=%d)\n",
cmdidToStr( cmd[0] ),
cmd[0],*read1,*read2);
return recordStatus(handle,SM_OK);
}
SM_STATUS smReceiveErrorHandler( smbus handle, smbool flushrx )
{
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
//empty pending rx buffer to avoid further parse errors
if(flushrx==smtrue)
{
smbool success;
do{
smuint8 rx;
success=smBDRead(handle,&rx);
}while(success==smtrue);
}
smResetSM485variables(handle);
smBus[handle].receiveComplete=smtrue;
return recordStatus(handle,SM_ERR_COMMUNICATION);
}
SM_STATUS smAppendSMCommandToQueue( smbus handle, int smpCmdType,smint32 paramvalue )
{
smuint8 txbyte;
int cmdlength;
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
switch(smpCmdType)
{
case SMPCMD_SETPARAMADDR:
cmdlength=2;
break;
case SMPCMD_24B:
cmdlength=3;
break;
case SMPCMD_32B:
cmdlength=4;
break;
default:
return recordStatus(handle,SM_ERR_PARAMETER);
break;
}
//check if space if buffer
if(smBus[handle].cmd_send_queue_bytes>(SM485_MAX_PAYLOAD_BYTES-cmdlength) )
{
smBus[handle].transmitBufFull=smtrue; //when set true, smExecute will do nothing but clear transmit buffer. so this prevents any of overflowed commands getting thru
return recordStatus(handle,SM_ERR_LENGTH); //overflow, too many commands in buffer
}
if(smpCmdType==SMPCMD_SETPARAMADDR)
{
SMPayloadCommand16 newcmd;
newcmd.ID=SMPCMD_SETPARAMADDR;
newcmd.param=paramvalue;
// bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes, 5);
// bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes, 6);
/*
FIXME
ei toimi, menee vaa nollaa*/
bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[1]);
bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[0]);
}
if(smpCmdType==SMPCMD_24B)
{
SMPayloadCommand24 newcmd;
newcmd.ID=SMPCMD_24B;
newcmd.param=paramvalue;
bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[2]);
bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[1]);
bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[0]);
}
if(smpCmdType==SMPCMD_32B)
{
SMPayloadCommand32 newcmd;
newcmd.ID=SMPCMD_32B;
newcmd.param=paramvalue;
bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[3]);
bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[2]);
bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[1]);
bufput8bit( smBus[handle].recv_rsbuf, smBus[handle].cmd_send_queue_bytes++, ((unsigned char*)&newcmd)[0]);
}
return recordStatus(handle,SM_OK);
}
SMPayloadCommandRet32 smConvertToPayloadRet32_16(SMPayloadCommandRet16 in)
{
SMPayloadCommandRet32 out;
out.ID=in.ID;
out.retData=(long)in.retData; //negative value, extend ones
return out;
}
//for library internal use only
SM_STATUS smTransmitReceiveCommandQueue( const smbus bushandle, const smaddr targetaddress, smuint8 cmdid )
{
SM_STATUS stat;
//check if bus handle is valid & opened
if(smIsHandleOpen(bushandle)==smfalse) return recordStatus(bushandle,SM_ERR_NODEVICE);
if(smBus[bushandle].transmitBufFull!=smtrue) //dont send/receive commands if queue was overflowed by user error
{
stat=smSendSMCMD(bushandle,cmdid,targetaddress, smBus[bushandle].cmd_send_queue_bytes, smBus[bushandle].recv_rsbuf ); //send commands to bus
if(stat!=SM_OK) return recordStatus(bushandle,stat);
}
smBus[bushandle].cmd_send_queue_bytes=0;
smBus[bushandle].cmd_recv_queue_bytes=0;//counted upwards at every smGetQueued.. and compared to payload size
if(smBus[bushandle].transmitBufFull!=smtrue)//dont send/receive commands if queue was overflowed by user error
{
stat=smReceiveReturnPacket(bushandle);//blocking wait & receive return values from bus
if(stat!=SM_OK) return recordStatus(bushandle,stat); //maybe timeouted
}
smBus[bushandle].transmitBufFull=smfalse;//reset overflow status
return recordStatus(bushandle,SM_OK);
}
SM_STATUS smExecuteCommandQueue( const smbus bushandle, const smaddr targetaddress )
{
//check if bus handle is valid & opened
if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;
return recordStatus(bushandle,smTransmitReceiveCommandQueue(bushandle,targetaddress,SMCMD_INSTANT_CMD));
}
SM_STATUS smUploadCommandQueueToDeviceBuffer( const smbus bushandle, const smaddr targetaddress )
{
return recordStatus(bushandle,smTransmitReceiveCommandQueue(bushandle,targetaddress,SMCMD_BUFFERED_CMD));
}
//return number of how many bytes waiting to be read with smGetQueuedSMCommandReturnValue
SM_STATUS smBytesReceived( const smbus bushandle, smint32 *bytesinbuffer )
{
if(smIsHandleOpen(bushandle)==smfalse) return recordStatus(bushandle,SM_ERR_NODEVICE);
smint32 bytes=smBus[bushandle].recv_payloadsize - smBus[bushandle].cmd_recv_queue_bytes;//how many bytes waiting to be read with smGetQueuedSMCommandReturnValue
*bytesinbuffer=bytes;
return recordStatus(bushandle,SM_OK);
}
SM_STATUS smGetQueuedSMCommandReturnValue( const smbus bushandle, smint32 *retValue )
{
smuint8 rxbyte, rettype;
//check if bus handle is valid & opened
if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;
//if get called so many times that receive queue buffer is already empty, return error
if(smBus[bushandle].cmd_recv_queue_bytes>=smBus[bushandle].recv_payloadsize)
{
smDebug(Trace,bushandle,"Packet receive error, return data coudn't be parsed\n");
//return 0
if(retValue!=NULL) *retValue=0;//check every time if retValue is set NULL by caller -> don't store anything to it if its NULL
return recordStatus(bushandle,SM_ERR_LENGTH);//not a single byte left
}
//get first byte to deterime packet length
rxbyte=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
rettype=rxbyte>>6; //extract ret packet header 2 bits
//read rest of data based on packet header:
if(rettype == SMPRET_16B)
{
//extract return packet and convert to 32 bit and return
SMPayloadCommandRet16 read;
smuint8 *readBuf=(smuint8*)&read;
readBuf[1]=rxbyte;
readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
smDebug(Trace,bushandle,"RET16B: %d\n",read.retData);
if(retValue!=NULL) *retValue=read.retData;
return recordStatus(bushandle,SM_OK);
}
if(rettype == SMPRET_24B)
{
//extract return packet and convert to 32 bit and return
SMPayloadCommandRet24 read;
smuint8 *readBuf=(smuint8*)&read;
readBuf[2]=rxbyte;
readBuf[1]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
smDebug(Trace,bushandle,"RET24B: %d\n",read.retData);
if(retValue!=NULL) *retValue=read.retData;
return recordStatus(bushandle,SM_OK);
}
if(rettype == SMPRET_32B)
{
//extract return packet and convert to 32 bit and return
SMPayloadCommandRet32 read;
smuint8 *readBuf=(smuint8*)&read;
readBuf[3]=rxbyte;
readBuf[2]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
readBuf[1]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
readBuf[0]=bufget8bit(smBus[bushandle].recv_rsbuf, smBus[bushandle].cmd_recv_queue_bytes++);
smDebug(Trace,bushandle,"RET32B: %d\n",read.retData);
if(retValue!=NULL) *retValue=read.retData;
return recordStatus(bushandle,SM_OK);
}
if(rettype == SMPRET_OTHER) //8bit
{
//extract return packet and convert to 32 bit and return
SMPayloadCommandRet8 read;
smuint8 *readBuf=(smuint8*)&read;
readBuf[0]=rxbyte;
smDebug(Trace,bushandle,"RET_OTHER: %d\n",read.retData);
if(retValue!=NULL) *retValue=read.retData;
return recordStatus(bushandle,SM_OK);
}
return recordStatus(bushandle,SM_ERR_PARAMETER); //something went wrong, rettype not known
}
SM_STATUS smReceiveReturnPacket( smbus bushandle )
{
//check if bus handle is valid & opened
if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;
smDebug(bushandle, High, " Reading reply packet\n");
do
{
smuint8 ret;
SM_STATUS stat;
smbool succ=smBDRead(bushandle,&ret);
if(succ==smfalse)
{
smReceiveErrorHandler(bushandle,smfalse);
smDebug(bushandle, High, " smRawCommand RX read byte failed\n");
return recordStatus(bushandle,SM_ERR_COMMUNICATION);
}
else
{
smDebug(bushandle, High, " got byte %02x \n",ret);
}
stat=smParseReturnData( bushandle, ret );
if(stat!=SM_OK) return recordStatus(bushandle,stat);
} while(smBus[bushandle].receiveComplete==smfalse); //loop until complete packaget has been read
//return data read complete
smDebug(bushandle,Mid, "< %s (id=%d, addr=%d, payload=%d)\n",
cmdidToStr( smBus[bushandle].recv_cmdid ),
smBus[bushandle].recv_cmdid,
smBus[bushandle].recv_addr,
smBus[bushandle].recv_payloadsize);
return recordStatus(bushandle,SM_OK);
}
/** Set stream where debug output is written. By default nothing is written. */
LIB void smSetDebugOutput( smVerbosityLevel level, FILE *stream )
{
smDebugThreshold=level;
smDebugOut=stream;
}
//short returndata16=0, payload=0;
//can be called at any frequency
SM_STATUS smParseReturnData( smbus handle, smuint8 data )
{
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
//buffered variable allows placing if's in any order (because recv_state may changes in this function)
smBus[handle].recv_state=smBus[handle].recv_state_next;
smBus[handle].receiveComplete=smfalse;//overwritten to true later if complete
if(smBus[handle].recv_state==WaitPayload)
{
smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);
//normal handling for all payload data
if(smBus[handle].recv_storepos<SM485_MAX_PAYLOAD_BYTES)
smBus[handle].recv_rsbuf[smBus[handle].recv_storepos++]=data;
else//rx payload buffer overflow
{
return recordStatus(handle,(smReceiveErrorHandler(handle,smtrue)));
}
//all received
if(smBus[handle].recv_payloadsize<=smBus[handle].recv_storepos)
smBus[handle].recv_state_next=WaitCrcHi;
return recordStatus(handle,SM_OK);
}
if(smBus[handle].recv_state==WaitCmdId)
{
smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);
smBus[handle].recv_cmdid=data;
switch(data&SMCMD_MASK_PARAMS_BITS)//commands with fixed payload size
{
case SMCMD_MASK_2_PARAMS: smBus[handle].recv_payloadsize=2; smBus[handle].recv_state_next=WaitAddr; break;
case SMCMD_MASK_0_PARAMS: smBus[handle].recv_payloadsize=0; smBus[handle].recv_state_next=WaitAddr; break;
case SMCMD_MASK_N_PARAMS: smBus[handle].recv_payloadsize=-1; smBus[handle].recv_state_next=WaitPayloadSize;break;//-1 = N databytes
default:
return recordStatus(handle,(smReceiveErrorHandler(handle, smtrue)));
break; //error, unsupported command id
}
return recordStatus(handle,SM_OK);
}
//no data payload size known yet
if(smBus[handle].recv_state==WaitPayloadSize)
{
smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);
smBus[handle].recv_payloadsize=data;
smBus[handle].recv_state_next=WaitAddr;
return recordStatus(handle,SM_OK);
}
if(smBus[handle].recv_state==WaitAddr)
{
smBus[handle].recv_crc=calcCRC16(data,smBus[handle].recv_crc);
smBus[handle].recv_addr=data;//can be receiver or sender addr depending on cmd
if(smBus[handle].recv_payloadsize>smBus[handle].recv_storepos)
smBus[handle].recv_state_next=WaitPayload;
else
smBus[handle].recv_state_next=WaitCrcHi;
return recordStatus(handle,SM_OK);
}
if(smBus[handle].recv_state==WaitCrcHi)
{
smBus[handle].recv_read_crc_hi=data;//crc_msb
smBus[handle].recv_state_next=WaitCrcLo;
return recordStatus(handle,SM_OK);
}
//get crc_lsb, check crc and execute
if(smBus[handle].recv_state==WaitCrcLo)
{
if(((smBus[handle].recv_read_crc_hi<<8)|data)!=smBus[handle].recv_crc)
{
//CRC error
return recordStatus(handle,(smReceiveErrorHandler(handle,smtrue)));
}
else
{
//CRC ok
//if(smBus[handle].recv_addr==config.deviceAddress || smBus[handle].recv_cmdid==SMCMD_GET_CLOCK_RET || smBus[handle].recv_cmdid==SMCMD_PROCESS_IMAGE ) executeSMcmd();
smBus[handle].receiveComplete=smtrue;
}
//smResetSM485variables(handle);
smBus[handle].recv_storepos=0;
smBus[handle].recv_crc=SM485_CRCINIT;
smBus[handle].recv_state_next=WaitCmdId;
return recordStatus(handle,SM_OK);
}
return recordStatus(handle,SM_OK);
}
//reply consumes 16 bytes in payload buf, so max calls per cycle is 7
SM_STATUS smAppendGetParamCommandToQueue( smbus handle, smint16 paramAddress )
{
SM_STATUS stat=SM_NONE;
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
//possible errors will set bits to stat
stat|=smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_LEN ); //2b
stat|=smAppendSMCommandToQueue( handle, SMPCMD_24B, SMPRET_32B );//3b
stat|=smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_ADDR );//2b
stat|=smAppendSMCommandToQueue( handle, SMPCMD_24B, paramAddress );//3b
//=10 bytes
return recordStatus(handle,stat);
}
SM_STATUS smGetQueuedGetParamReturnValue( const smbus bushandle, smint32 *retValue )
{
smint32 retVal=0;
SM_STATUS stat=SM_NONE;
//check if bus handle is valid & opened
if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;
//must get all inserted commands from buffer
stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );//4x4b
stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );
stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );
stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal ); //the real return value is here
if(retValue!=NULL) *retValue=retVal;
return recordStatus(bushandle,stat);
}
//consumes 6 bytes in payload buf, so max calls per cycle is 20
SM_STATUS smAppendSetParamCommandToQueue( smbus handle, smint16 paramAddress, smint32 paramValue )
{
SM_STATUS stat=SM_NONE;
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
stat|=smAppendSMCommandToQueue( handle, SMPCMD_SETPARAMADDR, paramAddress );//2b
stat|=smAppendSMCommandToQueue( handle, SMPCMD_32B, paramValue );//4b
return recordStatus(handle,stat);
}
SM_STATUS smGetQueuedSetParamReturnValue( const smbus bushandle, smint32 *retValue )
{
smint32 retVal=0;
SM_STATUS stat=SM_NONE;
//check if bus handle is valid & opened
if(smIsHandleOpen(bushandle)==smfalse) return SM_ERR_NODEVICE;
//must get all inserted commands from buffer
stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal );
stat|=smGetQueuedSMCommandReturnValue( bushandle, &retVal ); //the real return value is here
if(retValue!=NULL) *retValue=retVal;
return recordStatus(bushandle,stat);
}
SM_STATUS smGetBufferClock( const smbus handle, const smaddr targetaddr, smuint16 *clock )
{
SM_STATUS stat;
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return recordStatus(handle,SM_ERR_NODEVICE);
stat=smSendSMCMD(handle, SMCMD_GET_CLOCK ,targetaddr, 0, NULL ); //send get clock commands to bus
if(stat!=SM_OK) return recordStatus(handle,stat);
stat=smReceiveReturnPacket(handle);//blocking wait & receive return values from bus
if(stat!=SM_OK) return recordStatus(handle,stat); //maybe timeouted
if(clock!=NULL)
*clock=bufget16bit(smBus[handle].recv_rsbuf,0);
smBus[handle].recv_storepos=0;
return recordStatus(handle,SM_OK);
}
/** Simple read & write of parameters with internal queueing, so only one call needed.
Use these for non-time critical operations. */
SM_STATUS smRead1Parameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1 )
{
SM_STATUS smStat=0;
smStat|=smAppendGetParamCommandToQueue(handle,paramId1);
smStat|=smExecuteCommandQueue(handle,nodeAddress);
smStat|=smGetQueuedGetParamReturnValue(handle,paramVal1);
return recordStatus(handle,smStat);
}
SM_STATUS smRead2Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 )
{
SM_STATUS smStat=0;
smStat|=smAppendGetParamCommandToQueue(handle,paramId1);
smStat|=smAppendGetParamCommandToQueue(handle,paramId2);
smStat|=smExecuteCommandQueue(handle,nodeAddress);
smStat|=smGetQueuedGetParamReturnValue(handle,paramVal1);
smStat|=smGetQueuedGetParamReturnValue(handle,paramVal2);
return recordStatus(handle,smStat);
}
SM_STATUS smRead3Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 ,const smint16 paramId3, smint32 *paramVal3 )
{
SM_STATUS smStat=0;
smStat|=smAppendGetParamCommandToQueue(handle,paramId1);
smStat|=smAppendGetParamCommandToQueue(handle,paramId2);
smStat|=smAppendGetParamCommandToQueue(handle,paramId3);
smStat|=smExecuteCommandQueue(handle,nodeAddress);
smStat|=smGetQueuedGetParamReturnValue(handle,paramVal1);
smStat|=smGetQueuedGetParamReturnValue(handle,paramVal2);
smStat|=smGetQueuedGetParamReturnValue(handle,paramVal3);
return recordStatus(handle,smStat);
}
SM_STATUS smSetParameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId, smint32 paramVal )
{
smint32 nul;
SM_STATUS smStat=0;
smStat|=smAppendSetParamCommandToQueue( handle, paramId, paramVal );
smStat|=smExecuteCommandQueue(handle,nodeAddress);
smStat|=smGetQueuedSetParamReturnValue( handle, &nul );
return recordStatus(handle,smStat);
}
//accumulates status to internal variable by ORing the bits. returns same value that is fed as paramter
SM_STATUS recordStatus( const smbus handle, const SM_STATUS stat )
{
//check if bus handle is valid & opened
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
smBus[handle].cumulativeSmStatus|=stat;
return stat;
}
/** This function returns all occurred SM_STATUS bits after smOpenBus or resetCumulativeStatus call*/
SM_STATUS getCumulativeStatus( const smbus handle )
{
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
return smBus[handle].cumulativeSmStatus;
}
/** Reset cululative status so getCumultiveStatus returns 0 after calling this until one of the other functions are called*/
void resetCumulativeStatus( const smbus handle )
{
if(smIsHandleOpen(handle)==smfalse) return SM_ERR_NODEVICE;
smBus[handle].cumulativeSmStatus=0;
}

144
software/ros_packages/rover_arm/src/simplemotion/simplemotion.h Normal file
View File

@@ -0,0 +1,144 @@
//Global SimpleMotion functions & definitions
//Copyright (c) Granite Devices Oy
/*
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; version 2 of the License.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
*/
#ifndef SIMPLEMOTION_H
#define SIMPLEMOTION_H
#ifdef WIN32
//dll specs
#ifdef BUILD_DLL
#define LIB __declspec(dllexport)
#else
// #define LIB __declspec(dllimport)
#define LIB
#endif
#else
#define LIB
#endif
#include <stdio.h>
#include "simplemotion_defs.h"
#ifdef __cplusplus
extern "C"{
#endif
//possible return values (SM_STATUS type)
#define SM_NONE 0
#define SM_OK 1
#define SM_ERR_NODEVICE 2
#define SM_ERR_BUS 4
#define SM_ERR_COMMUNICATION 8
#define SM_ERR_PARAMETER 16
#define SM_ERR_LENGTH 32
///////////////////////////////////////////////////////////////////////////////////////
//TYPES & VALUES //////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
//declare SM lib types
typedef long smbus;
typedef unsigned long smuint32;
typedef unsigned short smuint16;
typedef unsigned char smuint8;
typedef long smint32;
typedef short smint16;
typedef char smint8;
typedef char smbool;
#define smtrue 1
#define smfalse 0
typedef int SM_STATUS;
typedef smuint8 smaddr;
//comment out to disable, gives smaller & faster code
#define ENABLE_DEBUG_PRINTS
typedef enum _smVerbosityLevel {Off,Low,Mid,High,Trace} smVerbosityLevel;
//max number of simultaneously opened buses. change this and recompiple SMlib if
//necessary (to increase channels or reduce to save memory)
//#define SM_MAX_BUSES 5
///////////////////////////////////////////////////////////////////////////////////////
//FUNCTIONS////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////
/** Open SM RS485 communication bus. Parameters:
-devicename: "USB2VSD" or com port as "COMx" where x=1-16
-return value: handle to be used with all other commands, -1 if fails
*/
LIB smbus smOpenBus( const char * devicename );
/** Set timeout of how long to wait reply packet from bus. Must be set before smOpenBus and cannot be changed afterwards
* max value 5000ms. In unix this is rounded to 100ms (rounding downwards), so 99 or less gives 0ms timeout.
*
*This is the only function that returns SM_STATUS which doesn't accumulate status bits to be read with getCumulativeStatus because it has no bus handle
*/
LIB SM_STATUS smSetTimeout( smuint16 millsecs );
/** Close connection to given bus handle number. This frees communication link therefore makes it available for other apps for opening.
-return value: a SM_STATUS value, i.e. SM_OK if command succeed
*/
LIB SM_STATUS smCloseBus( const smbus bushandle );
/** Return SM lib version number in hexadecimal format.
Ie V 2.5.1 would be 0x020501 and 1.2.33 0x010233 */
LIB smuint32 smGetVersion();
/** Set stream where debug output is written. By default nothing is written. */
LIB void smSetDebugOutput( smVerbosityLevel level, FILE *stream );
/** This function returns all occurred SM_STATUS bits after smOpenBus or resetCumulativeStatus call*/
LIB SM_STATUS getCumulativeStatus( const smbus handle );
/** Reset cululative status so getCumultiveStatus returns 0 after calling this until one of the other functions are called*/
LIB void resetCumulativeStatus( const smbus handle );
/** SMV2 Device communication functionss */
LIB SM_STATUS smAppendCommandToQueue( smbus handle, smuint8 cmdid, smuint16 param );
LIB SM_STATUS smExecuteCommandQueue( const smbus bushandle, const smaddr targetaddress );
LIB smuint16 smGetQueuedCommandReturnValue( const smbus bushandle, smuint16 cmdnumber );
LIB SM_STATUS smUploadCommandQueueToDeviceBuffer( const smbus bushandle, const smaddr targetaddress );
LIB SM_STATUS smBytesReceived( const smbus bushandle, smint32 *bytesinbuffer );
LIB SM_STATUS smAppendSMCommandToQueue( smbus handle, int smpCmdType, smint32 paramvalue );
LIB SM_STATUS smGetQueuedSMCommandReturnValue( const smbus bushandle, smint32 *retValue );
LIB SM_STATUS smAppendGetParamCommandToQueue( smbus handle, smint16 paramAddress );
LIB SM_STATUS smGetQueuedGetParamReturnValue( const smbus bushandle, smint32 *retValue );
LIB SM_STATUS smAppendSetParamCommandToQueue( smbus handle, smint16 paramAddress, smint32 paramValue );
LIB SM_STATUS smGetQueuedSetParamReturnValue( const smbus bushandle, smint32 *retValue );
/** Simple read & write of parameters with internal queueing, so only one call needed.
Use these for non-time critical operations. */
LIB SM_STATUS smRead1Parameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1 );
LIB SM_STATUS smRead2Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 );
LIB SM_STATUS smRead3Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 ,const smint16 paramId3, smint32 *paramVal3 );
LIB SM_STATUS smSetParameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId, smint32 paramVal );
LIB SM_STATUS smGetBufferClock( const smbus handle, const smaddr targetaddr, smuint16 *clock );
/** smFastUpdateCycle uses special SimpleMotion command to perform fast turaround communication. May be used with cyclic real time control. Parameter & return data
*content are application specific and defined . */
LIB SM_STATUS smFastUpdateCycle( smbus handle, smuint8 nodeAddress, smuint16 write1, smuint16 write2, smuint16 *read1, smuint16 *read2);
#ifdef __cplusplus
}
#endif
#endif // SIMPLEMOTION_H

754
software/ros_packages/rover_arm/src/simplemotion/simplemotion_defs.h Normal file
View File

@@ -0,0 +1,754 @@
/*
* simplemotion_defs.h
* This file contains addresses for simple motion parameters (SMP's) and if parameter is a bit field
* or enum (low number of choices), then also bits or choices in the register are defined here.
*
* Created on: 25.2.2012
* Author: Tero
*/
/* SMV2 address space areas:
* 0 NULL, can read or write but has no effect
* 1-63 SM bus specific parameters
* 128-167 digital I/O registers for GPIO
* 168-199 analog I/O reigisters for GPIO
* 201-8190 application specific parameter addresses. in this case motor drive registers
* 8191 NOP command. if SM command SMPCMD_SET_PARAM_ADDR with SMP_ADDR_NOP as parameter is sent, no actions are taken in nodes. it's NOP (no operation) command.
*/
/* SMV2 protocol change log:
* Version 20:
* -V20 was introduced with Argon FW 1.0.0 and present at least until 1.4.0
* Version 21 (is backwards compatible in syntax but not in setpoint behavior, see details below):
* -V25 was introduced with IONI
* -setpoint calculation is different:
* now there is only one common setpoint and all ABS and INC commands (buffered & instant)
* change the same variable. In V20 there was separate setpoint variables for each stream
* (instant & buffered) and for INC and ABS separately and finally all setpoints were summed together.
* Formula for total setpoint is = SMV2setpoint+OptionalPhysicalSetpoint (step/dir, analog etc)
* -in buffered stream, only setpoint commands execute on timed pace. all others execute as fast as possible.
* this makes it possible to insert any parameter read/write commands in middle of buffered motion.
* -implemented watchdog functionality in new param SMP_FAULT_BEHAVIOR
* -added param SMP_ADDRESS_OFFSET
*/
/* Important when using SMV2 protocol:
*
* SMP_SM_VERSION_COMPAT defines the oldest SM bus version in which the current version is
* fully backwards compatible. If you write app that supports SM bus versions between N and M (i.e. N=20 M=25),
* then check that connected device complies with the test (pseudo code):
*
*
smuint32 NEWEST_SUPPORTED_SMV2_VERSION=M;
smuint32 OLDEST_SUPPORTED_SMV2_VERSION=N;
smuint32 smv2version=getParam(SMP_SM_VERSION);
smuint32 smv2compatversion=getParam(SMP_SM_COMPAT_VERSION);
if(smv2version>NEWEST_SUPPORTED_SMV2_VERSION && smv2compatversion>NEWEST_SUPPORTED_SMV2_VERSION)
{
error("SimpleMotion protocol version too new, not supported by this software version. Check for upgrades. Connected SMV2 Version: " + smv2version);
return false;
}
if(smv2version<OLDEST_SUPPORTED_SMV2_VERSION && smv2compatversion < OLDEST_SUPPORTED_SMV2_VERSION)
{
error("SimpleMotion protocol version too old, not supported by this software version. Try older versions. Connected SMV2 Version: " + smv2version);
return false;
}
*
* If test fails, then connected device may not be compatible with your app.
*
* Its strongly recommended to read these paramers and verify compatibility before any other actions
* and do same kind of test also with SMP_FIRMWARE_VERSION and SMP_FIRMWARE_BACKWARDS_COMP_VERSION.
*/
#ifndef SIMPLEMOTION_DEFS_H_
#define SIMPLEMOTION_DEFS_H_
/*
* bit shift macros
*/
#ifndef BV
//bitvise shifts
#define BV(bit) (1<<(bit))
#define BVL(bit) (1L<<(bit))
#endif
/*
* SimpleMotion V2 constants
*/
/* broadcast address, if packet is sent to SM_BROADCAST_ADDR then all nodes in the network
* will read it but no-one will send a reply because it would cause packet collision*/
#define SM_BROADCAST_ADDR 0
//these bits define attributes over parameter address to retreive different types of values as return data
#define SMP_VALUE_MASK 0x0000
#define SMP_MIN_VALUE_MASK 0x4000 //so requesting i.e. param (SMP_TIMEOUT|SMP_MIN_VALUE_MASK) will give the minimum settable value for param SMP_TIMEOUT
#define SMP_MAX_VALUE_MASK 0x8000
//mask to filter attributes
#define SMP_ATTRIBUTE_BITS_MASK 0xC000//C=1100
//mask for addresses
#define SMP_ADDRESS_BITS_MASK 0x1FFF //E=1110
/*
* SM payload command types and return value types.
*/
//SMP packet header bits (2 bits). These determine content length and type
#define SMPCMD_SETPARAMADDR 2 //Deprecated name, dont use these. Use new names seen below
#define SMPCMD_SET_PARAM_ADDR 2 //Deprecated name, dont use these. Use new names seen below
#define SMPCMD_24B 1 //Deprecated name, dont use these. Use new names seen below
#define SMPCMD_32B 0 //Deprecated name, dont use these. Use new names seen below
#define SMPRET_CMD_STATUS 3 //Deprecated name, dont use these. Use new names seen below
#define SMPRET_OTHER 3 //Deprecated name, dont use these. Use new names seen below
#define SMPRET_16B 2 //Deprecated name, dont use these. Use new names seen below
#define SMPRET_24B 1 //Deprecated name, dont use these. Use new names seen below
#define SMPRET_32B 0 //Deprecated name, dont use these. Use new names seen below
//
//new naming for the above values (more clear meanings):
//
//OUTBOUND SM COMMAND PAYLOAD SUBPACKET TYPES
//each SMCMD_INSTANT_CMD and SMCMD_BUFFERED_CMD payload is filled with subpackets of following types
//sets param address (one of SMP_ defines) where next written value goes to.
//consumes 2 bytes from payload buffer.
#define SM_SET_WRITE_ADDRESS 2
//writes value to previously defined address. consumes 3 bytes (24 bits) from payload buffer.
//can hold value size up to 22 bits. bits above 22 are clipped (transmitted as 0s)
#define SM_WRITE_VALUE_24B 1
//writes value to previously defined address. consumes 4 bytes (32 bits) from payload buffer.
//can hold value size up to 30 bits. bits above 30 are clipped (transmitted as 0s)
#define SM_WRITE_VALUE_32B 0
//INBOUND SM COMMAND PAYLOAD SUBPACKET TYPES
//each outbound subpacket will return one of these. the type and address what is being returned
//is defined by parameters SMP_RETURN_PARAM_ADDR and SMP_RETURN_PARAM_LEN
//return value contains SM bus status bits (possible faults etc).
//consumes 1 byte from payload buffer
#define SM_RETURN_STATUS 3
//return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR
//consumes 2 byte from payload buffer. can contain value up to 14 bits. value greater than 14 bits is clipped (padded with 0s)
#define SM_RETURN_VALUE_16B 2
//return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR
//consumes 3 byte from payload buffer. can contain value up to 14 bits. value greater than 22 bits is clipped (padded with 0s)
#define SM_RETURN_VALUE_24B 1
//return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR
//consumes 4 byte from payload buffer. can contain value up to 30 bits. value greater than 14 bits is clipped (padded with 0s)
#define SM_RETURN_VALUE_32B 0
/* subpacket format
*
* header:
* 2 bits packet type which defines also lenght of subpacket
* data:
* rest of subpacket contains the transmitted value
*
* example:
* SM_WRITE_VALUE_32B packet looks like this
*
* -2 first bits are 00
* -next 30 bits carry the value
*
* SMV2 library takes care of forming subpackets so user only needs to care about using the macros above
*
*/
//if command is to set param addr to this -> NOP
#define SMP_ADDR_NOP 0x3fff
//SMPCommand values that are returned with every SMPCommand when SMPRET_OTHER set as SMP_RETURN_PARAM_LEN:
#define SMP_CMD_STATUS_ACK 0
#define SMP_CMD_STATUS_NACK 1
#define SMP_CMD_STATUS_INVALID_ADDR 2
#define SMP_CMD_STATUS_INVALID_VALUE 4
#define SMP_CMD_STATUS_VALUE_TOO_HIGH 8
#define SMP_CMD_STATUS_VALUE_TOO_LOW 16
#define SMP_CMD_STATUS_OTHER_MASK32 (3L<<30) //1byte packet. payload codes:
//params 0-63 are reserved for SM specific params, application specifics are >64-8090
#define SMP_SM_RESERVED_ADDRESS_SPACE 63
//SMP=Simple motion parameter
#define SMP_NULL 0
#define SMP_NODE_ADDRSS 1
#define SMP_BUS_MODE 2
//possible values:
#define SMP_BUS_MODE_DFU 0
#define SMP_BUS_MODE_NORMAL 1
#define _SMP_BUS_MODE_LAST 1
/* SMP_SM_VERSION returns current SM protocol version */
#define SMP_SM_VERSION 3
/* SMP_SM_VERSION_COMPAT defines the oldest SM bus version in which the current version is
* fully backwards compatible. */
#define SMP_SM_VERSION_COMPAT 4
#define SMP_BUS_SPEED 5
#define SMP_BUFFER_FREE_BYTES 6
#define SMP_BUFFERED_CMD_STATUS 7
#define SMP_BUFFERED_CMD_PERIOD 8
#define SMP_RETURN_PARAM_ADDR 9
#define SMP_RETURN_PARAM_LEN 10
/*SMP_TIMOUT defines how long device waits for one packet to transmit before discarding it. unit 0.1 milliseconds*/
#define SMP_TIMEOUT 12
#define SMP_CUMULATIVE_STATUS 13 //error bits are set here if any, (SMP_CMD_STATUS_... bits). clear by writing 0
#define SMP_ADDRESS_OFFSET 14 /*used to set or offset device address along physical method, i.e. DIP SW + offset to allow greater range of addresses than switch allows. */
/* SMP_FAULT_BEHAVIOR defines maximum amount of time between to valid received SM packets to device and other SM
* fault behavior that affect drive operation.
*
* If comm is broken longer than watchdog time, drive will go fault stop state.
* Can be used for additional safety stop when drives are controlled only onver SM bus.
*
* Parameter is bit field:
* bit 0 (LSB): enable device fault stop on any SM comm error (CRC, invalid value etc)
* bits 1-7: reserved, always 0
* bits 8-17: watchdog timeout value. nonzero enables watchdog. scale: 1 count=10ms, so allows 0.01-10.230 s delay.
* bits 18-32: reserved, always 0
*/
#define SMP_FAULT_BEHAVIOR 15
//bit mask
#define SM_BUFCMD_STAT_IDLE 1
#define SM_BUFCMD_STAT_RUN 2
#define SM_BUFCMD_STAT_UNDERRUN 4
#define SM_BUFCMD_STAT_OVERRUN 8
//each DIGITAL variable is 16 bits thus holds 16 i/o's
#define SMP_DIGITAL_IN_VALUES_1 128
#define SMP_DIGITAL_IN_VALUES_2 129
#define SMP_DIGITAL_IN_VALUES_3 130
#define SMP_DIGITAL_IN_VALUES_4 131
#define SMP_DIGITAL_IN_CHANGED_1 132
#define SMP_DIGITAL_IN_CHANGED_2 133
#define SMP_DIGITAL_IN_CHANGED_3 134
#define SMP_DIGITAL_IN_CHANGED_4 135
#define SMP_DIGITAL_OUT_VALUE_1 136
#define SMP_DIGITAL_OUT_VALUE_2 137
#define SMP_DIGITAL_OUT_VALUE_3 138
#define SMP_DIGITAL_OUT_VALUE_4 139
//set IO pin direction: 0=input, 1=output. May not be supported by HW.
#define SMP_DIGITAL_IO_DIRECTION_1 140
#define SMP_DIGITAL_IO_DIRECTION_2 141
#define SMP_DIGITAL_IO_DIRECTION_3 142
#define SMP_DIGITAL_IO_DIRECTION_4 143
#define SMP_ANALOG_IN_VALUE_1 168
#define SMP_ANALOG_IN_VALUE_2 169
#define SMP_ANALOG_IN_VALUE_3 170
#define SMP_ANALOG_IN_VALUE_4 171
#define SMP_ANALOG_IN_VALUE_5 172
// continue to .. 183
#define SMP_ANALOG_OUT_VALUE_1 184
#define SMP_ANALOG_OUT_VALUE_2 185
#define SMP_ANALOG_OUT_VALUE_3 186
#define SMP_ANALOG_OUT_VALUE_4 187
#define SMP_ANALOG_OUT_VALUE_5 188
// continue to .. 200
/*
* List of motor control parameters.
*
* WARNING: Don't assume anything about these variables! Refer to SimpleMotion V2 documentation
* for details. If unknown, ask Granite Devices.
*/
//control setpoint values (pos, vel or torq depending on control mode):
#define SMP_INCREMENTAL_SETPOINT 550
#define SMP_ABSOLUTE_SETPOINT 551
//old names for backwards compatibility:
#define SMP_INCREMENTAL_POS_TARGET SMP_INCREMENTAL_SETPOINT
#define SMP_ABSOLUTE_POS_TARGET SMP_ABSOLUTE_SETPOINT
#define SMP_FAULTS 552
//bitfield bits:
#define FLT_FOLLOWERROR BV(1)
#define FLT_OVERCURRENT BV(2)
#define FLT_COMMUNICATION BV(3)
#define FLT_ENCODER BV(4)
#define FLT_OVERTEMP BV(5)
#define FLT_UNDERVOLTAGE BV(6)
#define FLT_OVERVOLTAGE BV(7)
#define FLT_PROGRAM_OR_MEM BV(8)
#define FLT_HARDWARE BV(9)
#define FLT_OVERVELOCITY BV(10)
#define FLT_INIT BV(11)
#define FLT_MOTION BV(12)
#define FLT_RANGE BV(13)
#define FLT_PSTAGE_FORCED_OFF BV(14)
#define FLT_HOST_COMM_ERROR BV(15)
//IO side macros
#define FLT_GC_COMM BV(15)
#define FLT_QUEUE_FULL FLT_PROGRAM_OR_MEM
#define FLT_SM485_ERROR FLT_COMMUNICATION
#define FLT_FIRMWARE FLT_PROGRAM_OR_MEM //non-recoverable program error
#define FLT_ALLOC FLT_PROGRAM_OR_MEM //memory etc allocation failed
#define SMP_FIRST_FAULT 8115
#define SMP_STATUS 553
//bitfield bits:
#define STAT_RESERVED_ BV(0)
#define STAT_TARGET_REACHED BV(1)//this is 1 when trajectory planner target reached
#define STAT_FERROR_RECOVERY BV(2)
#define STAT_RUN BV(3)//run is true only if motor is being actually driven. run=0 clears integrators etc
#define STAT_ENABLED BV(4)
#define STAT_FAULTSTOP BV(5)
#define STAT_FERROR_WARNING BV(6)//follow error warning, recovering or disabled
#define STAT_STO_ACTIVE BV(7)
#define STAT_SERVO_READY BV(8)//ready for user command: initialized, running (no fault), not recovering, not homing & no homing aborted, not running sequence
#define STAT_BRAKING BV(10)
#define STAT_HOMING BV(11)
#define STAT_INITIALIZED BV(12)
#define STAT_VOLTAGES_OK BV(13)
#define STAT_PERMANENT_STOP BV(15)//outputs disabled until reset
#define SMP_SYSTEM_CONTROL 554 //writing 1 initiates settings save to flash, writing 2=device restart, 4=abort buffered motion
//possible values listed
//save active settings to flash:
#define SMP_SYSTEM_CONTROL_SAVECFG 1
//restart drive:
#define SMP_SYSTEM_CONTROL_RESTART 2
//abort buffered commands (discard rest of buffer)
#define SMP_SYSTEM_CONTROL_ABORTBUFFERED 4
//next 3 for factory use only:
#define SMP_SYSTEM_SAMPLE_TEST_VARIABLES 8 //production testing function
#define SMP_SYSTEM_START_PRODUCTION_TEST 16 //production testing function
#define SMP_SYSTEM_STOP_PRODUCTION_TEST 32 //production testing function
//restart device into DFU mode
#define SMP_SYSTEM_CONTROL_RESTART_TO_DFU_MODE 64
//read miscellaneous device specific flag bits
#define SMP_SYSTEM_CONTROL_GET_FLAGS 128
/*start measure motor RL function.
* poll debug param 5 for state
* -100 fail
* 700 busy
* 800 success -new RL values inserted in drive params
*
* SMP_STATUS must obey:
* requiredStats=STAT_INITIALIZED|STAT_RUN|STAT_ENABLED|STAT_VOLTAGES_OK;
deniedStats=STAT_FERROR_RECOVERY|STAT_PERMANENT_STOP|STAT_STO_ACTIVE|STAT_FAULTSTOP|STAT_HOMING;
*/
#define SMP_SYSTEM_CONTROL_MEASURE_MOTOR_RL 256
//resets position mode FB and setpoint values to 0, and also resets homing status. useful after using in vel or torq mode.
#define SMP_SYSTEM_CONTROL_RESET_FB_AND_SETPOINT 512
//follow error tolerance for position control:
#define SMP_POS_FERROR_TRIP 555
//velocity follow error trip point. units: velocity command (counts per PIDcycle * divider)
#define SMP_VEL_FERROR_TRIP 556
//recovery velocity from disabled/fault state. same scale as other velocity limits
#define SMP_POS_ERROR_RECOVERY_SPEED 557
//motor type:
#define SMP_MOTOR_MODE 558
//factory state, no choice:
#define MOTOR_NONE 0
//DC servo:
#define MOTOR_DC 1
//2 phase AC motor
#define MOTOR_AC_VECTOR_2PHA 2 /*2 phase ac or bldc */
//3 phase AC motor
#define MOTOR_AC_VECTOR 3 /*3 phase ac or bldc */
//2 phase stepper
#define MOTOR_STEPPER_2PHA 4 /*2 phase stepper */
//for drive internal use only:
#define _MOTOR_LAST 7
//position follow error trip point
//velocity follow error trip point. units: velocity command (counts per PIDcycle * divider)
//#define CFG_INPUT_FILTER_LEN 30
#define SMP_CONTROL_MODE 559
//control mode choices:
#define CM_TORQUE 3
#define CM_VELOCITY 2
#define CM_POSITION 1
#define CM_NONE 0
#define SMP_INPUT_MULTIPLIER 560
#define SMP_INPUT_DIVIDER 561
#define SMP_INPUT_REFERENCE_MODE 562
//choices:
#define SMP_INPUT_REFERENCE_MODE_SERIALONLY 0
#define SMP_INPUT_REFERENCE_MODE_PULSETRAIN 1
#define SMP_INPUT_REFERENCE_MODE_QUADRATURE 2
#define SMP_INPUT_REFERENCE_MODE_PWM 3
#define SMP_INPUT_REFERENCE_MODE_ANALOG 4
#define SMP_INPUT_REFERENCE_MODE_RESERVED1 5
#define SMP_INPUT_REFERENCE_MODE_RESERVED2 6
#define SMP_INPUT_REFERENCE_MODE_LAST_ 6
//setpoint source offset when absolute setpoint is used (pwm/analog)
#define SMP_ABS_IN_OFFSET 563
//scaler, not used after VSD-E
#define SMP_ABS_IN_SCALER 564
//FB1 device resolution, pulses per rev
#define SMP_ENCODER_PPR 565
//motor polepairs, not used with DC motor
#define SMP_MOTOR_POLEPAIRS 566
//flag bits & general
#define SMP_DRIVE_FLAGS 567
//bitfield bits:
#define FLAG_DISABLED_AT_STARTUP BV(0)
#define FLAG_NO_DCBUS_FAULT BV(1)/*obsolete?*/
#define FLAG_ENABLE_DIR_INPUT_ON_ABS_SETPOINT BV(2) /*if 1, then use direction input signal for analog and PWM setpoint mode*/
#define FLAG_INVERT_ENCODER BV(3)
#define FLAG_INVERT_MOTOR_DIRECTION BV(4) /*invert positive direction*/
#define FLAG_ENABLE_REQUIRES_PULSING BV(5) /*enable signal must be fed 50hz-1khz toggling signal to validate enable status*/
#define FLAG_USE_PULSE_IN_ACCEL_LIMIT BV(7)/*obsolete*/
#define FLAG_2PHASE_AC_MOTOR BV(9)/*obsolete*/
#define FLAG_ALLOW_VOLTAGE_CLIPPING BV(10)
#define FLAG_USE_INPUT_LP_FILTER BV(11)
#define FLAG_USE_PID_CONTROLLER BV(12)//PIV is the default if bit is 0/*obsolete*/
#define FLAG_INVERTED_HALLS BV(13)
#define FLAG_USE_HALLS BV(14)
#define FLAG_MECH_BRAKE_DURING_PHASING BV(15)
#define FLAG_LIMIT_SWITCHES_NORMALLY_CLOSED_TYPE BV(16)
#define SMP_MOTION_FAULT_THRESHOLD 568
#define SMP_HV_VOLTAGE_HI_LIMIT 569
#define SMP_HV_VOLTAGE_LOW_LIMIT 570
#define SMP_MAX_OUTPUT_POWER 571 //in watts
#define SMP_MOTOR_MAX_SPEED 572
//in Ioni only
#define SMP_ELECTRICAL_MODE 573
#define EL_MODE_STANDARD 0
#define EL_MODE_IONICUBE 1
#define EL_MODE_IONIZER 2
//primary feedback loop 200-299
#define SMP_VEL_I 200
#define SMP_POS_P 201
#define SMP_VEL_P 202
#define SMP_VEL_FF 220
#define SMP_ACC_FF 221
#define SMP_POS_FF 222
//anti dither limits
#define SMP_ANTIDITHER_MODE 230
//secondary feedback loop 300-399
//NOT IMPLEMENTED YET
//current loop related 400-499
//direct torque gains, not used after VSD-E
#define SMP_TORQ_P 401
#define SMP_TORQ_I 402
//motor inductance and resistance
#define SMP_MOTOR_RES 405
#define SMP_MOTOR_IND 406
//continuous current limit mA
#define SMP_TORQUELIMIT_CONT 410
//peak current limit mA
#define SMP_TORQUELIMIT_PEAK 411
//homing current limit mA
#define SMP_TORQUELIMIT_HOMING 415
//next 2 set fault sensitivity
#define SMP_TORQUEFAULT_MARGIN 420
#define SMP_TORQUEFAULT_OC_TOLERANCE 421
/* next four parameters allow compensation of motor detent torque and torque ripple (cogging torque).
* xxx_TORQUE_FUNCTION and xxx_TORQUE_AMPLITUDE sets function, value range is 0-25 and used like:
*
* if((function%1)==0) xxx_torque_modifier=sin(electrical_angle*(function/2))*amplitude;
* else xxx_torque_modifier=cos(electrical_angle*(function/2))*amplitude;
*
* i.e. functions go like:
* 0=no effect (sin(0a))
* 1=constant (cos(0a))
* 2=sin(1a)
* 3=cos(1a)
* 4=sin(2a)
* 5=cos(a2)
* 6=sin(3a)
* 7=cos(3a)
*
*
* Detent torque compensation value (static_torque modifier) is added to the original torque setpoint.
* Ripple torque compensation value (dynamic_torque modifier) will modulate the original torque setpoint.
* Overall formula how these affect to troque output is:
*
* torqueSetpoint=torqueSetpoint*(dynamic_torque_modifier+1) +static_torque_modifier;
*/
#define SMP_MOTOR_STATIC_TORQUE_FUNCTION 424
#define SMP_MOTOR_STATIC_TORQUE_AMPLITUDE 425
#define SMP_MOTOR_DYNAMIC_TORQUE_FUNCTION 426
#define SMP_MOTOR_DYNAMIC_TORQUE_AMPLITUDE 427
//motor thermal time constant in seconds:
#define SMP_MOTOR_THERMAL_TIMECONSTANT 430
//how fast to ramp up voltage during phase search:
#define SMP_PHASESEARCH_VOLTAGE_SLOPE 480
//by default this is calculated from other motor params:
#define SMP_PHASESEARCH_CURRENT 481
//selector value 0-9 = 100 - 3300Hz (see Granity):
#define SMP_TORQUE_LPF_BANDWIDTH 490
//motor rev to rotary/linear unit scale. like 5mm/rev or 0.1rev/rev. 30bit parameter & scale 100000=1.0
#define SMP_AXIS_SCALE 491
//output unit 0=mm 1=um 2=inch 3=revolutions 4=degrees
#define SMP_AXIS_UNITS 492
//0=none 1=qei1 2=qei2 3=resolver 4=ssi 5=biss
#define SMP_FB1_DEVICE_SELECTION 493
#define SMP_FBD_NONE 0
#define SMP_FBD_INCR1 1
#define SMP_FBD_INCR2 2
#define SMP_FBD_RESOLVER 3
#define SMP_FBD_SSI 4
#define SMP_FBD_BISS 5
#define SMP_FBD_SINCOS16X 6
#define SMP_FBD_SINCOS64X 7
#define SMP_FBD_SINCOS256X 8
#define SMP_FB2_DEVICE_SELECTION 494
//in 1/2500 seconds.
#define SMP_GOAL_FAULT_FILTER_TIME 495
//in speed scale
#define SMP_OVERSPEED_FAULT_LIMIT 496
//0=min 6=max
#define SMP_OVERCURRENT_FAULT_SENSITIVITY 497
//when hit limt switch, do: 0=nothing, 1=disable torque, 2=fault stop, 3=servoed stop
#define SMP_LIMIT_SW_FUNCTION 498
//choices:
#define SMP_LIMIT_SW_DISABLED 0
#define SMP_LIMIT_SW_NOTORQUE 1
#define SMP_LIMIT_SW_FAULTSTOP 2
#define SMP_LIMIT_SW_SERVOSTOP 3
#define _SMP_LIMIT_SW_LAST 3
#define SMP_CONTROL_BITS1 2533
//bitfiled values:
//CB1 & CB2 enables must be 1 to have drive enabled
//Control bits 1 are controlled by host software
#define SMP_CB1_ENABLE BV(0)//software enable
#define SMP_CB1_CLEARFAULTS BV(1)
#define SMP_CB1_QUICKSTOP BV(2)//not implemented at the moment
#define SMP_CB1_USE_TRAJPLANNER BV(3)//not implemented at the moment
#define SMP_CB1_START_HOMING BV(4)//write 1 here to start homing //not implemented at the moment
#define SMP_STATIC_CBS1 (SMP_CB1_ENABLE|SMP_CB1_USE_TRAJPLANNER)
#define SMP_CONTROL_BITS2 2534
//bitfiled values:
//both enables must be 1 to have drive enabled
//Control bits 2 are controlled by physical inputs, dont try to write on these thu SMV2. reading is ok
#define SMP_CB2_ENABLE BV(0)//hadrware enable pin
#define SMP_CB2_ENA_POS_FEED BV(1)
#define SMP_CB2_ENA_NEG_FEED BV(2)
#define SMP_CB2_HOMESW_ON BV(3)
#define SMP_CB2_CLEARFAULTS BV(4)
#define SMP_CB2_START_HOMING BV(5)
//trajectory planner
#define SMP_TRAJ_PLANNER_ACCEL 800
#define SMP_TRAJ_PLANNER_STOP_DECEL 801
#define SMP_TRAJ_PLANNER_VEL 802
//accel limit for homing & ferror recovery
//if set to 0, use CFG_TRAJ_PLANNER_ACCEL instead (for backwards compatibility)
#define SMP_TRAJ_PLANNER_HOMING_ACCEL 803
#define SMP_TRAJ_PLANNER_HOMING_VEL 804
#define SMP_TRAJ_PLANNER_HOMING_BITS 806
//homing config bit field. illegal to set Not to use INDEX or HOME
#define HOMING_POS_INDEX_POLARITY BV(0)
#define HOMING_POS_INDEX_SEARCH_DIRECTION BV(1)
#define HOMING_POS_HOME_SWITCH_POLARITY BV(2)
#define HOMING_USE_HOME_SWITCH BV(3)
#define HOMING_USE_INDEX_PULSE BV(4)
#define HOMING_HOME_AT_POWER_ON BV(5)
#define HOMING_FULL_SPEED_OFFSET_MOVE BV(6)
#define HOMING_ENABLED BV(7) /*if 0, homing cant be started */
#define _HOMING_CFG_MAX_VALUE 0x00ff
//starting and stopping homing:
#define SMP_HOMING_CONTROL 7532
//hard stop triggered by follow error in encoder counts
#define SMP_TRAJ_PLANNER_HOMING_HARD_STOP_THRESHOLD 808
//32bit position offset in encoder counts, value must not overflow on divider*offset
#define SMP_TRAJ_PLANNER_HOMING_OFFSET 823
//signed 32bit absolute position limits, active after homing. disabled when both=0
#define SMP_ABSPOSITION_HI_LIMIT 835
#define SMP_ABSPOSITION_LO_LIMIT 836
//readouts
#define SMP_ACTUAL_BUS_VOLTAGE 900
#define SMP_ACTUAL_TORQUE 901
#define SMP_ACTUAL_VELOCITY_FB 902
#define SMP_ACTUAL_POSITION_FB 903
#define SMP_SCOPE_CHANNEL_VALUE 904
#define SMP_SCOPE_CHANNEL_SELECT 905
#define SMP_MECH_BRAKE_RELEASE_DELAY 910
#define SMP_MECH_BRAKE_ENGAGE_DELAY 911
#define SMP_DYNAMIC_BRAKING_SPEED 912
#define SMP_BRAKE_STOP_ENGAGE_DELAY SMP_MECH_BRAKE_ENGAGE_DELAY /* SMP_BRAKE_STOP_ENGAGE_DELAY is old name, kept for compatibility*/
/*Special torque mode specific settings, currently IONI only. This paramterer is a bitfield
* defined as: bits 0-7: option bits, bits 8-29: rotation angle limit (in feedback counts).
*
* Guide:
* 0 = disabled, standard drive operation
* nonzero = calculate value by: round(allowed_rotation_in_counts/256)*256 + modebits
*
* where modebits is a bitfield defined as:
* bit 0: input scaling. if true, analog input 0-5V sets torque scale to 10-100%. Note: cant use analog input for torque setpoint now.
* bits 1-7: reserved for future use.
*
* example 1: we want rotation limit of 8000 encoder counts (for each direction) and no scaling input, we
* use value 7936.
*
* example 2: we want rotation limit of 8000 encoder counts (for each direction) and torque
* command scaling input, we use value 7937
* example 3: no rotation limit but just saling is on: use value 1
*
* Tip: use Granity Ctrl+P (Shortcut) dialog to set value:
* http://granitedevices.com/wiki/Reading_and_writing_an_arbitrary_parameter_with_Granity
*/
#define SMP_TORQUE_MODE_ATTRIBUTES 920
//////////////////////////////////////////////////////////////////////////////////RUNTIME PARAMS 5000-9999
#define SMP_CAPTURE_SOURCE 5020
//bitfield values (shift these with BV())
#define CAPTURE_TORQUE_TARGET 1
#define CAPTURE_TORQUE_ACTUAL 2
#define CAPTURE_VELOCITY_TARGET 3
#define CAPTURE_VELOCITY_ACTUAL 4
#define CAPTURE_POSITION_TARGET 5
#define CAPTURE_POSITION_ACTUAL 6
#define CAPTURE_FOLLOW_ERROR 7
#define CAPTURE_OUTPUT_VOLTAGE 8
#define CAPTURE_BUS_VOLTAGE 9
#define CAPTURE_STATUSBITS 10
#define CAPTURE_FAULTBITS 11
#define CAPTURE_P_OUT 12
#define CAPTURE_I_OUT 13
#define CAPTURE_D_OUT 14
#define CAPTURE_FF_OUT 15
#define CAPTURE_RAW_POS 25
//8 bit signed values combined, only for return data, not for scope
#define CAPTURE_TORQ_AND_FERROR 26
//rest are availalbe in debug/development firmware only:
#define CAPTURE_PWM1 16
#define CAPTURE_PWM2 17
#define CAPTURE_PWM3 18
#define CAPTURE_DEBUG1 19
#define CAPTURE_DEBUG2 20
#define CAPTURE_CURRENT1 21
#define CAPTURE_CURRENT2 22
#define CAPTURE_ACTUAL_FLUX 23
#define CAPTURE_OUTPUT_FLUX 24
#define SMP_CAPTURE_TRIGGER 5011
//choices:
#define TRIG_NONE 0
#define TRIG_INSTANT 1
#define TRIG_FAULT 2
#define TRIG_TARGETCHANGE 3
#define TRIG_TARGETCHANGE_POS 4
#define TRIG_SERIALCMD 5
#define SMP_CAPTURE_SAMPLERATE 5012
//rdonly
#define SMP_CAPTURE_BUF_LENGHT 5013
//this is looped 0-n to make samples 0-n readable from SMP_CAPTURE_BUFFER_GET_VALUE
#define SMP_CAPTURE_BUFFER_GET_ADDR 5333
#define SMP_CAPTURE_BUFFER_GET_VALUE 5334
//#define RUNTIME_FEATURES1 6000
#define SMP_SERIAL_NR 6002
#define SMP_UID_NR 6003
#define SMP_DRIVE_CAPABILITIES 6006
#define SMP_FIRMWARE_VERSION 6010
#define SMP_FIRMWARE_BACKWARDS_COMP_VERSION 6011
#define SMP_GC_FIRMWARE_VERSION 6014
#define SMP_GC_FIRMWARE_BACKWARDS_COMP_VERSION 6015
//added in ioni:
#define SMP_FIRMWARE_BUILD_REVISION 6016
#define SMP_DEVICE_TYPE 6020
#define SMP_PID_FREQUENCY 6055
//affects only in MC_VECTOR & MC_BLDC mode
//used in drive development only, do not use if unsure
#define SMP_OVERRIDE_COMMUTATION_ANGLE 8000
#define SMP_TORQUE_CMD_OVERRIDE 8001
#define SMP_OVERRIDE_COMMUTATION_FREQ 8003
#define SMP_OVERRIDE_REGENRES_DUTY 8004
#define SMP_DEVICE_TEMPERATURE 8007//reported in 0.01 celsius steps
#define SMP_CURRENT_LIMITED_TO_MA 8008//actual current limit (based on user settings, device temperature, voltage etc)
#define SMP_CURRENT_LIMIT_REASON 8009 //last reason why current was clampled
#define CURR_LIMIT_REASON_NONE 0
#define CURR_LIMIT_REASON_VOLTAGE_SATURATION 1
#define CURR_LIMIT_REASON_SETTINGS 2
#define CURR_LIMIT_REASON_I2T 3
#define CURR_LIMIT_REASON_DRIVE_TEMPERATURE 4
#define CURR_LIMIT_REASON_POWER_CAP 5
/*IO side CPU sends encoder counter at index every time index is encountered. homing uses this info */
#define SMP_INDEX_PULSE_LOCATION 8005
//GC side debug params
#define SMP_DEBUGPARAM1 8100
#define SMP_DEBUGPARAM2 8101
#define SMP_DEBUGPARAM3 8102
//for STM32 side override
#define SMP_DEBUGPARAM4 8103
#define SMP_DEBUGPARAM5 8104
#define SMP_DEBUGPARAM6 8105
#define SMP_FAULT_LOCATION1 8110 //this is for GraniteCore side
#define SMP_FAULT_LOCATION2 8111 //this is for Argon IO side
#define SMP_OVERRIDE_PARAM_ADDR_1 8120
#define SMP_OVERRIDE_PARAM_ADDR_2 8121
#define SMP_OVERRIDE_PARAM_ADDR_3 8122
#define SMP_OVERRIDE_PARAM_VAL_1 8125
#define SMP_OVERRIDE_PARAM_VAL_2 8126
#define SMP_OVERRIDE_PARAM_VAL_3 8127
/*DFU Bootloader parameters begin:*/
//SMP_FAULTS bitfield bits:
#define FLT_INVALID_FW BV(5)
#define FLT_FLASHING_FAIL BV(6)
#define FLT_FLASHING_COMMSIDE_FAIL BV(7)
//SMP_STATUS bitfield bits:
#define STAT_GC_FLASHED_ONCE BV(1)//to prevent flashing it twice in same GCBL session which causes error
//write a value here to execute a bootloader function
#define SMP_BOOTLOADER_FUNCTION 191
//upload 16bits of data to buffer. max buffer length 4096 bytes before it must be written with "write block" function
//total uploaded amount must be multiple of 4 bytes before issuing write function
#define SMP_BOOTLOADER_UPLOAD 192
//bootloaded status
#define SMP_BOOTLOADER_STAT 193
#define BOOTLOADER_STAT_FLASH_VERIFIED_OK BV(0)
#define SMP_BOOTLOADER_WRITE_OTP 194
/*DFU Bootloader parameters end*/
#endif /* SIMPLEMOTION_DEFS_H_ */

121
software/ros_packages/rover_arm/src/simplemotion/simplemotion_private.h Normal file
View File

@@ -0,0 +1,121 @@
//Internal functions & definitions, not for library user
//Copyright (c) Granite Devices Oy
/*
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; version 2 of the License.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
*/
#ifndef SIMPLEMOTION_PRIVATE_H
#define SIMPLEMOTION_PRIVATE_H
#include "simplemotion.h"
#include "busdevice.h"
#include <stdio.h>
#define SM_VERSION 0x020001
//max number of simultaneously opened buses. change this and recompiple SMlib if
//necessary (to increase channels or reduce to save memory)
#define SM_MAX_BUSES 10
//bus device types
#define BUSDEV_NONE 0
#define BUSDEV_RS 1 /* rs232 like com port support */
#define BUSDEV_FTDI 2 /*not implemented yet: direct FTDI lib support*/
#define SM_BUSDEVICENAME_LEN 64
//default timeout in ms
//Argon drive's worst case response time should be ~20ms with max length packets
#define SM_READ_TIMEOUT 500
extern const smuint8 table_crc16_hi[];
extern const smuint8 table_crc16_lo[];
extern const smuint8 table_crc8[];
extern FILE *smDebugOut; //such as stderr or file handle. if NULL, debug info disbled
extern smuint16 readTimeoutMs;
void smDebug( smbus handle, smVerbosityLevel verbositylevel, char *format, ...);
SM_STATUS smRawCmd( const char *axisname, smuint8 cmd, smuint16 val, smuint32 *retdata );
typedef struct {
/* ID=0 param size 30 bits (cmd total 4 bytes)
* ID=1 param size 22 bits (cmd total 3 bytes)
* ID=2 set parameter store address, param: 14 bits=register address (cmd total 2 bytes)
* ID=3 reserved
*/
long param :30; //LSB 30 bits
long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID
} __attribute__ ((packed)) SMPayloadCommand32;
typedef struct {
/* ID=0 param size 30 bits (cmd total 4 bytes)
* ID=1 param size 22 bits (cmd total 3 bytes)
* ID=2 set parameter store address, param: 14 bits=register address (cmd total 2 bytes)
* ID=3 reserved
*/
long param :14; //LSB 30 bits
long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID
} __attribute__ ((packed)) SMPayloadCommand16;
typedef struct {
/* ID=0 param size 30 bits (cmd total 4 bytes)
* ID=1 param size 22 bits (cmd total 3 bytes)
* ID=2 set parameter store address, param: 14 bits=register address (cmd total 2 bytes)
* ID=3 reserved
*/
long param :22; //MSB 30 bits
long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID
} __attribute__ ((packed)) SMPayloadCommand24;
//SM payload command return data structure
typedef struct {
/* ID=0 ret data 30 bits (tot 4 bytes)
* ID=1 ret data 22 bits (tot 3 bytes)
* ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK.
* ID=3 reserved
*/
long retData: 30; //LSB 30 bits
long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID
} __attribute__ ((packed)) SMPayloadCommandRet32;
//SM payload command return data structure
typedef struct {
/* ID=0 ret data 30 bits (tot 4 bytes)
* ID=1 ret data 22 bits (tot 3 bytes)
* ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK.
* ID=3 reserved
*/
long retData: 22; //LSB 30 bits
long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID
} __attribute__ ((packed)) SMPayloadCommandRet24;
//SM payload command return data structure
typedef struct {
/* ID=0 ret data 30 bits (tot 4 bytes)
* ID=1 ret data 22 bits (tot 3 bytes)
* ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK.
* ID=3 reserved
*/
long retData: 14; //LSB 30 bits
long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID
} __attribute__ ((packed)) SMPayloadCommandRet16;
//SM payload command return data structure
typedef struct {
/* ID=0 ret data 30 bits (tot 4 bytes)
* ID=1 ret data 22 bits (tot 3 bytes)
* ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK.
* ID=3 reserved
*/
long retData: 6; //LSB 30 bits
long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID
} __attribute__ ((packed)) SMPayloadCommandRet8;
#endif // SIMPLEMOTION_PRIVATE_H

79
software/ros_packages/rover_arm/src/simplemotion/sm485.h Normal file
View File

@@ -0,0 +1,79 @@
//VSD SM485 bus definitions
#ifndef SM485_H
#define SM485_H
//SERIAL COMMANDS
#define SM485_CRCINIT 0x0
#define SM485_BUFSIZE 128
#define SM485_RSBUFSIZE 128
#define SM485_CMDBUFSIZE 4096
//cmd number must be 0-31
#define SM485_VERSION 90
#define SM485_VERSION_COMPAT 85
#define SM485_MAX_PAYLOAD_BYTES 120
//00xb
#define SMCMD_MASK_0_PARAMS 0
//01xb, 2 bytes as payload
#define SMCMD_MASK_2_PARAMS 2
//10xb
#define SMCMD_MASK_N_PARAMS 4
//11xb
#define SMCMD_MASK_RESERVED 6
//xx1b
#define SMCMD_MASK_RETURN 1
//110b
#define SMCMD_MASK_PARAMS_BITS 6
#define SMCMD_ID(cmdnumber, flags) (((cmdnumber)<<3)|flags)
//format 5, u8 bytesfollowing,u8 toaddr,cmddata,u16 crc
//return, 6, u8 bytesfollowing,u8 fromaddr, returndata,u16 crc
#define SMCMD_INSTANT_CMD SMCMD_ID(4,SMCMD_MASK_N_PARAMS)
#define SMCMD_INSTANT_CMD_RET SMCMD_ID(4,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
//format ID, u8 bytesfollowing,u8 toaddr,cmddata,u16 crc
//return, ID, u8 bytesfollowing,u8 fromaddr,returndata,u16 crc
#define SMCMD_BUFFERED_CMD SMCMD_ID(6,SMCMD_MASK_N_PARAMS)
#define SMCMD_BUFFERED_CMD_RET SMCMD_ID(6,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
//read return data from buffered cmds. repeat this command before SMCMD_BUFFERED_CMD until N!=120 from download
#define SMCMD_BUFFERED_RETURN_DATA SMCMD_ID(7,SMCMD_MASK_0_PARAMS)
#define SMCMD_BUFFERED_RETURN_DATA_RET SMCMD_ID(7,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
//cmd 20,u8 toaddr, u8 crc
//ret 21, u8=2, u8 fromaddr, u16 clock,u16 crc, muut clientit lukee t?n
//oldret 21, u8 fromaddr, u16 clock,u16 crc, muut clientit lukee t?n
#define SMCMD_GET_CLOCK SMCMD_ID(10,SMCMD_MASK_0_PARAMS)
#define SMCMD_GET_CLOCK_RET SMCMD_ID(10,SMCMD_MASK_2_PARAMS|SMCMD_MASK_RETURN)
// SMCMD_FAST_UPDATE_CYCLE is a high priority command for fast realtime cyclic operation. content and lenght of
// payload data may be application specific and may be controlled by other variables
// cmd u8 ID, u8 toaddr, u32 data u16 crc
// ret u8 ID, u8 fromaddr, u32 data, u16 crc
#define SMCMD_FAST_UPDATE_CYCLE SMCMD_ID(2,SMCMD_MASK_0_PARAMS)
#define SMCMD_FAST_UPDATE_CYCLE_RET SMCMD_ID(2,SMCMD_MASK_0_PARAMS|SMCMD_MASK_RETURN)
#ifdef PROCESS_IMAGE_SUPPORT
//PROCESS_IMAGE communication not supported by SM V2.0.0. placeholders here
//upload process data to bus devices. each device knows it's data offset in payload
//(set by SMREGISTER_PDATA_IN_OFFSET, which is 0xffff if not accepting anything)
#define SMCMD_PROCESS_IMAGE SMCMD_ID(5,SMCMD_MASK_N_PARAMS)
//retrun of feedback process data is the tricky part
//the device that has process data address of 0 answers first including SMCMD header (3bytes) and then its own return process data
//SMREGISTER_PDATA_OUT_OFFSET is location of device's return processdata
#define SMCMD_PROCESS_IMAGE_RET SMCMD_ID(5,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN)
#endif
#endif

97
software/ros_packages/rover_arm/src/simplemotion/sm_consts.c Normal file
View File

@@ -0,0 +1,97 @@
#include "simplemotion_private.h"
/* Table of CRC16 values for high-order byte */
const smuint8 table_crc16_hi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
};
/* Table of CRC16 values for low-order byte */
const smuint8 table_crc16_lo[] = {
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
0x43, 0x83, 0x41, 0x81, 0x80, 0x40
};
//http://www.maxim-ic.com/appnotes.cfm/an_pk/3749
const smuint8 table_crc8[] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15,
0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
};