Fawkes API
Fawkes Development Version
|
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