command.cpp

Go to the documentation of this file.

/*****************************************************************************
** Includes
*****************************************************************************/

#include "../../include/kobuki_driver/command.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace kobuki {

/*****************************************************************************
** Static variables initialization
*****************************************************************************/

const unsigned char Command::header0 = 0xaa;
const unsigned char Command::header1 = 0x55;

/*****************************************************************************
** Implementation [Command Generators]
*****************************************************************************/

Command Command::SetLedArray(const enum LedNumber &number, const enum LedColour &colour, Command::Data &current_data)
{
  // gp_out is 16 bits
  uint16_t value;
  if (number == Led1)
  {
    value = colour; // defined with the correct bit specification.
    current_data.gp_out = (current_data.gp_out & 0xfcff) | value; // update first
  }
  else
  {
    value = colour << 2;
    current_data.gp_out = (current_data.gp_out & 0xf3ff) | value; // update first
  }
  Command outgoing;
  outgoing.data = current_data;
  outgoing.data.command = Command::SetDigitalOut;
  return outgoing;
}

Command Command::SetDigitalOutput(const DigitalOutput &digital_output, Command::Data &current_data)
{
  uint16_t values = 0x0000;
  uint16_t clear_mask = 0xfff0;
  for ( unsigned int i = 0; i < 4; ++i ) {
    if ( digital_output.mask[i] ) {
      if ( digital_output.values[i] ) {
        values |= ( 1 << i );
      }
    } else {
      clear_mask |= ( 1 << i ); // don't clear this bit, so set a 1 here
    }
  }
  current_data.gp_out = (current_data.gp_out & clear_mask) | values;
  Command outgoing;
  outgoing.data = current_data;
  outgoing.data.command = Command::SetDigitalOut;
  return outgoing;
}

Command Command::SetExternalPower(const DigitalOutput &digital_output, Command::Data &current_data)
{
  uint16_t values = 0x0000;
  uint16_t clear_mask = 0xff0f;
  for ( unsigned int i = 0; i < 4; ++i ) {
    if ( digital_output.mask[i] ) {
      if ( digital_output.values[i] ) {
        values |= ( 1 << (i+4) );
      }
    } else {
      clear_mask |= ( 1 << (i+4) ); // don't clear this bit, so set a 1 here
    }
  }
  current_data.gp_out = (current_data.gp_out & clear_mask) | values;
  Command outgoing;
  outgoing.data = current_data;
  outgoing.data.command = Command::SetDigitalOut;
  return outgoing;
}

Command Command::PlaySoundSequence(const enum SoundSequences &number, Command::Data & /* current_data */)
{
  uint16_t value; // gp_out is 16 bits
  value = number; // defined with the correct bit specification.

  Command outgoing;
  outgoing.data.segment_name = value;
  outgoing.data.command = Command::SoundSequence;
  return outgoing;
}

Command Command::GetVersionInfo()
{
  Command outgoing;
  outgoing.data.request_flags = 0;
  outgoing.data.request_flags |= static_cast<uint16_t>(HardwareVersion);
  outgoing.data.request_flags |= static_cast<uint16_t>(FirmwareVersion);
  outgoing.data.request_flags |= static_cast<uint16_t>(UniqueDeviceID);
  outgoing.data.command = Command::RequestExtra;
  return outgoing;
}

Command Command::SetVelocityControl(DiffDrive& diff_drive)
{
  Command outgoing;
  std::vector<short> velocity_commands = diff_drive.velocityCommands();
  outgoing.data.speed = velocity_commands[0];
  outgoing.data.radius = velocity_commands[1];
  outgoing.data.command = Command::BaseControl;
  return outgoing;
}

Command Command::SetVelocityControl(const int16_t &speed, const int16_t &radius)
{
  Command outgoing;
  outgoing.data.speed = speed;
  outgoing.data.radius = radius;
  outgoing.data.command = Command::BaseControl;
  return outgoing;
}

Command Command::SetControllerGain(const unsigned char &type, const unsigned int &p_gain,
                                   const unsigned int &i_gain, const unsigned int &d_gain)
{
  Command outgoing;
  outgoing.data.type = type;
  outgoing.data.p_gain = p_gain;
  outgoing.data.i_gain = i_gain;
  outgoing.data.d_gain = d_gain;
  outgoing.data.command = Command::SetController;
  return outgoing;
}

Command Command::GetControllerGain()
{
  Command outgoing;
  outgoing.data.command = Command::GetController;
  outgoing.data.reserved = 0;
  return outgoing;
}

/*****************************************************************************
** Implementation [Serialisation]
*****************************************************************************/
void Command::resetBuffer(Buffer& buffer) {
  buffer.clear();
  buffer.resize(64);
  buffer.push_back(Command::header0);
  buffer.push_back(Command::header1);
  buffer.push_back(0); // just initialise, we usually write in the payload here later (size of payload only, not stx, not etx, not length)
}

bool Command::serialise(ecl::PushAndPop<unsigned char> & byteStream)
{
  // need to be sure we don't pass through an emum to the Trans'd buildBytes functions.
  unsigned char cmd = static_cast<unsigned char>(data.command);
  unsigned char length = 0;
  switch (data.command)
  {
    case BaseControl:
      buildBytes(cmd, byteStream);
      buildBytes(length=4, byteStream);
      buildBytes(data.speed, byteStream);
      buildBytes(data.radius, byteStream);
      break;
    case Sound:
      buildBytes(cmd, byteStream);
      buildBytes(length=3, byteStream);
      buildBytes(data.note, byteStream);
      buildBytes(data.duration, byteStream);
      break;
    case SoundSequence:
      buildBytes(cmd, byteStream);
      buildBytes(length=1, byteStream);
      buildBytes(data.segment_name, byteStream);
      break;
    case RequestExtra:
      buildBytes(cmd, byteStream);
      buildBytes(length=2, byteStream);
      buildBytes(data.request_flags, byteStream);
      break;
    case ChangeFrame:
      buildBytes(cmd, byteStream);
      buildBytes(length=1, byteStream);
      buildBytes(data.frame_id, byteStream);
      break;
    case RequestEeprom:
      buildBytes(cmd, byteStream);
      buildBytes(length=1, byteStream);
      buildBytes(data.frame_id, byteStream);
      break;
    case SetDigitalOut:
    { // this one controls led, external power sources, gp digitial output
      buildBytes(cmd, byteStream);
      buildBytes(length=2, byteStream);
      buildBytes(data.gp_out, byteStream);
      break;
    }
    case SetController:
      buildBytes(cmd, byteStream);
      buildBytes(length=13, byteStream);
      buildBytes(data.type, byteStream);
      buildBytes(data.p_gain, byteStream);
      buildBytes(data.i_gain, byteStream);
      buildBytes(data.d_gain, byteStream);
      break;
    case GetController:
      buildBytes(cmd, byteStream);
      buildBytes(length=1, byteStream);
      buildBytes(data.reserved, byteStream);
      break;
    default:
      return false;
      break;
  }
  return true;
}



} // namespace kobuki