command.cpp
Go to the documentation of this file.
1 
10 /*****************************************************************************
11 ** Includes
12 *****************************************************************************/
13 
14 #include "../../include/kobuki_driver/command.hpp"
15 
16 /*****************************************************************************
17 ** Namespaces
18 *****************************************************************************/
19 
20 namespace kobuki {
21 
22 /*****************************************************************************
23 ** Static variables initialization
24 *****************************************************************************/
25 
26 const unsigned char Command::header0 = 0xaa;
27 const unsigned char Command::header1 = 0x55;
28 
29 /*****************************************************************************
30 ** Implementation [Command Generators]
31 *****************************************************************************/
32 
53 Command Command::SetLedArray(const enum LedNumber &number, const enum LedColour &colour, Command::Data &current_data)
54 {
55  // gp_out is 16 bits
56  uint16_t value;
57  if (number == Led1)
58  {
59  value = colour; // defined with the correct bit specification.
60  current_data.gp_out = (current_data.gp_out & 0xfcff) | value; // update first
61  }
62  else
63  {
64  value = colour << 2;
65  current_data.gp_out = (current_data.gp_out & 0xf3ff) | value; // update first
66  }
67  Command outgoing;
68  outgoing.data = current_data;
70  return outgoing;
71 }
72 
84 Command Command::SetDigitalOutput(const DigitalOutput &digital_output, Command::Data &current_data)
85 {
86  uint16_t values = 0x0000;
87  uint16_t clear_mask = 0xfff0;
88  for ( unsigned int i = 0; i < 4; ++i ) {
89  if ( digital_output.mask[i] ) {
90  if ( digital_output.values[i] ) {
91  values |= ( 1 << i );
92  }
93  } else {
94  clear_mask |= ( 1 << i ); // don't clear this bit, so set a 1 here
95  }
96  }
97  current_data.gp_out = (current_data.gp_out & clear_mask) | values;
98  Command outgoing;
99  outgoing.data = current_data;
101  return outgoing;
102 }
103 
115 Command Command::SetExternalPower(const DigitalOutput &digital_output, Command::Data &current_data)
116 {
117  uint16_t values = 0x0000;
118  uint16_t clear_mask = 0xff0f;
119  for ( unsigned int i = 0; i < 4; ++i ) {
120  if ( digital_output.mask[i] ) {
121  if ( digital_output.values[i] ) {
122  values |= ( 1 << (i+4) );
123  }
124  } else {
125  clear_mask |= ( 1 << (i+4) ); // don't clear this bit, so set a 1 here
126  }
127  }
128  current_data.gp_out = (current_data.gp_out & clear_mask) | values;
129  Command outgoing;
130  outgoing.data = current_data;
132  return outgoing;
133 }
134 
136 {
137  uint16_t value; // gp_out is 16 bits
138  value = number; // defined with the correct bit specification.
139 
140  Command outgoing;
141  outgoing.data.segment_name = value;
143  return outgoing;
144 }
145 
147 {
148  Command outgoing;
149  outgoing.data.request_flags = 0;
150  outgoing.data.request_flags |= static_cast<uint16_t>(HardwareVersion);
151  outgoing.data.request_flags |= static_cast<uint16_t>(FirmwareVersion);
152  outgoing.data.request_flags |= static_cast<uint16_t>(UniqueDeviceID);
154  return outgoing;
155 }
156 
158 {
159  Command outgoing;
160  std::vector<short> velocity_commands = diff_drive.velocityCommands();
161  outgoing.data.speed = velocity_commands[0];
162  outgoing.data.radius = velocity_commands[1];
163  outgoing.data.command = Command::BaseControl;
164  return outgoing;
165 }
166 
167 Command Command::SetVelocityControl(const int16_t &speed, const int16_t &radius)
168 {
169  Command outgoing;
170  outgoing.data.speed = speed;
171  outgoing.data.radius = radius;
172  outgoing.data.command = Command::BaseControl;
173  return outgoing;
174 }
175 
176 Command Command::SetControllerGain(const unsigned char &type, const unsigned int &p_gain,
177  const unsigned int &i_gain, const unsigned int &d_gain)
178 {
179  Command outgoing;
180  outgoing.data.type = type;
181  outgoing.data.p_gain = p_gain;
182  outgoing.data.i_gain = i_gain;
183  outgoing.data.d_gain = d_gain;
185  return outgoing;
186 }
187 
189 {
190  Command outgoing;
192  outgoing.data.reserved = 0;
193  return outgoing;
194 }
195 
196 /*****************************************************************************
197 ** Implementation [Serialisation]
198 *****************************************************************************/
203  buffer.clear();
204  buffer.resize(64);
205  buffer.push_back(Command::header0);
206  buffer.push_back(Command::header1);
207  buffer.push_back(0); // just initialise, we usually write in the payload here later (size of payload only, not stx, not etx, not length)
208 }
209 
210 bool Command::serialise(ecl::PushAndPop<unsigned char> & byteStream)
211 {
212  // need to be sure we don't pass through an emum to the Trans'd buildBytes functions.
213  unsigned char cmd = static_cast<unsigned char>(data.command);
214  unsigned char length = 0;
215  switch (data.command)
216  {
217  case BaseControl:
218  buildBytes(cmd, byteStream);
219  buildBytes(length=4, byteStream);
220  buildBytes(data.speed, byteStream);
221  buildBytes(data.radius, byteStream);
222  break;
223  case Sound:
224  buildBytes(cmd, byteStream);
225  buildBytes(length=3, byteStream);
226  buildBytes(data.note, byteStream);
227  buildBytes(data.duration, byteStream);
228  break;
229  case SoundSequence:
230  buildBytes(cmd, byteStream);
231  buildBytes(length=1, byteStream);
232  buildBytes(data.segment_name, byteStream);
233  break;
234  case RequestExtra:
235  buildBytes(cmd, byteStream);
236  buildBytes(length=2, byteStream);
237  buildBytes(data.request_flags, byteStream);
238  break;
239  case ChangeFrame:
240  buildBytes(cmd, byteStream);
241  buildBytes(length=1, byteStream);
242  buildBytes(data.frame_id, byteStream);
243  break;
244  case RequestEeprom:
245  buildBytes(cmd, byteStream);
246  buildBytes(length=1, byteStream);
247  buildBytes(data.frame_id, byteStream);
248  break;
249  case SetDigitalOut:
250  { // this one controls led, external power sources, gp digitial output
251  buildBytes(cmd, byteStream);
252  buildBytes(length=2, byteStream);
253  buildBytes(data.gp_out, byteStream);
254  break;
255  }
256  case SetController:
257  buildBytes(cmd, byteStream);
258  buildBytes(length=13, byteStream);
259  buildBytes(data.type, byteStream);
260  buildBytes(data.p_gain, byteStream);
261  buildBytes(data.i_gain, byteStream);
262  buildBytes(data.d_gain, byteStream);
263  break;
264  case GetController:
265  buildBytes(cmd, byteStream);
266  buildBytes(length=1, byteStream);
267  buildBytes(data.reserved, byteStream);
268  break;
269  default:
270  return false;
271  break;
272  }
273  return true;
274 }
275 
276 
277 
278 } // namespace kobuki
SoundSequences
Definition: sound.hpp:30
unsigned int p_gain
Definition: command.hpp:103
static Command SetControllerGain(const unsigned char &type, const unsigned int &p_gain, const unsigned int &i_gain, const unsigned int &d_gain)
Definition: command.cpp:176
static const unsigned char header1
Definition: command.hpp:134
double radius
unsigned char segment_name
Definition: command.hpp:87
const unsigned char length
uint16_t request_flags
Definition: command.hpp:90
unsigned int i_gain
Definition: command.hpp:104
unsigned char frame_id
Definition: command.hpp:93
static Command SetLedArray(const enum LedNumber &number, const enum LedColour &colour, Command::Data &current_data)
Definition: command.cpp:53
unsigned int d_gain
Definition: command.hpp:105
double speed
unsigned char duration
Definition: command.hpp:82
static Command SetDigitalOutput(const DigitalOutput &digital_output, Command::Data &current_data)
Definition: command.cpp:84
void velocityCommands(const double &vx, const double &wz)
Definition: diff_drive.cpp:133
unsigned char reserved
Definition: command.hpp:108
static Command SetVelocityControl(DiffDrive &diff_drive)
Definition: command.cpp:157
unsigned char type
Definition: command.hpp:102
Data structure containing data for commands.
Definition: command.hpp:66
void resetBuffer(Buffer &buffer)
Definition: command.cpp:202
static Command GetVersionInfo()
Definition: command.cpp:146
bool serialise(ecl::PushAndPop< unsigned char > &byteStream)
Definition: command.cpp:210
void buildBytes(const T &V, ecl::PushAndPop< unsigned char > &buffer)
static Command PlaySoundSequence(const enum SoundSequences &number, Command::Data &current_data)
Definition: command.cpp:135
static Command GetControllerGain()
Definition: command.cpp:188
static const unsigned char header0
Definition: command.hpp:133
ecl::PushAndPop< unsigned char > Buffer
Definition: command.hpp:37
static Command SetExternalPower(const DigitalOutput &digital_output, Command::Data &current_data)
Definition: command.cpp:115


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Mon Mar 27 2017 01:02:59