command.cpp
Go to the documentation of this file.
00001 
00010 /*****************************************************************************
00011 ** Includes
00012 *****************************************************************************/
00013 
00014 #include "../../include/kobuki_driver/command.hpp"
00015 
00016 /*****************************************************************************
00017 ** Namespaces
00018 *****************************************************************************/
00019 
00020 namespace kobuki {
00021 
00022 /*****************************************************************************
00023 ** Static variables initialization
00024 *****************************************************************************/
00025 
00026 const unsigned char Command::header0 = 0xaa;
00027 const unsigned char Command::header1 = 0x55;
00028 
00029 /*****************************************************************************
00030 ** Implementation [Command Generators]
00031 *****************************************************************************/
00032 
00053 Command Command::SetLedArray(const enum LedNumber &number, const enum LedColour &colour, Command::Data &current_data)
00054 {
00055   // gp_out is 16 bits
00056   uint16_t value;
00057   if (number == Led1)
00058   {
00059     value = colour; // defined with the correct bit specification.
00060     current_data.gp_out = (current_data.gp_out & 0xfcff) | value; // update first
00061   }
00062   else
00063   {
00064     value = colour << 2;
00065     current_data.gp_out = (current_data.gp_out & 0xf3ff) | value; // update first
00066   }
00067   Command outgoing;
00068   outgoing.data = current_data;
00069   outgoing.data.command = Command::SetDigitalOut;
00070   return outgoing;
00071 }
00072 
00084 Command Command::SetDigitalOutput(const DigitalOutput &digital_output, Command::Data &current_data)
00085 {
00086   uint16_t values = 0x0000;
00087   uint16_t clear_mask = 0xfff0;
00088   for ( unsigned int i = 0; i < 4; ++i ) {
00089     if ( digital_output.mask[i] ) {
00090       if ( digital_output.values[i] ) {
00091         values |= ( 1 << i );
00092       }
00093     } else {
00094       clear_mask |= ( 1 << i ); // don't clear this bit, so set a 1 here
00095     }
00096   }
00097   current_data.gp_out = (current_data.gp_out & clear_mask) | values;
00098   Command outgoing;
00099   outgoing.data = current_data;
00100   outgoing.data.command = Command::SetDigitalOut;
00101   return outgoing;
00102 }
00103 
00115 Command Command::SetExternalPower(const DigitalOutput &digital_output, Command::Data &current_data)
00116 {
00117   uint16_t values = 0x0000;
00118   uint16_t clear_mask = 0xff0f;
00119   for ( unsigned int i = 0; i < 4; ++i ) {
00120     if ( digital_output.mask[i] ) {
00121       if ( digital_output.values[i] ) {
00122         values |= ( 1 << (i+4) );
00123       }
00124     } else {
00125       clear_mask |= ( 1 << (i+4) ); // don't clear this bit, so set a 1 here
00126     }
00127   }
00128   current_data.gp_out = (current_data.gp_out & clear_mask) | values;
00129   Command outgoing;
00130   outgoing.data = current_data;
00131   outgoing.data.command = Command::SetDigitalOut;
00132   return outgoing;
00133 }
00134 
00135 Command Command::PlaySoundSequence(const enum SoundSequences &number, Command::Data &current_data)
00136 {
00137   uint16_t value; // gp_out is 16 bits
00138   value = number; // defined with the correct bit specification.
00139 
00140   Command outgoing;
00141   outgoing.data.segment_name = value;
00142   outgoing.data.command = Command::SoundSequence;
00143   return outgoing;
00144 }
00145 
00146 Command Command::GetVersionInfo()
00147 {
00148   Command outgoing;
00149   outgoing.data.request_flags = 0;
00150   outgoing.data.request_flags |= static_cast<uint16_t>(HardwareVersion);
00151   outgoing.data.request_flags |= static_cast<uint16_t>(FirmwareVersion);
00152   outgoing.data.request_flags |= static_cast<uint16_t>(UniqueDeviceID);
00153   outgoing.data.command = Command::RequestExtra;
00154   return outgoing;
00155 }
00156 
00157 Command Command::SetVelocityControl(DiffDrive& diff_drive)
00158 {
00159   Command outgoing;
00160   std::vector<short> velocity_commands = diff_drive.velocityCommands();
00161   outgoing.data.speed = velocity_commands[0];
00162   outgoing.data.radius = velocity_commands[1];
00163   outgoing.data.command = Command::BaseControl;
00164   return outgoing;
00165 }
00166 
00167 Command Command::SetVelocityControl(const int16_t &speed, const int16_t &radius)
00168 {
00169   Command outgoing;
00170   outgoing.data.speed = speed;
00171   outgoing.data.radius = radius;
00172   outgoing.data.command = Command::BaseControl;
00173   return outgoing;
00174 }
00175 
00176 Command Command::SetControllerGain(const unsigned char &type, const unsigned int &p_gain,
00177                                    const unsigned int &i_gain, const unsigned int &d_gain)
00178 {
00179   Command outgoing;
00180   outgoing.data.type = type;
00181   outgoing.data.p_gain = p_gain;
00182   outgoing.data.i_gain = i_gain;
00183   outgoing.data.d_gain = d_gain;
00184   outgoing.data.command = Command::SetController;
00185   return outgoing;
00186 }
00187 
00188 Command Command::GetControllerGain()
00189 {
00190   Command outgoing;
00191   outgoing.data.command = Command::GetController;
00192   outgoing.data.reserved = 0;
00193   return outgoing;
00194 }
00195 
00196 /*****************************************************************************
00197 ** Implementation [Serialisation]
00198 *****************************************************************************/
00202 void Command::resetBuffer(Buffer& buffer) {
00203   buffer.clear();
00204   buffer.resize(64);
00205   buffer.push_back(Command::header0);
00206   buffer.push_back(Command::header1);
00207   buffer.push_back(0); // just initialise, we usually write in the payload here later (size of payload only, not stx, not etx, not length)
00208 }
00209 
00210 bool Command::serialise(ecl::PushAndPop<unsigned char> & byteStream)
00211 {
00212   // need to be sure we don't pass through an emum to the Trans'd buildBytes functions.
00213   unsigned char cmd = static_cast<unsigned char>(data.command);
00214   unsigned char length = 0;
00215   switch (data.command)
00216   {
00217     case BaseControl:
00218       buildBytes(cmd, byteStream);
00219       buildBytes(length=4, byteStream);
00220       buildBytes(data.speed, byteStream);
00221       buildBytes(data.radius, byteStream);
00222       break;
00223     case Sound:
00224       buildBytes(cmd, byteStream);
00225       buildBytes(length=3, byteStream);
00226       buildBytes(data.note, byteStream);
00227       buildBytes(data.duration, byteStream);
00228       break;
00229     case SoundSequence:
00230       buildBytes(cmd, byteStream);
00231       buildBytes(length=1, byteStream);
00232       buildBytes(data.segment_name, byteStream);
00233       break;
00234     case RequestExtra:
00235       buildBytes(cmd, byteStream);
00236       buildBytes(length=2, byteStream);
00237       buildBytes(data.request_flags, byteStream);
00238       break;
00239     case ChangeFrame:
00240       buildBytes(cmd, byteStream);
00241       buildBytes(length=1, byteStream);
00242       buildBytes(data.frame_id, byteStream);
00243       break;
00244     case RequestEeprom:
00245       buildBytes(cmd, byteStream);
00246       buildBytes(length=1, byteStream);
00247       buildBytes(data.frame_id, byteStream);
00248       break;
00249     case SetDigitalOut:
00250     { // this one controls led, external power sources, gp digitial output
00251       buildBytes(cmd, byteStream);
00252       buildBytes(length=2, byteStream);
00253       buildBytes(data.gp_out, byteStream);
00254       break;
00255     }
00256     case SetController:
00257       buildBytes(cmd, byteStream);
00258       buildBytes(length=13, byteStream);
00259       buildBytes(data.type, byteStream);
00260       buildBytes(data.p_gain, byteStream);
00261       buildBytes(data.i_gain, byteStream);
00262       buildBytes(data.d_gain, byteStream);
00263       break;
00264     case GetController:
00265       buildBytes(cmd, byteStream);
00266       buildBytes(length=1, byteStream);
00267       buildBytes(data.reserved, byteStream);
00268       break;
00269     default:
00270       return false;
00271       break;
00272   }
00273   return true;
00274 }
00275 
00276 
00277 
00278 } // namespace kobuki


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Thu Jun 6 2019 20:24:37