packages feed

RMP (empty) → 0.0.2

raw patch · 11 files changed

+984/−0 lines, 11 filesdep +HOpenCVdep +allocated-processordep +basesetup-changed

Dependencies added: HOpenCV, allocated-processor, base, cv-combinators, vector-space

Files

+ RMP.cabal view
@@ -0,0 +1,65 @@+name: RMP+version: 0.0.2+license: BSD3+maintainer: Noam Lewis <jones.noamle@gmail.com>+bug-reports: mailto:jones.noamle@gmail.com+category: System, Robotics+synopsis: Binding to code that controls a Segway RMP+Tested-With:   GHC == 6.10.4+description:+        This library provides an interface to a USB-connected Segway RMP.+        .+        It is based on code and information from <http://www.ai.sri.com/~vincent/segway.php>, +        and was tested on a Segway RMP 200.+        .+        WARNING: The Segway RMP is a dangerous (and massive) device, use this library with care. +        The library comes without warranty, and you may find the Segway running loose, harming people.+   +build-type: Simple+cabal-version:  >= 1.2+extra-source-files:+   src/System/RMP/rmpusb.h+   src/System/RMP/canio.h+   src/System/RMP/canio_rmpusb.h++library+   exposed-modules:+      System.RMP+      System.RMP.USB+   c-sources:+      src/System/RMP/rmpusb.cpp+      src/System/RMP/canio_rmpusb.cpp+   hs-Source-Dirs: src+   extra-libraries: canlib,ftd2xx,stdc+++   build-depends: base >= 4, allocated-processor >= 0.0.2+   ghc-options: -Wall+++executable rmp-test+  main-is: Test.hs+  c-sources:+      src/System/RMP/rmpusb.cpp+      src/System/RMP/canio_rmpusb.cpp+  hs-Source-Dirs: src+  Build-Depends: base >=4 && <5, allocated-processor >= 0.0.2+  Ghc-Options: -Wall +  Ghc-Prof-Options:  -prof -auto-all +  extra-libraries: canlib,ftd2xx,stdc+++  other-modules: System.RMP.USB, System.RMP+++executable rmp-test-facedetect+  main-is: FaceFollowTest.hs+  c-sources:+      src/System/RMP/rmpusb.cpp+      src/System/RMP/canio_rmpusb.cpp+  hs-Source-Dirs: src+  Build-Depends: base >=4 && <5, allocated-processor >= 0.0.2, cv-combinators >= 0.1.2.3, HOpenCV, vector-space+  Ghc-Options: -Wall   -fno-warn-type-defaults+  Ghc-Prof-Options:  -prof -auto-all +  extra-libraries: canlib,ftd2xx,stdc+++  other-modules: System.RMP.USB, System.RMP++--source-repository head+--  type: git+--  location: git://github.com/sinelaw/RMP.git
+ Setup.hs view
@@ -0,0 +1,2 @@+import Distribution.Simple+main = defaultMain
+ src/FaceFollowTest.hs view
@@ -0,0 +1,110 @@+-- A face-following robot+-- Todo: improve following by taking note of how much we moved++module Main where++import qualified AI.CV.ImageProcessors as ImageProcessors+import AI.CV.ImageProcessors(ImageProcessor, ImageSource, runTillKeyPressed)+import AI.CV.OpenCV.CV as CV+import AI.CV.OpenCV.CxCore(CvRect(..), CvSize(..))+import AI.CV.OpenCV.Types(PImage)++import System.RMP(velocityRMP)++import Control.Processor(IOProcessor, trace, fir, revertAfterT, holdMaybe)+import Data.VectorSpace(zeroV, (^-^), AdditiveGroup)++import Control.Monad(join)+import Prelude hiding ((.),id)+import Control.Arrow+import Control.Category+import Data.Maybe(listToMaybe)++-- Debugging+import qualified Debug.Trace as DT+traceId :: (Show a) => a -> a+--traceId x = DT.trace (show x) x+traceId = id+--+++defaultHead :: a -> [a] -> a+defaultHead def [] = def+defaultHead _   xs = head xs++imageResizeTo :: Integral a => a -> a -> ImageProcessor+imageResizeTo resX resY = ImageProcessors.resize (fromIntegral resX) (fromIntegral resY) CV.CV_INTER_LINEAR++faceDetect :: IOProcessor PImage [CvRect]+faceDetect = ImageProcessors.haarDetect "/usr/share/opencv/haarcascades/haarcascade_frontalface_alt_tree.xml" 1.1 3 CV.cvHaarFlagNone (CvSize 20 20)++videoSource :: ImageSource+videoSource = ImageProcessors.camera 0++fromIntegral2 :: (Integral b, Num c) => (b, b) -> (c, c)+fromIntegral2 = join (***) fromIntegral+++absMax :: (Num a, Ord a) => a -> a -> a+absMax b a = max (min a (abs b)) (- (abs b))+-----------------------------------------------------------------------------++-- | Calculates a measure for the distance to a rect using its area, given a reference area size.+calcDist :: (Num x, Ord x) => x -> CvRect -> x+calcDist reference rect = if rectArea > 1 then reference - rectArea else 0+    where w = rectWidth  rect+          h = rectHeight rect+          rectArea = traceId . uncurry (*) $ fromIntegral2 (w,h)++-- | Calculates a distance to the given rect using some hand-tuned parameters +calcTrans :: (Integral b, Integral a) => a -> a -> CvRect -> b+calcTrans resX resY = (`div` tranScale) . traceId . calcDist referenceArea+    where referenceArea = fromIntegral ((resX*resY) `div` 30)+          tranScale = 20 -- 5 for 160x120?++-- | Calculates the difference (direction) from the detect rect to the center of the screen.+-- the 'fromIntegral2' stuff is due to CInt not being a VectorSpace+calcDir :: (Integral a, Integral b, AdditiveGroup b) => a -> a -> CvRect -> (b, b)+calcDir resX resY rect = if rect /= zeroV then rectCenter ^-^ screenCenter else (0,0)+    where screenCenter = fromIntegral2 (resX `div` 2, resY `div` 2)+          rectCenter = fromIntegral2 (rectX rect + (rectWidth rect `div` 2), +                                      rectY rect + (rectHeight rect `div` 2))+  +-- | Takes a direction vector (x,y) and returns required rotation speed to align with that direction.+-- for now we disregard the 'x' component, because we can't really point our robot "up" or "down" anyway.+dirToRotation :: (Num a, Ord a, Integral a, Num b, Ord b) => (a,b) -> a+dirToRotation (yRot, _) = - round (fromIntegral yRot * rotScale)+    where rotScale = 1.4 -- for 160x120, should be 4?+  +-- | calculates the (translation, rotation) pair used to control the robot, from a detected rect.+-- currently translation is constantly 0.+calcTransRot :: (Num c, Ord c, Integral c, Integral a, Integral b, AdditiveGroup b) => a -> a -> CvRect -> (c, b)+calcTransRot resX resY = (calcTrans resX resY >>> absMax maxTransVelocity)  +                         &&& (calcDir resX resY >>> dirToRotation >>> absMax maxRotVelocity)++-- todo: a better solution than choosing the default if no faces detected, would be to keep tracking the last+-- known face?+controller :: Integral a => a -> a -> IOProcessor CvRect (Int, Int)+controller resX resY =  arr (calcTransRot resX resY)++clock :: IO Double+clock = return 1 -- todo implement really in some module that wraps SDL, GLUT or whatever.++-- | The maximum rotational and translational velocity of the robot+maxRotVelocity, maxTransVelocity :: Integral a => a+maxTransVelocity = 40+maxRotVelocity = 150++main :: IO ()  +main = runTillKeyPressed (videoSource >>> imageResizeTo resX resY +             >>> (id &&& averageFace) >>> second (faceToVel >>> trace >>> velocityRMP) &&& showVideo)+      where showVideo = (second . arr $ return) >>> ImageProcessors.drawRects >>> ImageProcessors.window 0+            --showVideo = arr (const ()) -- does nothing+            averageFace = lastFace --fir [0.9,0.1] 1 clock lastFace+            lastFace = revertAfterT 5 zeroV . holdMaybe zeroV clock $ (faceDetect >>> arr listToMaybe)+            resX = 240+            resY = 180+            faceToVel = controller resX resY+++
+ src/System/RMP.hs view
@@ -0,0 +1,110 @@+-- TODO: Check for null pointers and fail appropriately. perhaps use the ForeignPtrWrap module.++module System.RMP where++import System.RMP.USB+import Foreign+import Foreign.C.Types+import Control.Processor(IOProcessor, wrapProcessor, processor)+import Control.Arrow((>>>),arr)++type Packet = Ptr RMPPacket++data RMPState = RMPState { rmpCtx :: Ptr RMPUSB, rmpReadPkt :: Packet }++-- | A processor wrapper: Takes the robot controller (something that reads telemetries and sends commands)+-- and returns an IOProcessor that runs the robot.+rmp :: IOProcessor Packet Packet -> IOProcessor () ()+rmp = wrapProcessor readPacket writePacket allocRMP readConv writeConv releaseRMP +    where +      readPacket :: () -> RMPState -> IO RMPState+      readPacket _ state = do+        res <- rmpUsbReadPacket (rmpCtx state) (rmpReadPkt state)+        -- todo: deal with res != 0+        return state+      +      readConv :: RMPState -> IO Packet+      readConv = return . rmpReadPkt+      +      writePacket :: Packet -> RMPState -> IO RMPState+      writePacket pkt state = do+        res <- rmpUsbWritePacket (rmpCtx state) pkt+        -- todo: deal with res != 0+        return state+      +      writeConv :: RMPState -> IO ()+      writeConv = const (return ())+      +      allocRMP :: () -> IO RMPState+      allocRMP _ = do+        ctx <- rmpUsbNew+        rPkt <- rmpPacketNew+        return (RMPState ctx rPkt)+      +      releaseRMP :: RMPState -> IO ()+      releaseRMP state = do+        rmpPacketDelete (rmpReadPkt state)+        rmpUsbDelete (rmpCtx state)++-----------------------------------------------------------------------------++velocityPacket :: Integral a => IOProcessor (a, a) Packet+velocityPacket = processor proc alloc conv release +    where+      proc :: Integral a => (a,a) -> Packet -> IO Packet+      proc (trans, rot) pkt = do+        rmpPacketSetCommandVelocity pkt (fromIntegral trans) (fromIntegral rot)+        return pkt+        +      alloc :: Integral a => (a,a) -> IO Packet+      alloc _ = do+        pkt <- rmpPacketNew+        return pkt++      conv :: Packet -> IO Packet+      conv = return+      +      release :: Packet -> IO ()+      release pkt = do+        rmpPacketDelete pkt+      +++-- data Packet = Empty +--             | PitchRoll { pitchAngle :: Double; pitchRate :: Double, rollAngle :: Double, rollRate :: Double }+--             | VelocityYaw { wheelsVelocity :: (Double, Double), yawRate :: Double }+--             | WheelDisplacement { wheelsDisplacement :: (Double, Double) }++      +-----------------------------------------------------------------------------++-- | A dummy version of rmp that:+--        +-- 1. Does not read from the robot (Ignores all robot telemetries)+--+-- 2. Can only send commands+--+-- it's really an IOSink.+--+simpleRMP :: IOProcessor Packet ()+simpleRMP = processor writePacket allocRMP writeConv releaseRMP +    where +      writePacket :: Packet -> (Ptr RMPUSB) -> IO (Ptr RMPUSB)+      writePacket pkt rmpUsb = do+        res <- rmpUsbWritePacket rmpUsb pkt+        -- todo: deal with res != 0+        return rmpUsb+      +      writeConv :: (Ptr RMPUSB) -> IO ()+      writeConv = const . return $ ()+      +      allocRMP :: Packet -> IO (Ptr RMPUSB)+      allocRMP _ = rmpUsbNew+      +      releaseRMP :: (Ptr RMPUSB) -> IO ()+      releaseRMP = rmpUsbDelete+++-- | An even simple RMP interface, that supports only sending velocity commands, and nothing more.+velocityRMP :: Integral a => IOProcessor (a,a) ()+velocityRMP = velocityPacket >>> simpleRMP
+ src/System/RMP/USB.hsc view
@@ -0,0 +1,42 @@+{-# LANGUAGE ForeignFunctionInterface, EmptyDataDecls #-}++module System.RMP.USB where++import Foreign.C.Types+import Foreign +import Foreign.ForeignPtrWrap++data RMPUSB+data RMPPacket++foreign import ccall unsafe "rmpusb.h rmpusb_new"+  c_RMPUSBNew :: IO (Ptr RMPUSB)++rmpUsbNew :: IO (Ptr RMPUSB)+rmpUsbNew = errorName "Failed to create RMP-USB context" . checkPtr $ c_RMPUSBNew+  +foreign import ccall unsafe "rmpusb.h rmpusb_delete"+  rmpUsbDelete :: Ptr RMPUSB -> IO ()++foreign import ccall unsafe "rmpusb.h rmpusb_write_packet"+  rmpUsbWritePacket :: Ptr RMPUSB -> Ptr RMPPacket -> IO CInt++foreign import ccall unsafe "rmpusb.h rmpusb_read_packet"+  rmpUsbReadPacket :: Ptr RMPUSB -> Ptr RMPPacket -> IO CInt+++                       +foreign import ccall unsafe "rmpusb.h rmppacket_new"+  c_RMPPacketNew :: IO (Ptr RMPPacket)++rmpPacketNew :: IO (Ptr RMPPacket)+rmpPacketNew = errorName "Failed to create packet" . checkPtr $ c_RMPPacketNew+                    +foreign import ccall unsafe "rmpusb.h rmppacket_delete"+  rmpPacketDelete :: Ptr RMPPacket -> IO ()++foreign import ccall unsafe "rmpusb.h rmppacket_set_command_velocity"+  rmpPacketSetCommandVelocity :: Ptr RMPPacket -> CInt -> CInt -> IO ()+++
+ src/System/RMP/canio.h view
@@ -0,0 +1,94 @@+/* Taken and modified from http://www.ai.sri.com/~vincent/segway.php */+#ifndef _CANIO_H_+#define _CANIO_H_++#include <stdint.h>++#include <sys/types.h>+#include <string.h>+#include <stdio.h>++// Copied this from <canlib.h>. I assume that it's portable across CAN+// implementations.+#ifndef canMSG_STD+#define canMSG_STD              0x0002+#endif+++class CanPacket+{+public:+    long id;+    uint8_t msg[8];+    uint32_t dlc;+    uint32_t flags;++    CanPacket()+    {+        Clear();+    }++    void Clear() {+        memset(msg,0,sizeof(msg));++        flags = canMSG_STD;+        dlc = 8;+    }+    +    uint16_t GetSlot(int s)  const +    {+        return (uint16_t) ((msg[s*2] << 8) | (msg[s*2+1]));+    }++    void PutSlot(const int slot, const uint16_t val) +    {+        msg[slot*2] = (val >> 8) & 0xFF;+        msg[slot*2+1] = val & 0xFF;+    }++    void PutByte(const int byte, const uint16_t val) +    {+        msg[byte] = val & 0xFF;+    }++    char* toString() +    {+        static char buf[256];+        sprintf(buf, "id:%04lX %02X %02X %02X %02X %02X %02X %02X %02X",+                id, msg[0], msg[1], msg[2], msg[3], msg[4], msg[5], +                msg[6], msg[7]);++        return buf;+    }+};+++/* this class encapsulates the low level CAN stuff.... so it deals+   with reading and writing packets on the CAN channels.+   We make the assumption that we only have to read off of one+   channel though (looking at rmi_demo, it appears that this is+   OK.)+   A higher level entity will make sense of the packets, and call+   the read/write methods with the required timing.++   It wouldn't take much to make this an abstract base class so that+   the SegwayIO can use it, and then have different CAN hardwares+   implement the virtual methods.  So then we can just drop in +   a new CAN hardware driver class and everything would still work.+   Would also be able to split up the files, so we could keep+   canio.[cc,h] in player, and the CAN hardware specific files+   can be local.+*/++class CANIO+{+public:+    CANIO() {}+    virtual ~CANIO() {}+    virtual int Init() = 0;+    virtual int ReadPacket(CanPacket *pkt) = 0;+    virtual int WritePacket(CanPacket &pkt) = 0;+    virtual int Shutdown() = 0;+};++#endif
+ src/System/RMP/canio_rmpusb.cpp view
@@ -0,0 +1,392 @@+/* Taken and modified from http://www.ai.sri.com/~vincent/segway.php */+#include <sys/time.h>+#include <time.h>+#include <unistd.h>+#include <cstdlib>++#include "ftd2xx.h"+#include "canio.h"++#include "canio_rmpusb.h"++#define DEBUG_PRINT(fmt, ...)++CANIOrmpusb::CANIOrmpusb(const char* serial) : ready(false)+{+    this->serial = serial;+    DEBUG_PRINT("serial is set in CANIO : %s\n",this->serial);+}++CANIOrmpusb::~CANIOrmpusb()+{+}++int+CANIOrmpusb::Init()+{+    DWORD iVID = 0x0403;  // Vendor ID for Future Technology Devices Inc+    DWORD iPID = 0xE729;  // Product ID for FTD245BM chip in the Segway RMP+    FT_DEVICE ftDevice;+    DWORD deviceID;+    char SerialNumber[16];+    char Description[64];+++    ftStatus = FT_SetVIDPID(iVID, iPID);	// use our VID and PID+    if(ftStatus != FT_OK) {+        DEBUG_PRINT("Unable to set appropriate VID/PID for USB device\n");+        return ftStatus;+    }++    if(this->serial !=  NULL) {+	strncpy(SerialNumber,this->serial,16); +        DEBUG_PRINT("Looking to connect to Segway #:%s\n",SerialNumber);+        ftStatus = FT_OpenEx(SerialNumber, FT_OPEN_BY_SERIAL_NUMBER, &ftHandle);+    }+    else {+        char desc[] = "Robotic Mobile Platform";+        ftStatus = FT_OpenEx(desc,FT_OPEN_BY_DESCRIPTION,&ftHandle);+    }+    if(ftStatus != FT_OK) {+        DEBUG_PRINT("FT_Open(0) failed\n");+        return ftStatus;+    }++    // Get the info+    ftStatus = FT_GetDeviceInfo(ftHandle, &ftDevice,&deviceID,SerialNumber,Description,NULL);+//   if (ftStatus == FT_OK) {+//     DEBUG_PRINT("  SerialNumber=%s\n",SerialNumber);+//     DEBUG_PRINT("  Description=[%s]\n",Description);+//     DEBUG_PRINT("  ftHandle=0x%x\n",ftHandle);+//   }++    +    // Boost the baud rate+    ftStatus = FT_SetBaudRate(ftHandle,FT_BAUD_460800);+    if(ftStatus != FT_OK) {+        DEBUG_PRINT("Unable to increase USB baud rate\n");+        FT_Close(ftHandle);+        return ftStatus;+    }++    // Decrease the internal latency timer+    ftStatus = FT_SetLatencyTimer(ftHandle,2);+    if (ftStatus != FT_OK) {+        DEBUG_PRINT("Unable to decrease latency timer...continuing anyway\n");+    }+    ++    // Set a timeout value of 10ms for reads and writes+    ftStatus = FT_SetTimeouts(ftHandle,10,10);+    if (ftStatus != FT_OK) {+        DEBUG_PRINT("FT_SetTimeouts failed\n");+        FT_Close(ftHandle);+        return ftStatus;+    }+    +    DWORD rsize;+    ftStatus = FT_GetQueueStatus(ftHandle,&rsize);+    if (ftStatus != FT_OK) {+        DEBUG_PRINT("Unable to get Queue status\n");+    } else {+        DEBUG_PRINT("At Init there are %d characters in read queue\n",rsize);+    }++    ftStatus = FT_ResetDevice(ftHandle);+    if (ftStatus != FT_OK) {+        DEBUG_PRINT("Unable to reset USB FIFO\n");+        FT_Close(ftHandle);+        return ftStatus;+    }+ +    ftStatus = FT_Purge(ftHandle,FT_PURGE_RX | FT_PURGE_TX);+    if (ftStatus != FT_OK) {+        DEBUG_PRINT("Unable to clear USB read/write buffers\n");+        FT_Close(ftHandle);+        return ftStatus;+    }++    ftStatus = FT_ResetDevice(ftHandle);+    if (ftStatus != FT_OK) {+        DEBUG_PRINT("Unable to reset USB FIFO\n");+        FT_Close(ftHandle);+        return ftStatus;+    }++    ftStatus = FT_GetQueueStatus(ftHandle,&rsize);+    if (ftStatus != FT_OK) {+        DEBUG_PRINT("Unable to get Queue status\n");+    } else {+        DEBUG_PRINT("After purge/reset there are %d characters in read queue\n",rsize);+    }+    ready = true;+    rcount = timeouts = 0;+    rxbufcount = 0;+    writecount = readcount = 0;+    usbreadcount = nomsgreadcount = 0;+    return 0;+}++/* Closes the CAN channel+ *+ * returns: 0 on success, nonzero error code otherwise+ */+int+CANIOrmpusb::Shutdown()+{+    ftStatus = FT_Close(ftHandle);+    if(ftStatus != FT_OK) {+        DEBUG_PRINT("FT_Close failed\n");+        return ftStatus;+    }+    ready = false;+    return 0;+}+  +/* Writes the given packet+ *+ * returns: 0 on success, nonzero error code otherwise+ */+int+CANIOrmpusb::WritePacket(CanPacket &cpkt)+{+    DWORD bytes_written;+    DWORD bytes_to_write;++    writecount++;+#ifdef PRINTSTATS+    if ((writecount % 100) == 0) {+        DEBUG_PRINT("writes %d  read calls %d  device reads %d  aborted reads %d\n",writecount,readcount,usbreadcount,nomsgreadcount);+    }+#endif++    if (!ready) {+        return -1;+    }++    static struct timeval *last=NULL;+    struct timeval curr;+    +    // initialize the first timeval if necessary+    if (!last) {+        last = new struct timeval;+        gettimeofday(last,NULL);+    }++    // get the current time+    gettimeofday(&curr,NULL);+    +    // calculate how long since the last write+    double msecs = (curr.tv_sec - last->tv_sec)*1000 + (curr.tv_usec - last->tv_usec) / 1000;+    +    // if it's been less than 30 milliseconds, sleep so that we don't+    // overload the CAN bus+    if (msecs < 30) {+        usleep((30-msecs)*1000);+//        usleep(100*1000);+    }++    // create a USB packet from our CAN packet+    CANIOrmpusb::rmpUsbPacket packet(cpkt);++    DEBUG_PRINT("USB packet created: %s\n",packet.toString());+    +    bytes_to_write = 18;+    unsigned char *pData = packet.bytes;+    while (bytes_to_write > 0) {+        bytes_written = 0;+        ftStatus = FT_Write(ftHandle,pData,bytes_to_write,&bytes_written);+        if (ftStatus != FT_OK) {+            DEBUG_PRINT("Error while trying to write packet to USB port\n");+            return ftStatus;+        }+        if (bytes_written > bytes_to_write) {+            DEBUG_PRINT("Erronous bytes written returned by FT_Write: %lu", bytes_written);+            return -1;+        }+        bytes_to_write -= bytes_written;+        pData += bytes_written;+    }+    //    DEBUG_PRINT("USB packet sent: %s\n",packet.toString());++    // record the time+    gettimeofday(last,NULL);++    return 0;+}+        ++/* Reads a packet from the USB bus, and extracts the CAN data.+ */+int+CANIOrmpusb::ReadPacket(CanPacket *pkt)+{+    // We'll read as much as we possibly can, since the read call is slow.+    // The extra bytes will be kept around until next time, and just processed+    // from memory.++    readcount++;+    DWORD bytes_read;+    +    if (!ready) {+        return -1;+    }++    CANIOrmpusb::rmpUsbPacket packet;+++    if (rxbufcount > RX_WARNING_SIZE) {+        // Player is falling behind in processing the messages, so throw out+        // the oldest bytes+        memcpy(rxbuf,rxbuf+rxbufcount-RX_KEEP_SIZE,RX_KEEP_SIZE);+	//        DEBUG_PRINT("Warning: purged %d old bytes from USB interface\n", rxbufcount-RX_KEEP_SIZE);+        rxbufcount = RX_KEEP_SIZE;+    }+        +    if (rxbufcount < 180) {+        // only read more from the device if we're running low+        DWORD rsize;+        ftStatus = FT_GetQueueStatus(ftHandle,&rsize);+        if (ftStatus != FT_OK) {+            DEBUG_PRINT("Unable to get Queue status\n");+            return -1;+        }+        +        // Limit what we will read to the space left in the buffer+        if (rsize > (RX_BUFFER_SIZE - rxbufcount)) {+            rsize = RX_BUFFER_SIZE - rxbufcount;+        }+        +        if (rsize < 18) {+            // not a full packet available+            // it seems that if we try to read something now, it+            // provokes the FTDI chip into producing garbage, so instead+            // we'll wait until later to get at least 1 full packet+            return 0;+        }+        ftStatus = FT_Read(ftHandle,rxbuf+rxbufcount,rsize,&bytes_read);+        usbreadcount++;+        if (ftStatus != FT_OK) {+            DEBUG_PRINT("Error reading from USB device\n");+            return -1;+        }++        rxbufcount += bytes_read;++    }+    +    int i;+    // Now, search for the next valid packet+    for ( i = 0; i < ((int)rxbufcount-17); i++) {+        if ((rxbuf[i] != 0xF0) || (rxbuf[i+1] != 0x55) || (rxbuf[i+2] != 0xAA)) {+            continue;+        }+        +        // move bytes into 18-byte USB packet and check+        memcpy(packet.bytes,rxbuf+i,18);+        if(packet.computeChecksum() != packet.bytes[17]) {+            continue;+        }++        // extract CAN packet+        // this is what the v2.0 preliminary documentation says: ID is located in+        // bytes 6 and 7, but it doesn't actually say which 11 bits to use, so I was+        // using the low bits.  This is how packets are created when sending, but+        // I've learned it's totally wrong for received packets.+        // pkt->id = ((packet.bytes[6] << 8) + packet.bytes[7]) & 0x7FF;+        +        // this is what the Segway RMI_DEMO does for received packets.  It uses all 8+        // bits from byte 4 and the first 3 bits from byte 5.+        pkt->id = ((packet.bytes[4] << 3) | ((packet.bytes[5] >> 5) & 7)) & 0xfff;+        memcpy(pkt->msg,packet.pDATA,8);+        //  DEBUG_PRINT("CAN packet extracted: %s\n", pkt->toString());+        +        // shift the receive buffer+        memcpy(rxbuf,rxbuf+i+18,rxbufcount-i-18);+        rxbufcount -= (i+18);+        ++        // return the CAN bytes received+        return 10;+    }++    // we didn't find any valid messages+    // shift the receive buffer+    memcpy(rxbuf,rxbuf+i,rxbufcount-i);+    rxbufcount -= i;+    +    nomsgreadcount++;+    return 0;++}+++CANIOrmpusb::rmpUsbPacket::rmpUsbPacket() {+    InitPacket();+}++CANIOrmpusb::rmpUsbPacket::rmpUsbPacket(CanPacket &cpkt) {+    InitPacket();+    bytes[6] = (cpkt.id >> 8) & 0xff;+    bytes[7] = cpkt.id & 0xff;+    memcpy(pDATA,cpkt.msg,8);+    addChecksum();+}++void CANIOrmpusb::rmpUsbPacket::InitPacket() {+    bytes[0] = 0xF0;+    bytes[1] = 0x55;+    bytes[2] = 0;+    bytes[3] = 0;+    bytes[4] = 0;+    bytes[5] = 0;+    bytes[6] = 0;+    bytes[7] = 0;+    bytes[8] = 0;+    bytes[9] = 0;+    bytes[10] = 0;+    bytes[11] = 0;+    bytes[12] = 0;+    bytes[13] = 0;+    bytes[14] = 0;+    bytes[15] = 0;+    bytes[16] = 0;+    bytes[17] = 0;+    pID = bytes+6;+    pDATA = bytes+9;+}++CANIOrmpusb::rmpUsbPacket::~rmpUsbPacket() {+}++unsigned char CANIOrmpusb::rmpUsbPacket::computeChecksum() {+    // code copied from Segway RMP Interface Guide+    unsigned short checksum;+    unsigned short checksum_high;+    checksum = 0;+    for (int i = 0; i < 17; i++) {+        checksum += (short)bytes[i];+    }+    checksum_high = checksum >> 8;+    checksum &= 0xff;+    checksum += checksum_high;+    checksum_high = checksum >> 8;+    checksum &= 0xff;+    checksum += checksum_high;+    checksum = (~checksum +1) & 0xff;+    return (unsigned char) checksum;+}++void CANIOrmpusb::rmpUsbPacket::addChecksum() {+    bytes[17] = computeChecksum();+}++char * CANIOrmpusb::rmpUsbPacket::toString() {+    static char buf[256];+    buf[0] = 0;+    for (int i = 0; i < 18; i++) {+        sprintf(buf+strlen(buf),"%d:%02X ",i,bytes[i]);+    }+    return buf;+}+
+ src/System/RMP/canio_rmpusb.h view
@@ -0,0 +1,51 @@+/* Taken and modified from http://www.ai.sri.com/~vincent/segway.php */+#ifndef _RMPUSB_CANLIB_+#define _RMPUSB_CANLIB_++#include "ftd2xx.h"+#include "canio.h"++class CANIOrmpusb : public CANIO+{+private:+    FT_STATUS	ftStatus;+    FT_HANDLE	ftHandle;+    static const unsigned int RX_BUFFER_SIZE = 65536;+    static const unsigned int RX_WARNING_SIZE = 500;+    static const unsigned int RX_KEEP_SIZE = 200;+    unsigned char rxbuf[RX_BUFFER_SIZE];+    +    bool ready;+    const char* serial;+    int rcount;+    int timeouts;+    int writecount, readcount;+    int usbreadcount, nomsgreadcount;+    unsigned int rxbufcount;+    class rmpUsbPacket+    {+    public:+        rmpUsbPacket();+        rmpUsbPacket(CanPacket &cpkt);+        ~rmpUsbPacket();+        void InitPacket();+        unsigned char computeChecksum();+        void addChecksum();+        char *toString();++        unsigned char bytes[18];+        unsigned char *pID;+        unsigned char *pDATA;+    };++    +public:+    CANIOrmpusb(const char* serial=NULL);+    virtual ~CANIOrmpusb();+    virtual int Init();+    virtual int ReadPacket(CanPacket *pkt);+    virtual int WritePacket(CanPacket &pkt);+    virtual int Shutdown();+};++#endif // _RMPUSB_CANLIB_
+ src/System/RMP/rmpusb.cpp view
@@ -0,0 +1,81 @@+/* C++ -> C Interface */++#include <unistd.h>+#include "canio_rmpusb.h"+#include "rmpusb.h"++struct rmpusb {+    CANIOrmpusb *canio;+};++struct rmppacket {+    CanPacket pkt;+};++rmpusb_t *rmpusb_new()+{+    rmpusb_t *res = new rmpusb_t;+    if (NULL == res) {+        return NULL;+    }+    +    res->canio = new CANIOrmpusb;+    if (NULL == res->canio) {+        goto ERROR_EXIT;+    }++    if (0 != res->canio->Init()) {+        goto ERROR_EXIT;+    }++    return res;++ERROR_EXIT:+    delete res;+    return NULL;+    +}++void rmpusb_delete(rmpusb_t *rmpusb)+{+    if (NULL == rmpusb) {+        return;+    }+    if (NULL != rmpusb->canio) {+        rmpusb->canio->Shutdown();+        delete rmpusb->canio;+    }+    delete rmpusb;+}++int rmpusb_write_packet(rmpusb_t *rmpusb, rmppacket_t *cnpkt)+{+    return rmpusb->canio->WritePacket(cnpkt->pkt);+}++int rmpusb_read_packet(rmpusb_t *rmpusb, rmppacket_t *cnpkt)+{+    return rmpusb->canio->ReadPacket(&(cnpkt->pkt));+}++/******************************************************************************/++rmppacket_t *rmppacket_new() {+    return new rmppacket_t;+}++void rmppacket_delete(rmppacket_t *pkt) {+    delete pkt;+}+++void rmppacket_set_command_velocity(rmppacket_t *res, int trans, int rot) {+    res->pkt.Clear();+    res->pkt.id = 0x0413; // RMP_CAN_ID_COMMAND;+    res->pkt.PutSlot(0, (uint16_t)trans);+    res->pkt.PutSlot(1, (uint16_t)rot);+    res->pkt.PutSlot(2, (uint16_t)0); // RMP_CAN_CMD_NONE+    res->pkt.PutSlot(3, (uint16_t)0);+}++
+ src/System/RMP/rmpusb.h view
@@ -0,0 +1,24 @@+#ifndef _RMP_USB_H_+#define _RMP_USB_H_++extern "C" {+    +typedef struct rmpusb rmpusb_t;++typedef struct rmppacket rmppacket_t;+++rmpusb_t *rmpusb_new();+void rmpusb_delete(rmpusb_t *rmpusb);+int rmpusb_write_packet(rmpusb_t *rmpusb, rmppacket_t *cnpkt);+int rmpusb_read_packet(rmpusb_t *rmpusb, rmppacket_t *cnpkt);++/******************************************************************************/++rmppacket_t *rmppacket_new();+void rmppacket_delete(rmppacket_t *pkt);+void rmppacket_set_command_velocity(rmppacket_t *res, int trans, int rot);++};++#endif
+ src/Test.hs view
@@ -0,0 +1,13 @@+module Main where++import System.RMP+import Control.Processor(runUntil, IOProcessor, trace)++import Control.Arrow+++controller :: IOProcessor () (Int, Int)+controller = arr . const $ (0,-20)+  +main :: IO () +main = runUntil (controller >>> trace >>> velocityRMP) () (const . return $ False)