diff --git a/RMP.cabal b/RMP.cabal new file mode 100644 --- /dev/null +++ b/RMP.cabal @@ -0,0 +1,65 @@ +name: RMP +version: 0.0.2 +license: BSD3 +maintainer: Noam Lewis +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 , + 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 diff --git a/Setup.hs b/Setup.hs new file mode 100644 --- /dev/null +++ b/Setup.hs @@ -0,0 +1,2 @@ +import Distribution.Simple +main = defaultMain diff --git a/src/FaceFollowTest.hs b/src/FaceFollowTest.hs new file mode 100644 --- /dev/null +++ b/src/FaceFollowTest.hs @@ -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 + + + diff --git a/src/System/RMP.hs b/src/System/RMP.hs new file mode 100644 --- /dev/null +++ b/src/System/RMP.hs @@ -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 diff --git a/src/System/RMP/USB.hsc b/src/System/RMP/USB.hsc new file mode 100644 --- /dev/null +++ b/src/System/RMP/USB.hsc @@ -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 () + + + diff --git a/src/System/RMP/canio.h b/src/System/RMP/canio.h new file mode 100644 --- /dev/null +++ b/src/System/RMP/canio.h @@ -0,0 +1,94 @@ +/* Taken and modified from http://www.ai.sri.com/~vincent/segway.php */ +#ifndef _CANIO_H_ +#define _CANIO_H_ + +#include + +#include +#include +#include + +// Copied this from . 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 diff --git a/src/System/RMP/canio_rmpusb.cpp b/src/System/RMP/canio_rmpusb.cpp new file mode 100644 --- /dev/null +++ b/src/System/RMP/canio_rmpusb.cpp @@ -0,0 +1,392 @@ +/* Taken and modified from http://www.ai.sri.com/~vincent/segway.php */ +#include +#include +#include +#include + +#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; +} + diff --git a/src/System/RMP/canio_rmpusb.h b/src/System/RMP/canio_rmpusb.h new file mode 100644 --- /dev/null +++ b/src/System/RMP/canio_rmpusb.h @@ -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_ diff --git a/src/System/RMP/rmpusb.cpp b/src/System/RMP/rmpusb.cpp new file mode 100644 --- /dev/null +++ b/src/System/RMP/rmpusb.cpp @@ -0,0 +1,81 @@ +/* C++ -> C Interface */ + +#include +#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); +} + + diff --git a/src/System/RMP/rmpusb.h b/src/System/RMP/rmpusb.h new file mode 100644 --- /dev/null +++ b/src/System/RMP/rmpusb.h @@ -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 diff --git a/src/Test.hs b/src/Test.hs new file mode 100644 --- /dev/null +++ b/src/Test.hs @@ -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)