Fawkes API  Fawkes Development Version
RobotinoSensorInterface.cpp
00001 
00002 /***************************************************************************
00003  *  RobotinoSensorInterface.cpp - Fawkes BlackBoard Interface - RobotinoSensorInterface
00004  *
00005  *  Templated created:   Thu Oct 12 10:49:19 2006
00006  *  Copyright  2012  Tim Niemueller
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 #include <interfaces/RobotinoSensorInterface.h>
00025 
00026 #include <core/exceptions/software.h>
00027 
00028 #include <cstring>
00029 #include <cstdlib>
00030 
00031 namespace fawkes {
00032 
00033 /** @class RobotinoSensorInterface <interfaces/RobotinoSensorInterface.h>
00034  * RobotinoSensorInterface Fawkes BlackBoard Interface.
00035  * Sensor information of a Robotino robot
00036  * @ingroup FawkesInterfaces
00037  */
00038 
00039 
00040 
00041 /** Constructor */
00042 RobotinoSensorInterface::RobotinoSensorInterface() : Interface()
00043 {
00044   data_size = sizeof(RobotinoSensorInterface_data_t);
00045   data_ptr  = malloc(data_size);
00046   data      = (RobotinoSensorInterface_data_t *)data_ptr;
00047   data_ts   = (interface_data_ts_t *)data_ptr;
00048   memset(data_ptr, 0, data_size);
00049   add_fieldinfo(IFT_FLOAT, "mot_velocity", 3, &data->mot_velocity);
00050   add_fieldinfo(IFT_INT32, "mot_position", 3, &data->mot_position);
00051   add_fieldinfo(IFT_FLOAT, "mot_current", 3, &data->mot_current);
00052   add_fieldinfo(IFT_BOOL, "bumper", 1, &data->bumper);
00053   add_fieldinfo(IFT_FLOAT, "distance", 9, &data->distance);
00054   add_fieldinfo(IFT_BOOL, "digital_in", 8, &data->digital_in);
00055   add_fieldinfo(IFT_FLOAT, "analog_in", 8, &data->analog_in);
00056   add_fieldinfo(IFT_BOOL, "gyro_available", 1, &data->gyro_available);
00057   add_fieldinfo(IFT_FLOAT, "gyro_angle", 1, &data->gyro_angle);
00058   add_fieldinfo(IFT_FLOAT, "gyro_rate", 1, &data->gyro_rate);
00059   unsigned char tmp_hash[] = {0xfe, 0x8a, 0xcf, 0x8f, 0xe8, 0xb9, 0xf, 0x3b, 0x35, 0x3b, 0x1a, 0xea, 0xe7, 0x59, 0xab, 0xc8};
00060   set_hash(tmp_hash);
00061 }
00062 
00063 /** Destructor */
00064 RobotinoSensorInterface::~RobotinoSensorInterface()
00065 {
00066   free(data_ptr);
00067 }
00068 /* Methods */
00069 /** Get mot_velocity value.
00070  * Velocities of the wheels.
00071  * @return mot_velocity value
00072  */
00073 float *
00074 RobotinoSensorInterface::mot_velocity() const
00075 {
00076   return data->mot_velocity;
00077 }
00078 
00079 /** Get mot_velocity value at given index.
00080  * Velocities of the wheels.
00081  * @param index index of value
00082  * @return mot_velocity value
00083  * @exception Exception thrown if index is out of bounds
00084  */
00085 float
00086 RobotinoSensorInterface::mot_velocity(unsigned int index) const
00087 {
00088   if (index > 3) {
00089     throw Exception("Index value %u out of bounds (0..3)", index);
00090   }
00091   return data->mot_velocity[index];
00092 }
00093 
00094 /** Get maximum length of mot_velocity value.
00095  * @return length of mot_velocity value, can be length of the array or number of 
00096  * maximum number of characters for a string
00097  */
00098 size_t
00099 RobotinoSensorInterface::maxlenof_mot_velocity() const
00100 {
00101   return 3;
00102 }
00103 
00104 /** Set mot_velocity value.
00105  * Velocities of the wheels.
00106  * @param new_mot_velocity new mot_velocity value
00107  */
00108 void
00109 RobotinoSensorInterface::set_mot_velocity(const float * new_mot_velocity)
00110 {
00111   memcpy(data->mot_velocity, new_mot_velocity, sizeof(float) * 3);
00112   data_changed = true;
00113 }
00114 
00115 /** Set mot_velocity value at given index.
00116  * Velocities of the wheels.
00117  * @param new_mot_velocity new mot_velocity value
00118  * @param index index for of the value
00119  */
00120 void
00121 RobotinoSensorInterface::set_mot_velocity(unsigned int index, const float new_mot_velocity)
00122 {
00123   if (index > 3) {
00124     throw Exception("Index value %u out of bounds (0..3)", index);
00125   }
00126   data->mot_velocity[index] = new_mot_velocity;
00127   data_changed = true;
00128 }
00129 /** Get mot_position value.
00130  * Positions of the wheels.
00131  * @return mot_position value
00132  */
00133 int32_t *
00134 RobotinoSensorInterface::mot_position() const
00135 {
00136   return data->mot_position;
00137 }
00138 
00139 /** Get mot_position value at given index.
00140  * Positions of the wheels.
00141  * @param index index of value
00142  * @return mot_position value
00143  * @exception Exception thrown if index is out of bounds
00144  */
00145 int32_t
00146 RobotinoSensorInterface::mot_position(unsigned int index) const
00147 {
00148   if (index > 3) {
00149     throw Exception("Index value %u out of bounds (0..3)", index);
00150   }
00151   return data->mot_position[index];
00152 }
00153 
00154 /** Get maximum length of mot_position value.
00155  * @return length of mot_position value, can be length of the array or number of 
00156  * maximum number of characters for a string
00157  */
00158 size_t
00159 RobotinoSensorInterface::maxlenof_mot_position() const
00160 {
00161   return 3;
00162 }
00163 
00164 /** Set mot_position value.
00165  * Positions of the wheels.
00166  * @param new_mot_position new mot_position value
00167  */
00168 void
00169 RobotinoSensorInterface::set_mot_position(const int32_t * new_mot_position)
00170 {
00171   memcpy(data->mot_position, new_mot_position, sizeof(int32_t) * 3);
00172   data_changed = true;
00173 }
00174 
00175 /** Set mot_position value at given index.
00176  * Positions of the wheels.
00177  * @param new_mot_position new mot_position value
00178  * @param index index for of the value
00179  */
00180 void
00181 RobotinoSensorInterface::set_mot_position(unsigned int index, const int32_t new_mot_position)
00182 {
00183   if (index > 3) {
00184     throw Exception("Index value %u out of bounds (0..3)", index);
00185   }
00186   data->mot_position[index] = new_mot_position;
00187   data_changed = true;
00188 }
00189 /** Get mot_current value.
00190  * Motor currents.
00191  * @return mot_current value
00192  */
00193 float *
00194 RobotinoSensorInterface::mot_current() const
00195 {
00196   return data->mot_current;
00197 }
00198 
00199 /** Get mot_current value at given index.
00200  * Motor currents.
00201  * @param index index of value
00202  * @return mot_current value
00203  * @exception Exception thrown if index is out of bounds
00204  */
00205 float
00206 RobotinoSensorInterface::mot_current(unsigned int index) const
00207 {
00208   if (index > 3) {
00209     throw Exception("Index value %u out of bounds (0..3)", index);
00210   }
00211   return data->mot_current[index];
00212 }
00213 
00214 /** Get maximum length of mot_current value.
00215  * @return length of mot_current value, can be length of the array or number of 
00216  * maximum number of characters for a string
00217  */
00218 size_t
00219 RobotinoSensorInterface::maxlenof_mot_current() const
00220 {
00221   return 3;
00222 }
00223 
00224 /** Set mot_current value.
00225  * Motor currents.
00226  * @param new_mot_current new mot_current value
00227  */
00228 void
00229 RobotinoSensorInterface::set_mot_current(const float * new_mot_current)
00230 {
00231   memcpy(data->mot_current, new_mot_current, sizeof(float) * 3);
00232   data_changed = true;
00233 }
00234 
00235 /** Set mot_current value at given index.
00236  * Motor currents.
00237  * @param new_mot_current new mot_current value
00238  * @param index index for of the value
00239  */
00240 void
00241 RobotinoSensorInterface::set_mot_current(unsigned int index, const float new_mot_current)
00242 {
00243   if (index > 3) {
00244     throw Exception("Index value %u out of bounds (0..3)", index);
00245   }
00246   data->mot_current[index] = new_mot_current;
00247   data_changed = true;
00248 }
00249 /** Get bumper value.
00250  * Bumper pressed indicator.
00251  * @return bumper value
00252  */
00253 bool
00254 RobotinoSensorInterface::is_bumper() const
00255 {
00256   return data->bumper;
00257 }
00258 
00259 /** Get maximum length of bumper value.
00260  * @return length of bumper value, can be length of the array or number of 
00261  * maximum number of characters for a string
00262  */
00263 size_t
00264 RobotinoSensorInterface::maxlenof_bumper() const
00265 {
00266   return 1;
00267 }
00268 
00269 /** Set bumper value.
00270  * Bumper pressed indicator.
00271  * @param new_bumper new bumper value
00272  */
00273 void
00274 RobotinoSensorInterface::set_bumper(const bool new_bumper)
00275 {
00276   data->bumper = new_bumper;
00277   data_changed = true;
00278 }
00279 
00280 /** Get distance value.
00281  * Distance sensor values.
00282  * @return distance value
00283  */
00284 float *
00285 RobotinoSensorInterface::distance() const
00286 {
00287   return data->distance;
00288 }
00289 
00290 /** Get distance value at given index.
00291  * Distance sensor values.
00292  * @param index index of value
00293  * @return distance value
00294  * @exception Exception thrown if index is out of bounds
00295  */
00296 float
00297 RobotinoSensorInterface::distance(unsigned int index) const
00298 {
00299   if (index > 9) {
00300     throw Exception("Index value %u out of bounds (0..9)", index);
00301   }
00302   return data->distance[index];
00303 }
00304 
00305 /** Get maximum length of distance value.
00306  * @return length of distance value, can be length of the array or number of 
00307  * maximum number of characters for a string
00308  */
00309 size_t
00310 RobotinoSensorInterface::maxlenof_distance() const
00311 {
00312   return 9;
00313 }
00314 
00315 /** Set distance value.
00316  * Distance sensor values.
00317  * @param new_distance new distance value
00318  */
00319 void
00320 RobotinoSensorInterface::set_distance(const float * new_distance)
00321 {
00322   memcpy(data->distance, new_distance, sizeof(float) * 9);
00323   data_changed = true;
00324 }
00325 
00326 /** Set distance value at given index.
00327  * Distance sensor values.
00328  * @param new_distance new distance value
00329  * @param index index for of the value
00330  */
00331 void
00332 RobotinoSensorInterface::set_distance(unsigned int index, const float new_distance)
00333 {
00334   if (index > 9) {
00335     throw Exception("Index value %u out of bounds (0..9)", index);
00336   }
00337   data->distance[index] = new_distance;
00338   data_changed = true;
00339 }
00340 /** Get digital_in value.
00341  * Digital input values.
00342  * @return digital_in value
00343  */
00344 bool *
00345 RobotinoSensorInterface::is_digital_in() const
00346 {
00347   return data->digital_in;
00348 }
00349 
00350 /** Get digital_in value at given index.
00351  * Digital input values.
00352  * @param index index of value
00353  * @return digital_in value
00354  * @exception Exception thrown if index is out of bounds
00355  */
00356 bool
00357 RobotinoSensorInterface::is_digital_in(unsigned int index) const
00358 {
00359   if (index > 8) {
00360     throw Exception("Index value %u out of bounds (0..8)", index);
00361   }
00362   return data->digital_in[index];
00363 }
00364 
00365 /** Get maximum length of digital_in value.
00366  * @return length of digital_in value, can be length of the array or number of 
00367  * maximum number of characters for a string
00368  */
00369 size_t
00370 RobotinoSensorInterface::maxlenof_digital_in() const
00371 {
00372   return 8;
00373 }
00374 
00375 /** Set digital_in value.
00376  * Digital input values.
00377  * @param new_digital_in new digital_in value
00378  */
00379 void
00380 RobotinoSensorInterface::set_digital_in(const bool * new_digital_in)
00381 {
00382   memcpy(data->digital_in, new_digital_in, sizeof(bool) * 8);
00383   data_changed = true;
00384 }
00385 
00386 /** Set digital_in value at given index.
00387  * Digital input values.
00388  * @param new_digital_in new digital_in value
00389  * @param index index for of the value
00390  */
00391 void
00392 RobotinoSensorInterface::set_digital_in(unsigned int index, const bool new_digital_in)
00393 {
00394   if (index > 8) {
00395     throw Exception("Index value %u out of bounds (0..8)", index);
00396   }
00397   data->digital_in[index] = new_digital_in;
00398   data_changed = true;
00399 }
00400 /** Get analog_in value.
00401  * Analog input values.
00402  * @return analog_in value
00403  */
00404 float *
00405 RobotinoSensorInterface::analog_in() const
00406 {
00407   return data->analog_in;
00408 }
00409 
00410 /** Get analog_in value at given index.
00411  * Analog input values.
00412  * @param index index of value
00413  * @return analog_in value
00414  * @exception Exception thrown if index is out of bounds
00415  */
00416 float
00417 RobotinoSensorInterface::analog_in(unsigned int index) const
00418 {
00419   if (index > 8) {
00420     throw Exception("Index value %u out of bounds (0..8)", index);
00421   }
00422   return data->analog_in[index];
00423 }
00424 
00425 /** Get maximum length of analog_in value.
00426  * @return length of analog_in value, can be length of the array or number of 
00427  * maximum number of characters for a string
00428  */
00429 size_t
00430 RobotinoSensorInterface::maxlenof_analog_in() const
00431 {
00432   return 8;
00433 }
00434 
00435 /** Set analog_in value.
00436  * Analog input values.
00437  * @param new_analog_in new analog_in value
00438  */
00439 void
00440 RobotinoSensorInterface::set_analog_in(const float * new_analog_in)
00441 {
00442   memcpy(data->analog_in, new_analog_in, sizeof(float) * 8);
00443   data_changed = true;
00444 }
00445 
00446 /** Set analog_in value at given index.
00447  * Analog input values.
00448  * @param new_analog_in new analog_in value
00449  * @param index index for of the value
00450  */
00451 void
00452 RobotinoSensorInterface::set_analog_in(unsigned int index, const float new_analog_in)
00453 {
00454   if (index > 8) {
00455     throw Exception("Index value %u out of bounds (0..8)", index);
00456   }
00457   data->analog_in[index] = new_analog_in;
00458   data_changed = true;
00459 }
00460 /** Get gyro_available value.
00461  * True if gyro is available
00462  * @return gyro_available value
00463  */
00464 bool
00465 RobotinoSensorInterface::is_gyro_available() const
00466 {
00467   return data->gyro_available;
00468 }
00469 
00470 /** Get maximum length of gyro_available value.
00471  * @return length of gyro_available value, can be length of the array or number of 
00472  * maximum number of characters for a string
00473  */
00474 size_t
00475 RobotinoSensorInterface::maxlenof_gyro_available() const
00476 {
00477   return 1;
00478 }
00479 
00480 /** Set gyro_available value.
00481  * True if gyro is available
00482  * @param new_gyro_available new gyro_available value
00483  */
00484 void
00485 RobotinoSensorInterface::set_gyro_available(const bool new_gyro_available)
00486 {
00487   data->gyro_available = new_gyro_available;
00488   data_changed = true;
00489 }
00490 
00491 /** Get gyro_angle value.
00492  * Gyro angle value; rad
00493  * @return gyro_angle value
00494  */
00495 float
00496 RobotinoSensorInterface::gyro_angle() const
00497 {
00498   return data->gyro_angle;
00499 }
00500 
00501 /** Get maximum length of gyro_angle value.
00502  * @return length of gyro_angle value, can be length of the array or number of 
00503  * maximum number of characters for a string
00504  */
00505 size_t
00506 RobotinoSensorInterface::maxlenof_gyro_angle() const
00507 {
00508   return 1;
00509 }
00510 
00511 /** Set gyro_angle value.
00512  * Gyro angle value; rad
00513  * @param new_gyro_angle new gyro_angle value
00514  */
00515 void
00516 RobotinoSensorInterface::set_gyro_angle(const float new_gyro_angle)
00517 {
00518   data->gyro_angle = new_gyro_angle;
00519   data_changed = true;
00520 }
00521 
00522 /** Get gyro_rate value.
00523  * Gyro rate value; rad/sec
00524  * @return gyro_rate value
00525  */
00526 float
00527 RobotinoSensorInterface::gyro_rate() const
00528 {
00529   return data->gyro_rate;
00530 }
00531 
00532 /** Get maximum length of gyro_rate value.
00533  * @return length of gyro_rate value, can be length of the array or number of 
00534  * maximum number of characters for a string
00535  */
00536 size_t
00537 RobotinoSensorInterface::maxlenof_gyro_rate() const
00538 {
00539   return 1;
00540 }
00541 
00542 /** Set gyro_rate value.
00543  * Gyro rate value; rad/sec
00544  * @param new_gyro_rate new gyro_rate value
00545  */
00546 void
00547 RobotinoSensorInterface::set_gyro_rate(const float new_gyro_rate)
00548 {
00549   data->gyro_rate = new_gyro_rate;
00550   data_changed = true;
00551 }
00552 
00553 /* =========== message create =========== */
00554 Message *
00555 RobotinoSensorInterface::create_message(const char *type) const
00556 {
00557   throw UnknownTypeException("The given type '%s' does not match any known "
00558                              "message type for this interface type.", type);
00559 }
00560 
00561 
00562 /** Copy values from other interface.
00563  * @param other other interface to copy values from
00564  */
00565 void
00566 RobotinoSensorInterface::copy_values(const Interface *other)
00567 {
00568   const RobotinoSensorInterface *oi = dynamic_cast<const RobotinoSensorInterface *>(other);
00569   if (oi == NULL) {
00570     throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
00571                                 type(), other->type());
00572   }
00573   memcpy(data, oi->data, sizeof(RobotinoSensorInterface_data_t));
00574 }
00575 
00576 const char *
00577 RobotinoSensorInterface::enum_tostring(const char *enumtype, int val) const
00578 {
00579   throw UnknownTypeException("Unknown enum type %s", enumtype);
00580 }
00581 
00582 /* =========== messages =========== */
00583 /** Check if message is valid and can be enqueued.
00584  * @param message Message to check
00585  * @return true if the message is valid, false otherwise.
00586  */
00587 bool
00588 RobotinoSensorInterface::message_valid(const Message *message) const
00589 {
00590   return false;
00591 }
00592 
00593 /// @cond INTERNALS
00594 EXPORT_INTERFACE(RobotinoSensorInterface)
00595 /// @endcond
00596 
00597 
00598 } // end namespace fawkes