jakob.kampichler/libcreate
jakob.kampichler/libcreate
1
0
Fork 0
forked from compAIR/libcreate
Code Pull requests Activity

Add support for early model Roomba 400s and other robots using the original SCI protocol.

Browse source
This commit is contained in:
Ben Wolsieffer 2016-07-13 21:50:32 -04:00
parent c68a308c71
commit 618956e14c
18 changed files with 675 additions and 307 deletions
Download patch file Download diff file Expand all files Collapse all files

94
src/serial.cpp
View file

@ -5,11 +5,9 @@
namespace create {
Serial::Serial(boost::shared_ptr<Data> d, const uint8_t& header) :
Serial::Serial(boost::shared_ptr<Data> d) :
data(d),
headerByte(header),
port(io),
readState(READ_HEADER),
isReading(false),
dataReady(false),
corruptPackets(0),
@ -64,27 +62,10 @@ namespace create {
// Only allow once
if (isReading) return true;
// Request from Create that we want a stream containing all packets
uint8_t numPackets = data->getNumPackets();
std::vector<uint8_t> packetIDs = data->getPacketIDs();
uint8_t streamReq[2 + numPackets];
streamReq[0] = OC_STREAM;
streamReq[1] = numPackets;
int i = 2;
for (std::vector<uint8_t>::iterator it = packetIDs.begin(); it != packetIDs.end(); ++it) {
streamReq[i] = *it;
i++;
}
// Start OI
sendOpcode(OC_START);
// Start streaming data
send(streamReq, 2 + numPackets);
expectedNumBytes = data->getTotalDataBytes() + numPackets;
//TODO: handle boost exceptions
if (!startSensorStream()) return false;
io.reset();
@ -112,7 +93,7 @@ namespace create {
// Request data again
sendOpcode(OC_START);
send(streamReq, 2 + numPackets);
startSensorStream();
}
}
@ -158,74 +139,7 @@ namespace create {
// Should have read exactly one byte
if (size == 1) {
numBytesRead++;
byteSum += byteRead;
switch (readState) {
case READ_HEADER:
if (byteRead == headerByte) {
readState = READ_NBYTES;
byteSum = byteRead;
}
break;
case READ_NBYTES:
if (byteRead == expectedNumBytes) {
readState = READ_PACKET_ID;
numBytesRead = 0;
}
else {
//notifyDataReady();
readState = READ_HEADER;
}
break;
case READ_PACKET_ID:
packetID = byteRead;
if (data->isValidPacketID(packetID)) {
expectedNumDataBytes = data->getPacket(packetID)->nbytes;
assert(expectedNumDataBytes == 1 || expectedNumDataBytes == 2);
numDataBytesRead = 0;
packetBytes = 0;
readState = READ_PACKET_BYTES;
}
else {
//notifyDataReady();
readState = READ_HEADER;
}
break;
case READ_PACKET_BYTES:
numDataBytesRead++;
if (expectedNumDataBytes == 2 && numDataBytesRead == 1) {
// High byte first
packetBytes = (byteRead << 8) & 0xff00;
}
else {
// Low byte
packetBytes += byteRead;
}
if (numDataBytesRead >= expectedNumDataBytes) {
data->getPacket(packetID)->setTempData(packetBytes);
if (numBytesRead >= expectedNumBytes)
readState = READ_CHECKSUM;
else
readState = READ_PACKET_ID;
}
break;
case READ_CHECKSUM:
if ((byteSum & 0xFF) == 0) {
notifyDataReady();
}
else {
// Corrupt data
corruptPackets++;
}
totalPackets++;
// Start again
readState = READ_HEADER;
break;
} // end switch (readState)
processByte(byteRead);
} // end if (size == 1)
// Read the next byte