bsproxy/rcb3.c File Reference

Detailed Description

Built-in client device support for the RCB-3 controller.

Version:
$LastChangedDate$ $Rev$
Author:
Robin Knight (robin.knight@roadnarrows.com)
Copyright:
(C) 2007. RoadNarrows LLC. (http://www.roadnarrows.com)
All Rights Reserved

Definition in file rcb3.c.

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "rnr/rnrconfig.h"
#include "rnr/log.h"
#include "rcb3/rcb3prot.h"
#include "rcb3/rcb3lib.h"
#include "botsense/bsproxy_if.h"
#include "bsproxy.h"

Go to the source code of this file.

Defines

#define RCB3LIB_FAIL(pClient, bufCmd, fmt,)
 Print libRCB3 error and return from calling function.
#define RCB3LIB_CHK(rc, pClient, bufCmd)
 Check for libRCB3 errors.

Functions

static int RCB3CmdGetVersion (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 RCB-3 command to get RCB-3 firmware version.
static int RCB3CmdGetCurPos (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 RCB-3 command to all of the servos' current angular positions.
static int RCB3CmdMoveServo (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 RCB-3 command to move a servo to an ending position at a given speed.
static int RCB3CmdPlay (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 RCB-3 command to playback a motion or scenario.
static int RCB3CmdStop (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 RCB-3 command to stop.
int RCB3Dispatch (BsProxyServer_T *pServer, BsProxyClient_T *pClient, BsProxyClientDev_T *pDev, byte_t bufCmd[], size_t lenCmd)
 RCB-3 Robot Controller command dispatcher.
int ProxDevRCB3Read (BsProxyDev_T *pProxDev, byte_t bufRead[], size_t countRead)
 Read bytes from an RCB-3 controller device.
int ProxDevRCB3Write (BsProxyDev_T *pProxDev, byte_t bufWrite[], size_t countWrite)
 Write bytes to an RCB-3 controller device.
int ProxDevRCB3Trans (BsProxyDev_T *pProxDev, byte_t bufWrite[], size_t countWrite, byte_t bufRead[], size_t countRead)
 Do a write/read transaction with an RCB-3 controller device.

Define Documentation

#define RCB3LIB_CHK (  rc,
pClient,
bufCmd   ) 
 

Value:

{if(rc != RCB3_SUCCESS) return ClientSendFailRsp(pClient, bufCmd, \
      "%s: %s ", LOGFUNCNAME, RCB3ErrorGetMsg());}
Check for libRCB3 errors.

Parameters:
rc libRCB3 return code
pClient BotSense client.
bufCmd Received command buffer.

Definition at line 81 of file rcb3.c.

Referenced by RCB3CmdGetCurPos(), RCB3CmdGetVersion(), RCB3CmdMoveServo(), RCB3CmdPlay(), and RCB3CmdStop().

#define RCB3LIB_FAIL (  pClient,
bufCmd,
fmt   ) 
 

Value:

return ClientSendFailRsp(pClient, bufCmd, "%s " fmt, \
        RCB3ErrorGetMsg(), ##__VA_ARGS__);
Print libRCB3 error and return from calling function.

Parameters:
pClient BotSense client.
bufCmd Received command buffer.
fmt Error format string
... Error format arguments.

Definition at line 71 of file rcb3.c.

Function Documentation

int ProxDevRCB3Read (  BsProxyDev_T *  pProxDev,
byte_t  bufRead[],
size_t  countRead
) 
 

Read bytes from an RCB-3 controller device.

Parameters:
pProxDev Proxied device.
[out] bufRead Read buffer.
countRead Number of bytes to read from device to read buffer.
Returns:
Returns number of bytes read on success, <=0 on error.

Definition at line 377 of file rcb3.c.

References BsProxyDevRCB3_T::m_nHandle, BsProxyDev_T::m_sDevName, BsProxyDev_T::m_unDevSpec, and BsProxyDev_T::rcb3.

Referenced by CmdDevRead().

00378 {
00379   int   n;
00380 
00381   n = RCB3Receive(pProxDev->m_unDevSpec.rcb3.m_nHandle, bufRead, countRead);
00382 
00383   if( n <= 0 )
00384   {
00385     LOGSYSERROR("%s: RCB3Receive(%d...): %s",
00386                 pProxDev->m_sDevName, 
00387                 pProxDev->m_unDevSpec.rcb3.m_nHandle,
00388                 RCB3ErrorGetMsg());
00389   }
00390 
00391   return n;
00392 }

int ProxDevRCB3Trans (  BsProxyDev_T *  pProxDev,
byte_t  bufWrite[],
size_t  countWrite,
byte_t  bufRead[],
size_t  countRead
) 
 

Do a write/read transaction with an RCB-3 controller device.

Parameters:
pProxDev Proxied device.
bufWrite Write buffer.
countWrite Number of bytes to write to the device.
[out] bufRead Read buffer.
countRead Number of bytes to read from device to read buffer.
Returns:
Returns number of bytes read on success, <=0 on error.

Definition at line 434 of file rcb3.c.

References BsProxyDevRCB3_T::m_nHandle, BsProxyDev_T::m_sDevName, BsProxyDev_T::m_unDevSpec, and BsProxyDev_T::rcb3.

Referenced by CmdDevTrans().

00437 {
00438   int   rc;
00439 
00440   if( countRead == 0 )
00441   {
00442     rc = RCB3SendCmd(pProxDev->m_unDevSpec.rcb3.m_nHandle,
00443                             bufWrite, countWrite);
00444   }
00445   else if( countRead == 1 )
00446   {
00447     rc = RCB3AckTransaction(pProxDev->m_unDevSpec.rcb3.m_nHandle,
00448                             bufWrite, countWrite);
00449   }
00450   else
00451   {
00452     rc = RCB3DataTransaction(pProxDev->m_unDevSpec.rcb3.m_nHandle,
00453                             bufWrite, countWrite,
00454                             bufRead, countRead);
00455   }
00456 
00457   if( rc <= 0 )
00458   {
00459     LOGSYSERROR("%s: RCB3xTransaction(%d...): %s",
00460                 pProxDev->m_sDevName, 
00461                 pProxDev->m_unDevSpec.rcb3.m_nHandle,
00462                 RCB3ErrorGetMsg());
00463 
00464     return -1;
00465   }
00466 
00467   if( countRead == 1 )
00468   {
00469     bufRead[0] = RCB3RspIdAck;
00470   }
00471 
00472   return (int)countRead;
00473 }

int ProxDevRCB3Write (  BsProxyDev_T *  pProxDev,
byte_t  bufWrite[],
size_t  countWrite
) 
 

Write bytes to an RCB-3 controller device.

Parameters:
pProxDev Proxied device.
bufWrite Write buffer.
countWrite Number of bytes to write to the device.
Returns:
Returns number of written on success, <=0 on error.

Definition at line 403 of file rcb3.c.

References BsProxyDevRCB3_T::m_nHandle, BsProxyDev_T::m_sDevName, BsProxyDev_T::m_unDevSpec, and BsProxyDev_T::rcb3.

Referenced by CmdDevWrite().

00405 {
00406   int   rc;
00407 
00408   rc = RCB3SendCmd(pProxDev->m_unDevSpec.rcb3.m_nHandle, bufWrite, countWrite);
00409 
00410   if( rc <= 0 )
00411   {
00412     LOGSYSERROR("%s: RCB3SendCmd(%d...): %s",
00413                 pProxDev->m_sDevName, 
00414                 pProxDev->m_unDevSpec.rcb3.m_nHandle,
00415                 RCB3ErrorGetMsg());
00416 
00417     return -1;
00418   }
00419 
00420   return (int)countWrite;
00421 }

static int RCB3CmdGetCurPos (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
)  [static]
 

RCB-3 command to all of the servos' current angular positions.

Command Format:
cmdhdr handle cmd_id
Response Format:
rsphdr pos0_hi pos0_lo ... pos23_hi pos23_lo
rsphdr errmsg
Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 151 of file rcb3.c.

References BSPACK16(), BSPROXY_MSG_MAX_LEN, BSPROXY_RSP_HDR_LEN, BsProxyDevRCB3_T::m_nHandle, BsProxyClientDev_T::m_pProxDev, BsProxyDev_T::m_unDevSpec, BsProxyDev_T::rcb3, and RCB3LIB_CHK.

Referenced by RCB3Dispatch().

00155 {
00156   byte_t          bufRsp[BSPROXY_MSG_MAX_LEN];
00157   size_t          lenRsp;
00158   handle_t        nHandle;
00159   int             rc;
00160   ServoVecPos_T   vecPos;
00161   int             i;
00162 
00163   nHandle = pDev->m_pProxDev->m_unDevSpec.rcb3.m_nHandle;
00164 
00165   rc = RCB3TransGetCurPosition(nHandle, vecPos);
00166 
00167   RCB3LIB_CHK(rc, pClient, bufCmd);
00168 
00169   lenRsp            = BSPROXY_RSP_HDR_LEN;
00170 
00171   for(i=0; i<RCB3_PORT_NUMOF; ++i)
00172   {
00173     lenRsp += BSPACK16(vecPos[i], &bufRsp[lenRsp]);
00174   }
00175 
00176   return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
00177 }

static int RCB3CmdGetVersion (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
)  [static]
 

RCB-3 command to get RCB-3 firmware version.

Command Format:
cmdhdr handle cmd_id
Response Format:
rsphdr version
rsphdr errmsg
Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 104 of file rcb3.c.

References BSPROXY_MSG_MAX_LEN, BSPROXY_RSP_HDR_LEN, BsProxyDevRCB3_T::m_nHandle, BsProxyClientDev_T::m_pProxDev, BsProxyDev_T::m_unDevSpec, BsProxyDev_T::rcb3, and RCB3LIB_CHK.

Referenced by RCB3Dispatch().

00108 {
00109   byte_t        bufRsp[BSPROXY_MSG_MAX_LEN];
00110   size_t        lenRsp;
00111   handle_t      nHandle;
00112   int           rc;
00113   char          version[RCB3_PARAM_VERSION_LEN];
00114   char         *s;
00115 
00116   nHandle = pDev->m_pProxDev->m_unDevSpec.rcb3.m_nHandle;
00117 
00118   rc = RCB3TransGetVersion(nHandle, version);
00119 
00120   RCB3LIB_CHK(rc, pClient, bufCmd);
00121 
00122   lenRsp            = BSPROXY_RSP_HDR_LEN;
00123 
00124   for(s=version; *s; ++s)
00125   {
00126     bufRsp[lenRsp++]  = *s;
00127   }
00128   bufRsp[lenRsp++]  = 0;
00129 
00130   return ClientSendPassRsp(pClient, bufCmd, bufRsp, lenRsp);
00131 }

static int RCB3CmdMoveServo (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
)  [static]
 

RCB-3 command to move a servo to an ending position at a given speed.

Command Format:
cmdhdr handle cmd_id servo pos_hi pos_lo speed
Response Format:
rsphdr
rsphdr errmsg
Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 197 of file rcb3.c.

References BSPROXY_CMD_HDR_LEN, BSPROXY_MSG_MAX_LEN, BSPROXY_RSP_HDR_LEN, BSUNPACK16(), ClientSendPassRsp(), BsProxyDevRCB3_T::m_nHandle, BsProxyClientDev_T::m_pProxDev, BsProxyDev_T::m_unDevSpec, BsProxyDev_T::rcb3, and RCB3LIB_CHK.

Referenced by RCB3Dispatch().

00201 {
00202   byte_t          bufRsp[BSPROXY_MSG_MAX_LEN];
00203   size_t          offset;
00204   handle_t        nHandle;
00205   int             rc;
00206   byte_t          servo;
00207   uint_t          pos;
00208   byte_t          speed;
00209 
00210   nHandle = pDev->m_pProxDev->m_unDevSpec.rcb3.m_nHandle;
00211 
00212   offset  = BSPROXY_CMD_HDR_LEN+2;
00213   servo   = bufCmd[offset++];
00214   offset += BSUNPACK16(&bufCmd[offset], &pos);
00215   speed   = bufCmd[offset++];
00216 
00217   rc = RCB3TransMoveServo(nHandle, RCB3OptionDefault, servo, speed, pos);
00218   
00219   RCB3LIB_CHK(rc, pClient, bufCmd);
00220 
00221   return ClientSendPassRsp(pClient, bufCmd, bufRsp,
00222       (size_t)BSPROXY_RSP_HDR_LEN);
00223 
00224 }

static int RCB3CmdPlay (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
)  [static]
 

RCB-3 command to playback a motion or scenario.

Command Format:
cmdhdr handle cmd_id play_index
Response Format:
rsphdr
rsphdr errmsg
Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 244 of file rcb3.c.

References BSPROXY_CMD_HDR_LEN, BSPROXY_MSG_MAX_LEN, BSPROXY_RSP_HDR_LEN, ClientSendPassRsp(), BsProxyDevRCB3_T::m_nHandle, BsProxyClientDev_T::m_pProxDev, BsProxyDev_T::m_unDevSpec, BsProxyDev_T::rcb3, and RCB3LIB_CHK.

Referenced by RCB3Dispatch().

00248 {
00249   byte_t          bufRsp[BSPROXY_MSG_MAX_LEN];
00250   handle_t        nHandle;
00251   int             rc;
00252   byte_t          playindex;
00253 
00254   nHandle = pDev->m_pProxDev->m_unDevSpec.rcb3.m_nHandle;
00255 
00256   playindex   = bufCmd[BSPROXY_CMD_HDR_LEN+2];
00257 
00258   rc = RCB3TransPlay(nHandle, RCB3OptionDefault, playindex);
00259   
00260   RCB3LIB_CHK(rc, pClient, bufCmd);
00261 
00262   return ClientSendPassRsp(pClient, bufCmd, bufRsp,
00263       (size_t)BSPROXY_RSP_HDR_LEN);
00264 
00265 }

static int RCB3CmdStop (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
)  [static]
 

RCB-3 command to stop.

Command Format:
cmdhdr handle cmd_id
Response Format:
rsphdr
rsphdr errmsg
Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 285 of file rcb3.c.

References BSPROXY_MSG_MAX_LEN, BSPROXY_RSP_HDR_LEN, ClientSendPassRsp(), BsProxyDevRCB3_T::m_nHandle, BsProxyClientDev_T::m_pProxDev, BsProxyDev_T::m_unDevSpec, BsProxyDev_T::rcb3, and RCB3LIB_CHK.

Referenced by RCB3Dispatch().

00289 {
00290   byte_t          bufRsp[BSPROXY_MSG_MAX_LEN];
00291   handle_t        nHandle;
00292   chanmap_t       chanMap;
00293   int             rc;
00294 
00295 
00296   nHandle = pDev->m_pProxDev->m_unDevSpec.rcb3.m_nHandle;
00297 
00298   chanMap = ChanMapSetAll();
00299 
00300   rc = RCB3TransStop(nHandle, chanMap);
00301   
00302   RCB3LIB_CHK(rc, pClient, bufCmd);
00303 
00304   return ClientSendPassRsp(pClient, bufCmd, bufRsp,
00305       (size_t)BSPROXY_RSP_HDR_LEN);
00306 
00307 }

int RCB3Dispatch (  BsProxyServer_T *  pServer,
BsProxyClient_T *  pClient,
BsProxyClientDev_T *  pDev,
byte_t  bufCmd[],
size_t  lenCmd
) 
 

RCB-3 Robot Controller command dispatcher.

Parameters:
pServer BotSense server.
pClient BotSense client.
pDev Client device.
bufCmd Received command buffer.
lenCmd Received command length (number of bytes).
Returns:
Returns OK on success, RC_ERROR on failure.

Definition at line 324 of file rcb3.c.

References BSPROXY_CMD_HDR_LEN, ClientSendFailRsp(), BsProxyClient_T::m_sClientName, RCB3CmdGetCurPos(), RCB3CmdGetVersion(), RCB3CmdMoveServo(), RCB3CmdPlay(), and RCB3CmdStop().

Referenced by CmdDevCmd().

00328 {
00329   byte_t  byDevCmdId;
00330 
00331   byDevCmdId = bufCmd[BSPROXY_CMD_HDR_LEN+1];
00332 
00333   //
00334   // Dispatch RCB-3 command.
00335   //
00336   switch( byDevCmdId )
00337   {
00338     // get device identifiers
00339     case RCB3CmdIdGetVersion:
00340       return RCB3CmdGetVersion(pServer, pClient, pDev, bufCmd, lenCmd);
00341 
00342     // get servos' current positions
00343     case RCB3CmdIdGetCurPos:
00344       return RCB3CmdGetCurPos(pServer, pClient, pDev, bufCmd, lenCmd);
00345 
00346     // move one servo
00347     case RCB3CmdIdMoveServo:
00348       return RCB3CmdMoveServo(pServer, pClient, pDev, bufCmd, lenCmd);
00349 
00350     // playback a motion or scenario
00351     case RCB3CmdIdPlay:
00352       return RCB3CmdPlay(pServer, pClient, pDev, bufCmd, lenCmd);
00353 
00354     // move one servo
00355     case RCB3CmdIdStop:
00356       return RCB3CmdStop(pServer, pClient, pDev, bufCmd, lenCmd);
00357 
00358     default:
00359       LOGERROR("%s: BrainPack Foot: 0x%02x: unsupported device command",
00360           pClient->m_sClientName, byDevCmdId);
00361       return ClientSendFailRsp(pClient, bufCmd, "bad device command");
00362   }
00363 
00364   LOGERROR("%s: BrainPack Foot: here?", pClient->m_sClientName);
00365   return ClientSendFailRsp(pClient, bufCmd, "bad message");
00366 }

Generated on Wed Nov 28 11:01:08 2007 for botsense by doxygen 1.4.6