void SocksConnection::Close()
{
if(!_socket_open) return;
_socket_open = false;
if(_conn_id.count()) {
qDebug() << "MEM Send finish";
SendUpstreamPacket(FinishPacket(_conn_id).ToByteArray());
}
_socket->close();
_socket->deleteLater();
qDebug() << "Close()";
emit Closed();
}
/* ReadJointAngle - sends out a single read to a joint motor
* Return - joint angle in ticks of prompted joint
* - for failed read, return -99999
*/
int DarwinController::ReadJointAngle(int id){
unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
unsigned char txpacketread[] = {0, 0, id, 0x04, 0x02, 0x24, 0x02, 0};
FinishPacket(txpacketread);
int result = ReadWrite(txpacketread, rxpacket);
if(result == 0){
printf("Failed read! \n");
return -9999999;
} else {
return MakeWord((int)rxpacket[PARAMETER], (int)rxpacket[PARAMETER + 1]);
}
}
/* Ping - send a ping to a specific id
* Return - true if the ping was successful
*/
bool DarwinController::Ping(int id, int *error){
unsigned char txpacket[MAXNUM_TXPARAM + 10] = {0, };
unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
txpacket[ID] = (unsigned char)id;
txpacket[INSTRUCTION] = 1;
txpacket[LENGTH] = 2;
FinishPacket(txpacket);
int result = ReadWrite(txpacket, rxpacket);
if( result == 0){
return false;
} else {
return true;
}
}
/* ReadByte - Read a byte of data from the specified id and address
* Return - length of successfully read packet. Return bad number if fails.
*/
int DarwinController::ReadByte(int id, int address, int *byte){
unsigned char txpacket[MAXNUM_TXPARAM + 10] = {0, };
unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
txpacket[ID] = (unsigned char)id;
txpacket[INSTRUCTION] = READ;
txpacket[PARAMETER] = (unsigned char)address;
txpacket[PARAMETER+1] = 1;
txpacket[LENGTH] = 4;
int txlength = txpacket[LENGTH] + 4;
FinishPacket(txpacket); // don't know why i had this commented out.
int result = ReadWrite(txpacket, rxpacket);
if(result == txlength){
*byte = (int)rxpacket[PARAMETER];
}
return result;
}
/* WriteWord - Writes word out to the specified id and address
* Return - length of successfully read packet. Return -99999 if fails.
*/
int DarwinController::WriteWord(int id, int address, int value){
unsigned char txpacket[MAXNUM_TXPARAM + 10] = {0, };
txpacket[ID] = (unsigned char)id;
txpacket[INSTRUCTION] = WRITE;
txpacket[PARAMETER] = (unsigned char)address;
txpacket[PARAMETER+1] = (unsigned char)GetLowByte(value);
txpacket[PARAMETER+2] = (unsigned char)GetHighByte(value);
txpacket[LENGTH] = 5;
int txlength = txpacket[LENGTH] + 4;
FinishPacket(txpacket);
port.ClearPort();
int result = port.WritePort(txpacket, txlength);
if(result == txlength){ // do we want to return bool???
return result;
} else {
return -99999;
}
}
/* WriteByte - Writes a byte of data out to the specified id and address
* Return - length of successfully read packet. Return -99999 if fails.
*/
int DarwinController::WriteByte(int id, int address, int value){
unsigned char txpacket[MAXNUM_TXPARAM + 10] = {0, };
txpacket[ID] = (unsigned char)id;
txpacket[INSTRUCTION] = WRITE;
txpacket[PARAMETER] = (unsigned char)address;
txpacket[PARAMETER+1] = (unsigned char)value;
txpacket[LENGTH] = 4;
int txlength = txpacket[LENGTH] + 4;
FinishPacket(txpacket);
port.ClearPort();
int result = port.WritePort(txpacket, txlength);
if(result == txlength){
return result;
} else {
return -99999;
}
// robotis also sets an error bit -> do we want to do that?
}
static void StuffSample(BAROMETER *bp, ISIDL_TIMESTAMP *ttag, INT32 value)
{
char timestring[32];
static char *fid = "qdplus:StuffSample";
/* Since there are some startup delays in the barometer reading thread,
* we will discard all "missed" samples until we've seen one valid
* sample come through here. That will prevent the first packet from
* having a couple of unwarrented "missed" samples at the front.
*/
if (!bp->ValidSampleSeen) {
if (value == MISSED_BAROMETER_SAMPLE) {
return;
} else {
bp->ValidSampleSeen = TRUE;
}
}
if (bp->nsamp == 0) {
InsertQdplusSerialno(&bp->local.raw, bp->format.qdplus.serialno);
bp->out = StartPacket(&bp->local.raw, ttag, bp);
}
bp->out[bp->nsamp++] = htonl(value);
if (BarometerDebugEnabled()) {
utilLttostr(ttag->sys, 0, timestring);
LogMsg(LOG_INFO, "%s %3d/%d: %s %ld\n", bp->param.sn, bp->nsamp, bp->maxsamp, timestring, value);
}
if (bp->nsamp == bp->maxsamp) {
FinishPacket(&bp->local.raw, (UINT16) bp->nsamp, ttag);
bp->nsamp = 0;
ProcessLocalData(&bp->local);
}
}
