int RoboClaw::setMotorSpeed(e_motor motor, float value)
{
uint16_t sum = 0;
// bound
if (value > 1) { value = 1; }
if (value < -1) { value = -1; }
uint8_t speed = fabs(value) * 127;
// send command
if (motor == MOTOR_1) {
if (value > 0) {
return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
} else {
return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
}
} else if (motor == MOTOR_2) {
if (value > 0) {
return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
} else {
return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
}
}
return -1;
}
QVector<int> Elm327::_readPID(int mode, int pid) {
QStringList res;
QStringList data;
QVector<int> result;
QString reply = QString("%1 %2").arg(0x40 | mode, 2, 16, QLatin1Char('0')).arg(pid, 2, 16, QLatin1Char('0')).toUpper();
res = _sendCommand(QString("%1%2\r").arg(mode, 2, 16, QLatin1Char('0')).arg(pid, 2, 16, QLatin1Char('0')).toUpper());
//res.append("41 04 80");
//res.append("41 05 82");
//res.append("41 23 13 CE");
//res.append("41 0B 80");
//res.append("41 0F 28");
for(int i = 0; i < res.size(); i++) {
/* Szukamy odpowiedzi */
if (res.at(i).startsWith(reply)) { /* Odpowiedź */
data = res.at(i).split(' ');
for(int j = 2; j < data.size(); j++) {
result.append(data.at(j).toInt(NULL, 16));
}
}
}
return result;
}
boolean SOMO14D::play_complete(unsigned int song){
_sendCommand(song);
delay(20);
while (getState() == HIGH) delay(250);
return (digitalRead(_busyPin) == HIGH);
}
//
// setVolume
//
// set a specific volume for SOMO-14D
void SOMO14D::setVolume(uint8_t volume){
if (volume > 7)
volume = 7;
_volume = volume + SOMO14D_MIN_VOLUME;
_sendCommand(_volume);
}
//-----------------------------string method--------------------------------------
void CRedisClient::_set(const string &key, const string &value, CResult &result, const string& suffix , long time,const string suffix2 )
{
_socket.clearBuffer();
Command cmd( "SET" );
cmd << key << value;
if ( suffix != "" )
{
cmd << suffix;
}
if ( time != 0 )
{
std::stringstream ss;
ss << time ;
cmd << ss.str();
}
if ( suffix2 != "" )
{
cmd << suffix2;
}
_sendCommand( cmd );
_getReply( result );
}
void CRedisClient::hmget(const string &key, const CRedisClient::VecString &fields, CResult &result)
{
_socket.clearBuffer();
Command cmd( "HMGET" );
cmd << key;
VecString::const_iterator it = fields.begin();
VecString::const_iterator end = fields.end();
for ( ; it != end; ++it )
{
cmd << *it;
}
_sendCommand( cmd );
_getReply( result );
ReplyType type = result.getType();
if ( REDIS_REPLY_ERROR == type )
{
throw ReplyErr( result.getErrorString() );
}else if ( REDIS_REPLY_ARRAY != type )
{
throw ProtocolErr( "HMGET: data recved is not arry" );
}
}
bool Elm327::_setProtocol(int num) {
QStringList res;
res = _sendCommand(QString("AT SP %1\r").arg(num, 0, 16).toUpper());
return ((res.size() > 0) && (res.at(0) == "OK"));
}
void CRedisClient::hvals(const string &key, CResult &result)
{
_socket.clearBuffer();
Command cmd( "HVALS" );
cmd << key ;
_sendCommand( cmd );
_getReply( result );
}
void CRedisClient::debugSegfault()
{
Command cmd( "DEBUG" );
cmd<<"SEGFAULT";
_socket.clearBuffer();
_sendCommand(cmd);
}
void CRedisClient::hexists(const string &key, const string &field, CResult &result)
{
_socket.clearBuffer();;
Command cmd( "HEXISTS" );
cmd << key << field;
_sendCommand( cmd );
_getReply( result );
}
void CRedisClient::hincrbyfloat(const string &key, const string &field, float increment, CResult &result)
{
_socket.clearBuffer();
Command cmd( "HINCRBYFLOAT" );
cmd << key << field << increment;
_sendCommand( cmd );
_getReply( result );
}
void CRedisClient::hsetnx(const string &key, const string &field, const string &value, CResult &result)
{
_socket.clearBuffer();
Command cmd( "HSETNX" );
cmd << key << field << value;
_sendCommand( cmd );
_getReply( result );
}
void CRedisClient::get(const std::string &key, CResult& result )
{
_socket.clearBuffer();
Command cmd( "GET" );
cmd << key ;
_sendCommand( cmd );
_getReply( result );
}
void Elm327::start() {
if (!_serial->isOpen()) {
qDebug() << "ELM327: Serial not opened";
return;
}
_sendCommand("AT @1");
_setProtocol(0);
_started = true;
_watchTimer->start();
}
QNearFieldTarget::RequestId QNearFieldTagType3Symbian::check(const QMap<quint16, QList<quint16> > &serviceBlockList)
{
BEGIN
quint8 numberOfBlocks;
QByteArray command;
command.append(0x06); // command code
command.append(serviceBlockList2CmdParam(serviceBlockList, numberOfBlocks, true));
if (command.count() > 1)
{
END
return (_sendCommand(command));
}
void CRedisClient::slowlog(const CRedisClient::VecString &subcommand, CResult &reply)
{
Command cmd( "SLOWLOG" );
_socket.clearBuffer();
VecString::const_iterator it = subcommand.begin();
VecString::const_iterator end=subcommand.end();
for ( ; it !=end; ++it )
{
cmd << *it;
}
_sendCommand(cmd);
_getReply(reply);
}
void CRedisClient::monitor(uint64_t timeout, string& reply)
{
Command cmd("MONITOR");
_socket.clearBuffer();
_socket.setReceiveTimeout(timeout);
_sendCommand(cmd);
while(1)
{
_socket.readLine(reply);
reply=reply.substr(1);
//std::cout<<reply<<std::endl;
}
}
int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
{
uint16_t sum = 0;
// bound
if (value > 1) { value = 1; }
if (value < -1) { value = -1; }
int16_t duty = 1500 * value;
// send command
if (motor == MOTOR_1) {
return _sendCommand(CMD_SIGNED_DUTYCYCLE_1,
(uint8_t *)(&duty), 2, sum);
} else if (motor == MOTOR_2) {
return _sendCommand(CMD_SIGNED_DUTYCYCLE_2,
(uint8_t *)(&duty), 2, sum);
}
return -1;
}
void CRedisClient::hdel(const string &key, const CRedisClient::VecString &fields, CResult &result )
{
_socket.clearBuffer();;
Command cmd( "HDEL" );
cmd << key;
VecString::const_iterator it = fields.begin();
VecString::const_iterator end = fields.begin();
for ( ; it != end; ++it )
{
cmd << *it;
}
_sendCommand( cmd );
_getReply( result );
}
double Elm327::_readVoltage() {
QStringList res;
QString tmp;
double voltage;
res = _sendCommand("AT RV\r");
if (res.size() < 1)
return 0;
tmp = res.at(0);
if (tmp.isEmpty())
return 0;
voltage = tmp.left(tmp.size() - 1).toDouble();
return voltage;
}
void CRedisClient::punsubscribe( CResult& result, const VecString& pattern )
{
_socket.clearBuffer();
Command cmd( "PUNSUBSCRIBE" );
if ( pattern.size() != 0 )
{
VecString::const_iterator it = pattern.begin();
for ( ; it != pattern.end(); ++it )
{
cmd << *it ;
}
}
_sendCommand( cmd );
_getReply( result );
}
void CRedisClient::unsubscribe( CResult& result, const VecString& channel )
{
_socket.clearBuffer();
Command cmd( "UNSUBSCRIBE" );
if ( channel.size() != 0 )
{
VecString::const_iterator it = channel.begin();
for ( ; it != channel.end(); ++it )
{
cmd << *it ;
}
}
_sendCommand( cmd );
_getResult( cmd, result );
}
void CRedisClient::hmset(const string &key, const CRedisClient::MapString &pairs, CResult &result)
{
_socket.clearBuffer();;
Command cmd( "HMSET" );
cmd << key;
MapString::const_iterator it = pairs.begin();
MapString::const_iterator end = pairs.end();
for ( ; it !=end ; ++it )
{
cmd << it->first;
cmd << it->second;
}
_sendCommand( cmd );
_getReply( result );
}
void CRedisClient::hscan(const string &key, int64_t cursor, const string &match, uint64_t count, CResult &result)
{
_socket.clearBuffer();
Command cmd( "HSCAN" );
cmd << key << cursor;
if ( "" != match )
{
cmd << "MATH" << match;
}
if ( 0 != count )
{
cmd << "COUNT" << count;
}
_sendCommand( cmd );
_getReply( result );
}
int SSD1306::commit()
{
_sendCommand(SSD1306_COLUMNADDR);
_sendCommand(0); // Column start address (0 = reset)
_sendCommand(127); // Column end address (127 = reset)
_sendCommand(SSD1306_PAGEADDR);
_sendCommand(0); // Page start address (0 = reset)
_sendCommand(7); // Page end address
uint8_t data[SSD1306_TRANSACTION_SIZE+1];
for (size_t i=0; i < width()*height()/8/SSD1306_TRANSACTION_SIZE; i++) {
data[0] = 0x40;
memcpy(data+1, _buffer+i*SSD1306_TRANSACTION_SIZE, SSD1306_TRANSACTION_SIZE);
_i2c->writeBatch(_address, sizeof(data), data);
}
return 0;
}
int SSD1306::initialize()
{
// TODO: remove magic
_sendCommand(SSD1306_DISPLAYOFF);
_sendCommand(SSD1306_SETDISPLAYCLOCKDIV);
_sendCommand(0xF0);
_sendCommand(SSD1306_SETMULTIPLEX);
_sendCommand(0x3F);
_sendCommand(SSD1306_SETDISPLAYOFFSET);
_sendCommand(0x00);
_sendCommand(SSD1306_SETSTARTLINE | 0x0);
_sendCommand(SSD1306_CHARGEPUMP);
_sendCommand(_ext_vcc ? 0x10 : 0x14);
_sendCommand(SSD1306_MEMORYMODE);
_sendCommand(0x00);
_sendCommand(SSD1306_SEGREMAP | 0x1);
_sendCommand(SSD1306_COMSCANDEC);
_sendCommand(SSD1306_SETCOMPINS);
_sendCommand(0x12);
_sendCommand(SSD1306_SETCONTRAST);
_sendCommand(_ext_vcc ? 0x9F : 0xCF);
_sendCommand(SSD1306_SETPRECHARGE);
_sendCommand(_ext_vcc ? 0x22 : 0xF1);
_sendCommand(SSD1306_SETVCOMDETECT);
_sendCommand(0x40);
_sendCommand(SSD1306_DISPLAYALLON_RESUME);
_sendCommand(SSD1306_NORMALDISPLAY);
_sendCommand(SSD1306_DISPLAYON);
return 0;
}
int SSD1306::setInverted(bool inverted)
{
return _sendCommand(inverted ? SSD1306_INVERTDISPLAY : SSD1306_NORMALDISPLAY);
}
int RoboClaw::resetEncoders()
{
uint16_t sum = 0;
return _sendCommand(CMD_RESET_ENCODERS,
nullptr, 0, sum);
}
QNearFieldTarget::RequestId QNearFieldTagMifareSymbian::sendCommand(const QByteArray &command)
{
return _sendCommand(command);
}
int RoboClaw::readEncoder(e_motor motor)
{
uint16_t sum = 0;
if (motor == MOTOR_1) {
_sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum);
} else if (motor == MOTOR_2) {
_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
}
uint8_t rbuf[50];
usleep(5000);
int nread = read(_uart, rbuf, 50);
if (nread < 6) {
printf("failed to read\n");
return -1;
}
//printf("received: \n");
//for (int i=0;i<nread;i++) {
//printf("%d\t", rbuf[i]);
//}
//printf("\n");
uint32_t count = 0;
uint8_t *countBytes = (uint8_t *)(&count);
countBytes[3] = rbuf[0];
countBytes[2] = rbuf[1];
countBytes[1] = rbuf[2];
countBytes[0] = rbuf[3];
uint8_t status = rbuf[4];
uint8_t checksum = rbuf[5];
uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
checksum_mask;
// check if checksum is valid
if (checksum != checksum_computed) {
printf("checksum failed: expected %d got %d\n",
checksum, checksum_computed);
return -1;
}
int overFlow = 0;
if (status & STATUS_REVERSE) {
//printf("roboclaw: reverse\n");
}
if (status & STATUS_UNDERFLOW) {
//printf("roboclaw: underflow\n");
overFlow = -1;
} else if (status & STATUS_OVERFLOW) {
//printf("roboclaw: overflow\n");
overFlow = +1;
}
static int64_t overflowAmount = 0x100000000LL;
if (motor == MOTOR_1) {
_motor1Overflow += overFlow;
_motor1Position = float(int64_t(count) +
_motor1Overflow * overflowAmount) / _pulsesPerRev;
} else if (motor == MOTOR_2) {
_motor2Overflow += overFlow;
_motor2Position = float(int64_t(count) +
_motor2Overflow * overflowAmount) / _pulsesPerRev;
}
return 0;
}
