void Cedrus::XIDDevice::ConnectToMpod(unsigned char mpod, unsigned char action)
{
if (!m_config->IsXID2())
return;
int rate = 4;
if (action > 0)
{
m_baudRatePriorToMpod = GetBaudRate();
// m-pods operate at 19200 only
SetBaudRate(1);
}
else
{
switch (m_baudRatePriorToMpod)
{
case 9600:
rate = 0;
break;
case 19200:
rate = 1;
break;
case 38400:
rate = 2;
break;
case 57600:
rate = 3;
break;
case 115200:
rate = 4;
break;
default:
break;
}
}
static unsigned char ctm_cmd[4] = {'a','q'};
ctm_cmd[2] = '0' + mpod;
ctm_cmd[3] = '0' + action;
DWORD bytes_written = 0;
m_xidCon->Write(ctm_cmd, 4, &bytes_written);
SLEEP_FUNC(150 * SLEEP_INC);
m_podHostConfig = (action == 0 ? nullptr : m_config);
MatchConfigToModel(-1);
m_curMinorFwVer = GetMinorFirmwareVersion();
if (action == 0)
SetBaudRate(rate);
}
void SPI::Initialize(bool isMaster, bool isFullDuplex, SPI_BAUD_RATE baudRate, bool isClockPhase2,
bool isClockPolarity1, bool isLSBFirst, bool isSetSSM, bool is16Bit)
{
if (is16Bit) SetDFF_16bit();
Enable(true);
isClockPhase2 ? SetClockPhaseSecond() : SetClockPhaseFirst();
isClockPolarity1 ? SetClockPolarity1() : SetClockPolarity0();
isLSBFirst ? SetFrameFormatLSBFirst() : SetFrameFormatMSBFirst();
SetSSM (isSetSSM);
if (isMaster)
{
SetBaudRate (baudRate);
if (isSetSSM)
{
SetSSI(true);
}
}
else
{
SetSSI(false);
}
SetFullDuplex(isFullDuplex);
SetMaster(isMaster);
}
/*
* NextHeadFinderSetting
*/
void NextHeadFinderSetting()
{
struct tagSetting
{
BYTE HeadFinderPort;
DWORD BaudRate;
} Setting[4] = { 1, 19200, 2, 19200, 1, 115200, 2, 115200 };
//
int CurrentSetting;
//
for (int i = 0; i < 4; i++)
{
if (Setting[i].HeadFinderPort == HeadFinderPort && Setting[i].BaudRate == BaudRate)
{
CurrentSetting = i;
break;
}
}
//
CurrentSetting++;
CurrentSetting = CurrentSetting % 4;
//
SetHeadFinderPort(Setting[CurrentSetting].HeadFinderPort);
SetBaudRate(Setting[CurrentSetting].BaudRate);
}
bool
CAI302Device::EnableBulkMode(OperationEnvironment &env)
{
/* this operation should not be cancellable, because we don't know
what to do with a partial result */
NoCancelOperationEnvironment env2(env);
return !config.UsesSpeed() || config.bulk_baud_rate == 0 ||
config.bulk_baud_rate == config.baud_rate ||
SetBaudRate(config.bulk_baud_rate, env2);
}
bool
CAI302Device::DisableBulkMode(OperationEnvironment &env)
{
/* even if the operation was cancelled, switch back to the normal
baud rate safely */
NoCancelOperationEnvironment env2(env);
return !config.UsesSpeed() || config.bulk_baud_rate == 0 ||
config.bulk_baud_rate == config.baud_rate ||
SetBaudRate(config.baud_rate, env2);
}
void CSerialPDD::SetDefaultConfiguration()
{
// Default Value. Can be altered.
m_CommPorp.wPacketLength = 0xffff;
m_CommPorp.wPacketVersion = 0xffff;
m_CommPorp.dwServiceMask = SP_SERIALCOMM;
m_CommPorp.dwReserved1 = 0;
m_CommPorp.dwMaxTxQueue = 16;
m_CommPorp.dwMaxRxQueue = 16;
m_CommPorp.dwMaxBaud = BAUD_115200;
m_CommPorp.dwProvSubType = PST_RS232;
m_CommPorp.dwProvCapabilities =
PCF_DTRDSR | PCF_RLSD | PCF_RTSCTS |
PCF_SETXCHAR |
PCF_INTTIMEOUTS |
PCF_PARITY_CHECK |
PCF_SPECIALCHARS |
PCF_TOTALTIMEOUTS |
PCF_XONXOFF;
m_CommPorp.dwSettableBaud =
BAUD_075 | BAUD_110 | BAUD_150 | BAUD_300 | BAUD_600 |
BAUD_1200 | BAUD_1800 | BAUD_2400 | BAUD_4800 |
BAUD_7200 | BAUD_9600 | BAUD_14400 |
BAUD_19200 | BAUD_38400 | BAUD_56K | BAUD_128K |
BAUD_115200 | BAUD_57600 | BAUD_USER;
m_CommPorp.dwSettableParams =
SP_BAUD | SP_DATABITS | SP_HANDSHAKING | SP_PARITY |
SP_PARITY_CHECK | SP_RLSD | SP_STOPBITS;
m_CommPorp.wSettableData =
DATABITS_5 | DATABITS_6 | DATABITS_7 | DATABITS_8;
m_CommPorp.wSettableStopParity =
STOPBITS_10 | STOPBITS_20 |
PARITY_NONE | PARITY_ODD | PARITY_EVEN | PARITY_SPACE |
PARITY_MARK;
// Setup m_DCB.
// Set Detault Parameter.
SetOutputMode(FALSE, TRUE ); // No IR.
// For DCB. The PDD only need to take care BaudRate, ByteSize Parity & StopBit
m_DCB.DCBlength = sizeof(DCB);
SetBaudRate(m_DCB.BaudRate = 9600,FALSE);
SetByteSize(m_DCB.ByteSize = 8);
SetParity(m_DCB.Parity = NOPARITY);
SetStopBits(m_DCB.StopBits = ONESTOPBIT);
}
/*
* initHeadFinder
*/
int initHeadFinder(void *hWnd)
{
SetHeadFinderPort(HeadFinderPort);
SetBaudRate(BaudRate);
//
hHEFDCOMWnd = (HWND)hWnd;
useHeadFinder = FALSE;
isHeadFinder = FALSE;
fifoPos = 0;
//
CreateHEFDCOMInfo((HWND)hWnd);
if (!OpenConnection((HWND)hWnd))
return (0);
//
isHeadFinder = TRUE;
WriteCommBlock((HWND)hWnd, "0", 1);
//WriteCommBlock((HWND)hWnd,"Q",1);
useHF(TRUE);
return (1);
}
BOOL CSerialPDD::SetDCB(LPDCB lpDCB)
{
BOOL bReturn = TRUE;
if (IsOpen() && lpDCB && lpDCB->DCBlength == sizeof(DCB)) {
m_HardwareLock.Lock();
DCB LocalDCB = *lpDCB;
if (lpDCB->BaudRate != m_DCB.BaudRate) {
if (lpDCB->BaudRate == 0 || !SetBaudRate(lpDCB->BaudRate, m_fIREnable)) {
LocalDCB.BaudRate = m_DCB.BaudRate ;
bReturn = FALSE;
}
}
if (lpDCB->ByteSize != m_DCB.ByteSize ) {
if (!SetByteSize(lpDCB->ByteSize)) {
LocalDCB.ByteSize = m_DCB.ByteSize;
bReturn = FALSE;
}
}
if (lpDCB->Parity != m_DCB.Parity) {
if (!SetParity(lpDCB->Parity)) {
LocalDCB.Parity = m_DCB.Parity ;
bReturn=FALSE;
}
}
if (lpDCB->StopBits != m_DCB.StopBits) {
if (!SetStopBits(lpDCB->StopBits)) {
LocalDCB.StopBits = m_DCB.StopBits;
bReturn = FALSE;
}
}
m_DCB = LocalDCB;
m_HardwareLock.Unlock();
}
else
bReturn = FALSE;
return bReturn;
};
int main(int argc, char **argv)
{
int fd, res, baud_rate, rv, binary_mode;
FILE *fp;
struct termios newtio;
unsigned char in_c;
char sbuf[1];
if(argc < 3) {
printf("Serial port transmit program\n");
printf("Usage 1: %s device name baud rate file name\n", argv[0]);
printf("Usage 2: %s device baud file \"binary\"\n", argv[0]);
return 1;
}
fd = open(argv[1], O_WRONLY | O_NOCTTY | O_NDELAY);
if(fd < 0){
printf("Cannot write to the specified device!\n");
printf("Exiting...\n");
return 1;
}
if(argc > 2 && atoi(argv[2])) baud_rate = SetBaudRate(atoi(argv[2]));
else baud_rate = SetBaudRate(9600);
if(argc > 4) {
rv = strcmp(argv[4], "binary");
if(rv == 0) binary_mode = 1; else binary_mode = 0;
}
bzero(&newtio, sizeof(newtio));
newtio.c_cflag = baud_rate | CS8 | CREAD;
newtio.c_iflag = IGNBRK;
if(binary_mode == 1) {
newtio.c_oflag = 0;
}
else {
/* Postprocess the output: */
/* Map chars to upper case and map NL (linefeed) to CR-NL on output */
newtio.c_oflag = OPOST | OLCUC | ONLCR;
}
newtio.c_lflag = 0;
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 1;
#if defined (FASYNC) /* ASYNC I/O is a BDS feature */
/* Make the file descriptor asynchronous */
fcntl(fd, F_SETFL, FASYNC);
#endif
tcflush(fd, TCIFLUSH);
tcsetattr(fd, TCSANOW, &newtio);
fp = fopen(argv[3], "r+b");
if(fp == 0) {
printf("Could not open the %s file\n", argv[3]);
printf("Exiting...\n");
return 1;
}
printf("Sending file...\n");
while(feof(fp) == 0) {
in_c = fgetc(fp);
sbuf[0] = in_c;
sbuf[1] = 0;
res = write(fd, sbuf, 1);
if(res < 0) {
printf("Error writing to %s\n", argv[1]);
exit(1);
}
}
printf("%s was sent with no errors reported\n", argv[3]);
printf("Exiting...\n");
fclose(fp);
close(fd);
return 0;
}
void SPI::SetBaudRateDiv128()
{
SetBaudRate (SPI_BAUD_RATE_128);
}
void SPI::SetBaudRateDiv64()
{
SetBaudRate (SPI_BAUD_RATE_64);
}
void SPI::SetBaudRateDiv32()
{
SetBaudRate (SPI_BAUD_RATE_32);
}
void SPI::SetBaudRateDiv16()
{
SetBaudRate (SPI_BAUD_RATE_16);
}
void SPI::SetBaudRateDiv8()
{
SetBaudRate (SPI_BAUD_RATE_8);
}
MRESULT EXPENTRY fnwpHdwBaudRateDlg(HWND hwnd,USHORT msg,MPARAM mp1,MPARAM mp2)
{
LONG lBaud;
char szBaud[12];
int iIndex;
int iCurrentBaudIndex;
BYTE byTemp;
static bAllowClick;
static COMCTL *pstComCtl;
ULONG *pulBaudTable;
LONG lMaxBaud;
BAUDST stBaudRates;
switch (msg)
{
case WM_INITDLG:
// CenterDlgBox(hwnd);
// WinSetFocus(HWND_DESKTOP,hwnd);
bAllowClick = FALSE;
pstComCtl = PVOIDFROMMP(mp2);
if (GetBaudRates(pstComCtl->pstIOctl,pstComCtl->hCom,&stBaudRates))
{
WinDismissDlg(hwnd,FALSE);
return(FALSE);
}
if (pstComCtl->pszPortName != NULL)
WinSetWindowText(hwnd,pstComCtl->pszPortName);
lBaud = stBaudRates.stCurrentBaud.lBaudRate;
lMaxBaud = stBaudRates.stHighestBaud.lBaudRate;
if (lMaxBaud > 469800)
pulBaudTable = (ULONG *)aul12xBaudTable;
else
if (lMaxBaud > 115200)
pulBaudTable = (ULONG *)aul4xBaudTable;
else
pulBaudTable = (ULONG *)aulStdBaudTable;
if (lBaud == 0)
lBaud = DEF_BAUD;
WinSendDlgItemMsg(hwnd,HWB_BAUD,EM_SETTEXTLIMIT,MPFROMSHORT(6),(MPARAM)NULL);
iCurrentBaudIndex = -1;
iIndex = 0;
while (pulBaudTable[iIndex] != 0)
{
ltoa(pulBaudTable[iIndex],szBaud,10);
WinSendDlgItemMsg(hwnd,HWB_BAUD,LM_INSERTITEM,MPFROM2SHORT(LIT_END,0),MPFROMP(szBaud));
if (pulBaudTable[iIndex] == lBaud)
iCurrentBaudIndex = iIndex;
iIndex++;
}
if (iCurrentBaudIndex >= 0)
WinSendDlgItemMsg(hwnd,HWB_BAUD,LM_SELECTITEM,MPFROMSHORT((SHORT)iCurrentBaudIndex),(MPARAM)TRUE);
ltoa(lBaud,szBaud,10);
WinSetDlgItemText(hwnd,HWB_BAUD,szBaud);
WinPostMsg(hwnd,UM_INITLS,(MPARAM)0L,(MPARAM)0L);
break;
case UM_INITLS:
bAllowClick = TRUE;
break;
case WM_CONTROL:
if (SHORT2FROMMP(mp1) == BN_CLICKED)
if (bAllowClick)
if (!TCHdwProtocolDlg(hwnd,SHORT1FROMMP(mp1)))
return(FALSE);
break;
case WM_COMMAND:
switch( SHORT1FROMMP( mp1 ) )
{
case DID_OK:
WinQueryDlgItemText(hwnd,HWB_BAUD,sizeof(szBaud),szBaud);
SetBaudRate(pstComCtl->pstIOctl,pstComCtl->hCom,atol(szBaud));
WinDismissDlg(hwnd,TRUE);
break;
case DID_CANCEL:
WinDismissDlg(hwnd,FALSE);
break;
case DID_HELP:
DisplayHelpPanel(pstComCtl);
return((MRESULT)TRUE);
default:
return WinDefDlgProc(hwnd,msg,mp1,mp2);
}
break;
default:
return(WinDefDlgProc(hwnd,msg,mp1,mp2));
}
return(FALSE);
}
void SPI::SetBaudRateDiv256()
{
SetBaudRate (SPI_BAUD_RATE_256);
}
int main()
{
// Initialize PortHandler Structs
// Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
int port_num = PortHandler(DEVICENAME);
// Initialize PacketHandler Structs
PacketHandler();
int dxl_comm_result = COMM_TX_FAIL; // Communication result
UINT8_T dxl_error = 0; // Dynamixel error
UINT8_T dxl_baudnum_read; // Read baudnum
// Open port
if (OpenPort(port_num))
{
printf("Succeeded to open the port!\n");
}
else
{
printf("Failed to open the port!\n");
printf("Press any key to terminate...\n");
_getch();
return 0;
}
// Set port baudrate
if (SetBaudRate(port_num, BAUDRATE))
{
printf("Succeeded to change the baudrate!\n");
}
else
{
printf("Failed to change the baudrate!\n");
printf("Press any key to terminate...\n");
_getch();
return 0;
}
// Read present baudrate of the controller
printf("Now the controller baudrate is : %d\n", GetBaudRate(port_num));
// Try factoryreset
printf("[ID:%03d] Try factoryreset : ", DXL_ID);
FactoryReset(port_num, PROTOCOL_VERSION, DXL_ID, OPERATION_MODE);
if ((dxl_comm_result = GetLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
{
printf("Aborted\n");
PrintTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
return 0;
}
else if ((dxl_error = GetLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
PrintRxPacketError(PROTOCOL_VERSION, dxl_error);
// Wait for reset
printf("Wait for reset...\n");
msecSleep(2000);
printf("[ID:%03d] FactoryReset Success!\n", DXL_ID);
// Set controller baudrate to Dynamixel default baudrate
if (SetBaudRate(port_num, FACTORYRST_DEFAULTBAUDRATE))
{
printf("Succeed to change the controller baudrate to : %d\n", FACTORYRST_DEFAULTBAUDRATE);
}
else
{
printf("Failed to change the controller baudrate\n");
printf("Press any key to terminate...\n");
_getch();
return 0;
}
// Read Dynamixel baudnum
dxl_baudnum_read = Read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE);
if ((dxl_comm_result = GetLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
PrintTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
else if ((dxl_error = GetLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
PrintRxPacketError(PROTOCOL_VERSION, dxl_error);
else
printf("[ID:%03d] DXL baudnum is now : %d\n", DXL_ID, dxl_baudnum_read);
// Write new baudnum
Write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE, NEW_BAUDNUM);
if ((dxl_comm_result = GetLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
PrintTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
else if ((dxl_error = GetLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
PrintRxPacketError(PROTOCOL_VERSION, dxl_error);
else
printf("[ID:%03d] Set Dynamixel baudnum to : %d\n", DXL_ID, NEW_BAUDNUM);
// Set port baudrate to BAUDRATE
if (SetBaudRate(port_num, BAUDRATE))
{
printf("Succeed to change the controller baudrate to : %d\n", BAUDRATE);
}
else
{
printf("Failed to change the controller baudrate\n");
printf("Press any key to terminate...\n");
_getch();
return 0;
}
msecSleep(200);
// Read Dynamixel baudnum
dxl_baudnum_read = Read1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_BAUDRATE);
if ((dxl_comm_result = GetLastTxRxResult(port_num, PROTOCOL_VERSION)) != COMM_SUCCESS)
PrintTxRxResult(PROTOCOL_VERSION, dxl_comm_result);
else if ((dxl_error = GetLastRxPacketError(port_num, PROTOCOL_VERSION)) != 0)
PrintRxPacketError(PROTOCOL_VERSION, dxl_error);
else
printf("[ID:%03d] Dynamixel Baudnum is now : %d\n", DXL_ID, dxl_baudnum_read);
// Close port
ClosePort(port_num);
return 0;
}
int main(int argc, char **argv)
{
int fd, baud_rate, res, bytesmoved, echo_on, rv;
struct termios newtio;
char buf[255];
char *dumpfile = "datafile";
FILE *fp;
if(argc < 2) {
printf("Serial port receive program\n");
printf("Usage 1: %s device name [baud rate]\n", argv[0]);
printf("Usage 2: %s device name baud echo\n", argv[0]);
return 1;
}
/* Open the serial port without waiting for the requested modem connection */
fd = open(argv[1], O_RDWR | O_NOCTTY | O_NONBLOCK);
if (fd <0) {perror(argv[1]); exit(-1); }
if(argc > 2 && atoi(argv[2])) baud_rate = SetBaudRate(atoi(argv[2]));
else baud_rate = SetBaudRate(9600);
if(argc > 3) {
rv = strcmp(argv[3], "echo");
if(rv == 0) echo_on = 1; else echo_on = 0;
}
bzero(&newtio, sizeof(newtio));
/* Ignore the modem control lines with the CLOCAL flag */
newtio.c_cflag = baud_rate | CS8 | CREAD | CLOCAL;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
/* Set input mode (non-canonical, no echo,...) */
newtio.c_lflag = 0;
newtio.c_cc[VTIME] = 0;
newtio.c_cc[VMIN] = 1;
tcflush(fd, TCIFLUSH);
tcsetattr(fd, TCSANOW, &newtio);
printf("%s open for read/write access\n", argv[1]);
printf("Wating to receive...\n");
printf("\n");
fp = fopen(dumpfile, "w+b");
if(fp == 0) {
printf("Could not open %s for writting", dumpfile);
return 1;
}
while(STOP==FALSE) {
res = read(fd, buf, 255); /* Returns after 1 char has been input */
buf[res] = 0; /* Ensure null termination */
if(res > 0) {
bytesmoved = fwrite(buf, sizeof(char), res, fp);
if(bytesmoved < 0) {
printf("Errorr writing to %s", dumpfile);
break;
}
fflush(fp);
if(echo_on == 1) printf("%s", buf);
fflush(stdout);
}
}
fclose(fp);
close(fd);
return 0;
}
