/player-3.0.2/server/drivers/position/motionmind/motionmind.cc
C++ | 709 lines | 441 code | 72 blank | 196 comment | 36 complexity | 723aed24717f24e5866e8a50393bd1b3 MD5 | raw file
Possible License(s): GPL-2.0, LGPL-2.1, BSD-3-Clause
1/* 2 * Player - One Hell of a Robot Server 3 * Copyright (C) 2000 4 * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard 5 * 6 * 7 * This program is free software; you can redistribute it and/or modify 8 * it under the terms of the GNU General Public License as published by 9 * the Free Software Foundation; either version 2 of the License, or 10 * (at your option) any later version. 11 * 12 * This program is distributed in the hope that it will be useful, 13 * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 * GNU General Public License for more details. 16 * 17 * You should have received a copy of the GNU General Public License 18 * along with this program; if not, write to the Free Software 19 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 20 * 21 */ 22 23/* 24 * mmdriver.cpp 25 * motionminddriver 26 * 27 * Created by Chris Chambers on 31/03/08. 28 * 29 */ 30 31/** @ingroup drivers Drivers */ 32/** @{ */ 33/** @defgroup driver_motionmind motionmind 34 * @brief Solutions Cubed motor controller 35 36The motionmind driver is for communicating with a Solutions Cubed "Motion Mind" PID motor controller while it is in serial PID mode. 37Multiple boards can be daisy chained together and different drivers used for each one by simply giving each driver the address 38of the board that it is commanding. This driver allows for simple absolute position commands and publishes the position of the device as well. 39 40@par Compile-time dependencies 41 42- none 43 44@par Provides 45 46- @ref interface_position1d : publishes the position of the motor and allows for absolute position commands to be sent 47 48@par Requires 49 50- @ref interface_opaque : used to get the serial information form the motion mind control boards 51 52@par Configuration requests 53 54- none 55 56@par Supported commands 57 58- PLAYER_POSITION_1D_CMD_POS: The absolute position to send the actuator to 59- @todo : add support for PLAYER_POSITON_1D_CMD_VEL 60 61@par Configuration file options 62 63- address (int) 64 - Default 1 65 - Address of the motionmind board that you wish to control 66- cpr (int) 67 - Default 500 68 - counts per motor rotation 69- gear_ratio 70 - Default 1.0 71 - n:1 gear_ratio - robot position in m or rad:motor rotation 72 - divide the gear_ratio by 2*PI for rotational actuators 73 74@par Example 75 76@verbatim 77# Board number 1 78driver( 79 name "motionmind" 80 provides ["position1d:0"] 81 requires ["opaque:0"] 82 address 1 83) 84 85#Board Number 2 86driver( 87 name "motionmind" 88 provides ["position1d:1"] 89 requires ["opaque:0"] 90 address 2 91 cpr 500 92 gear_ratio 2.0 93) 94 95driver( 96 name "serialstream" 97 port "/dev/ttyS0" 98 transfer_rate 19200 99 parity "none" 100 provides ["opaque:0"] 101 alwayson 1 102 # IF ATTACHED TO MORE THAN ONE MOTIONMIND BOARD YOU MUST HAVE A WAIT TIME OR THINGS TIMEOUT 103 wait_time 40000 104) 105@endverbatim 106 107@author Chris Chambers 108 109*/ 110 111/** @} */ 112 113#if !defined (WIN32) 114 #include <unistd.h> 115#endif 116#include <string.h> 117#include <time.h> 118 119#include <libplayercore/playercore.h> 120 121#define DEFAULT_RX_BUFFER_SIZE 128 122#define DEFAULT_ADDRESS 1 123#define MESSAGE_LENGTH 7 124#define MM_WRITE_MESSAGE_LENGTH 8 125#define MM_MSG_WAIT 20000 //microseconds to wait before sending another command 126 // s/b 1250 according documentation but had comm errors below 20000 127#define MSG_TIMEOUT 250000 //microseconds before it sends another read request if it hasn't yet got a reply 128#define MM_DATA_WAIT 2500 //microseconds to wait before checking for response data 129#define MM_CPU_WAIT 10000 //microseconds to wait before checking for missing response data and to prevent CPU overloading 130#define MM_DEFAULT_CPR 500 131#define MM_DEFAULT_GEAR_RATIO 1.0 132#define MM_READ_POSITION 0x01 // Data0 133#define MM_READ_STATUS 0x01 // Data2 134#define MM_WRITE_REG 0x18 135 136// Status register bit packing 137#define MM_STATUS_NEGLIMIT 0x0001 138#define MM_STATUS_POSLIMIT 0x0002 139#define MM_STATUS_BRAKE 0x0004 140#define MM_STATUS_INDEX 0x0008 141#define MM_STATUS_BADRC 0x0010 142#define MM_STATUS_VNLIMIT 0x0020 143#define MM_STATUS_VPLIMIT 0x0040 144#define MM_STATUS_CURRENTLIMIT 0x0080 145#define MM_STATUS_PWMLIMIT 0x0100 146#define MM_STATUS_INPOSITION 0x0200 147 148// Register indexes for WRITE and WRITE STORE commands 149#define MM_REG_POSITION 0x00 150 151/////////////////////////////////////////////////////////////////////////////// 152// The class for the driver 153class MotionMind : public ThreadedDriver 154{ 155 public: 156 // Constructor; need that 157 MotionMind(ConfigFile* cf, int section); 158 ~MotionMind(); 159 160 // Must implement the following methods. 161 virtual int MainSetup(); 162 virtual void MainQuit(); 163 164 // This method will be invoked on each incoming message 165 virtual int ProcessMessage(QueuePointer & resp_queue, 166 player_msghdr* hdr, 167 void* data); 168 169 private: 170 171 // Main function for device thread. 172 virtual void Main(); 173 174 //Requests the pos of the robot if it hasnt been already 175 void FindCurrentPos(); 176 //Requests the status register of the robot if it hasn't been already 177 void FindCurrentStatus(); 178 179 // Checks whether or not you have sent a request for the position 180 bool pos_request_sent; 181 bool status_request_sent; 182 struct timeval msg_sent; 183 struct timeval time_sent_pos; 184 struct timeval time_sent_status; 185 186 int MsgWait(); 187 //Makes a command to be sent to the opaque driver 188 void makeAbsolutePositionCommand(uint8_t* buffer, unsigned int address, float position); 189 190 //Makes a command which requests the current osition bback from the motor ocntroller 191 void makeReadPositionCommand(uint8_t* buffer, unsigned int address); 192 193 //Makes a command which requests the current status back from the motor controller 194 void makeReadStatusCommand(uint8_t* buffer, unsigned int address); 195 196 //Makes a command which sets the odometry to the given value 197 void makeSetOdomReq(uint8_t* buffer, unsigned int address, const float position); 198 199 //Converts robot positions to absolute motionmind positions 200 int robot2abspos(const float position); 201 202 // The function to do stuff here 203 void DoStuff(); 204 205 // Opaque Driver info 206 Device *opaque; 207 player_devaddr_t opaque_id; 208 209 // The address of the board being addressed 210 uint8_t address; 211 212 // rx buffer 213 uint8_t * rx_buffer; 214 unsigned int rx_buffer_size; 215 unsigned int rx_count; 216 217 // player position1 data odometric pose, velocity and motor stall info 218 player_position1d_data_t pos_data; 219 220 // counts per rotation 221 int cpr; 222 223 // gear ratio robot:motor 224 double gear_ratio; 225}; 226 227 228 229//////////////////////////////////////////////////////////////////////////////// 230// Now the driver 231 232// A factory creation function, declared outside of the class so that it 233// can be invoked without any object context (alternatively, you can 234// declare it static in the class). In this function, we create and return 235// (as a generic Driver*) a pointer to a new instance of this driver. 236Driver* 237MotionMind_Init(ConfigFile* cf, int section) 238{ 239 // Create and return a new instance of this driver 240 return ((Driver*)(new MotionMind(cf, section))); 241 242} 243 244// A driver registration function, again declared outside of the class so 245// that it can be invoked without object context. In this function, we add 246// the driver into the given driver table, indicating which interface the 247// driver can support and how to create a driver instance. 248void motionmind_Register(DriverTable* table) 249{ 250 table->AddDriver("motionmind", MotionMind_Init); 251} 252 253//////////////////////////////////////////////////////////////////////////////// 254// Constructor. Retrieve options from the configuration file and do any 255// pre-Setup() setup. 256MotionMind::MotionMind(ConfigFile* cf, int section) 257 : ThreadedDriver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_POSITION1D_CODE) 258{ 259 this->opaque = NULL; 260 // Must have an opaque device 261 if (cf->ReadDeviceAddr(&this->opaque_id, section, "requires", 262 PLAYER_OPAQUE_CODE, -1, NULL) != 0) 263 { 264 puts ("No Opaque driver specified"); 265 this->SetError(-1); 266 return; 267 } 268 269 // Read options from the configuration file 270 address = cf->ReadInt(section, "address", DEFAULT_ADDRESS); 271 272 rx_count = 0; 273 rx_buffer_size = cf->ReadInt(section, "buffer_size", DEFAULT_RX_BUFFER_SIZE); 274 rx_buffer = new uint8_t[rx_buffer_size]; 275 assert(rx_buffer); 276 277 cpr = cf->ReadInt(section, "cpr", MM_DEFAULT_CPR); 278 279 this->gear_ratio = cf->ReadFloat(section, "gear_ratio", MM_DEFAULT_GEAR_RATIO); 280 if (this->gear_ratio == 0.0) fprintf (stderr,"gear_ratio cannot be 0.0: adjust your gear_ratio value"); 281 assert(this->gear_ratio != 0.0); 282 283 this->pos_request_sent = false; 284 this->status_request_sent = false; 285 GlobalTime->GetTime(&(this->msg_sent)); 286 return; 287} 288 289MotionMind::~MotionMind() 290{ 291 delete [] rx_buffer; 292} 293 294//////////////////////////////////////////////////////////////////////////////// 295// Set up the device. Return 0 if things go well, and -1 otherwise. 296int MotionMind::MainSetup() 297{ 298 puts("Setting up MotionMind driver"); 299 300 if(Device::MatchDeviceAddress(this->opaque_id, this->device_addr)) 301 { 302 PLAYER_ERROR("attempt to subscribe to self"); 303 return(-1); 304 } 305 306 if(!(this->opaque = deviceTable->GetDevice(this->opaque_id))) 307 { 308 PLAYER_ERROR("unable to locate suitable opaque device"); 309 return(-1); 310 } 311 312 if(this->opaque->Subscribe(this->InQueue) != 0) 313 { 314 PLAYER_ERROR("unable to subscribe to opaque device"); 315 return(-1); 316 } 317 318 puts("MotionMind driver ready"); 319 320 return(0); 321} 322 323 324//////////////////////////////////////////////////////////////////////////////// 325// Shutdown the device 326void MotionMind::MainQuit() 327{ 328 puts("MotionMind driver down"); 329 330 opaque->Unsubscribe(InQueue); 331 332 puts("MotionMind driver has been shutdown"); 333 334} 335 336 337 338// Process an incoming message 339int MotionMind::ProcessMessage(QueuePointer & resp_queue, 340 player_msghdr* hdr, 341 void* data) 342{ 343 assert(hdr); 344 assert(data); 345 346 if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_OPAQUE_DATA_STATE, opaque_id)) 347 { 348 player_opaque_data_t * recv = reinterpret_cast<player_opaque_data_t * > (data); 349 unsigned int messageOffset = rx_count; 350 rx_count += recv->data_count; 351 if (rx_count > rx_buffer_size) 352 { 353 PLAYER_WARN("MotionMind driver Buffer Full"); 354 rx_count = 0; 355 } 356 else 357 { 358 memcpy(&rx_buffer[messageOffset], recv->data, recv->data_count); 359 } 360 return 0; 361 } 362 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, 363 PLAYER_POSITION1D_CMD_POS, 364 this->device_addr)) 365 { 366 assert(hdr->size == sizeof(player_position1d_cmd_pos_t)); 367 player_position1d_cmd_pos_t * recv = reinterpret_cast<player_position1d_cmd_pos_t *> (data); 368 uint8_t * buffer = new uint8_t[MESSAGE_LENGTH]; 369 this->makeAbsolutePositionCommand(buffer, this->address, recv->pos); 370 player_opaque_data_t mData; 371 mData.data_count = MESSAGE_LENGTH; 372 mData.data = buffer; 373 this->MsgWait(); 374 opaque->PutMsg(this->InQueue, PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD_DATA, reinterpret_cast<void*>(&mData),0,NULL); 375 GlobalTime->GetTime(&(this->msg_sent)); 376 delete [] buffer; 377 return(0); 378 } 379 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, 380 PLAYER_POSITION1D_REQ_SET_ODOM, 381 this->device_addr)) 382 { 383 assert(hdr->size == sizeof(player_position1d_set_odom_req_t)); 384 player_position1d_set_odom_req_t* req = reinterpret_cast<player_position1d_set_odom_req_t*> (data); 385 uint8_t* buffer = new uint8_t[MM_WRITE_MESSAGE_LENGTH]; 386 this->makeSetOdomReq(buffer, this->address, req->pos); 387 player_opaque_data_t mData; 388 mData.data_count = MM_WRITE_MESSAGE_LENGTH; 389 mData.data = buffer; 390 this->MsgWait(); 391 opaque->PutMsg(this->InQueue, PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD_DATA, reinterpret_cast<void*>(&mData),0,NULL); 392 GlobalTime->GetTime(&(this->msg_sent)); 393 delete [] buffer; 394 return(0); 395 } 396 397 return -1; 398} 399 400//////////////////////////////////////////////////////////////////////////////// 401// Main function for device thread 402void MotionMind::Main() 403{ 404 405 // The main loop; interact with the device here 406 for(;;) 407 { 408 // test if we are supposed to cancel 409 pthread_testcancel(); 410 411 // Process incoming messages 412 ProcessMessages(); 413 414 // Ask for the current position 415 FindCurrentPos(); 416 // Ask for the current position status 417 FindCurrentStatus(); 418 // Publish position data 419 this->Publish(this->device_addr, PLAYER_MSGTYPE_DATA, PLAYER_POSITION1D_DATA_STATE, (void*)&pos_data); 420 421 usleep(MM_CPU_WAIT); 422 } 423 return; 424} 425 426// Makes the required serial packet to command a position or request the position 427void MotionMind::makeAbsolutePositionCommand(uint8_t* buffer, unsigned int address, float position) 428{ 429 unsigned int checksum = 0; 430 int posInt = robot2abspos(position); 431 char* pos = (char*)&posInt; 432 buffer[0] = 0x15; 433 checksum += buffer[0]; 434 buffer[1] = address; 435 checksum += buffer[1]; 436 buffer[2] = pos[0]; 437 checksum += buffer[2]; 438 buffer[3] = pos[1]; 439 checksum += buffer[3]; 440 buffer[4] = pos[2]; 441 checksum += buffer[4]; 442 buffer[5] = pos[3]; 443 checksum += buffer[5]; 444 buffer[6] = checksum; 445} 446 447void MotionMind::makeReadPositionCommand(uint8_t* buffer, unsigned int address) 448{ 449 unsigned int checksum = 0; 450 // Command 451 buffer[0] = 0x1A; 452 checksum += buffer[0]; 453 // Address 454 buffer[1] = address; 455 checksum += buffer[1]; 456 // Data0 457 buffer[2] = MM_READ_POSITION; 458 checksum += buffer[2]; 459 // Data1 460 buffer[3] = 0x00; 461 checksum += buffer[3]; 462 // Data2 463 buffer[4] = 0x00; 464 checksum += buffer[4]; 465 // Data3 466 buffer[5] = 0x00; 467 checksum += buffer[5]; 468 //Checksum 469 buffer[6] = checksum; 470} 471 472void MotionMind::makeReadStatusCommand(uint8_t* buffer, unsigned int address) 473{ 474 unsigned int checksum = 0; 475 // Command 476 buffer[0] = 0x1A; 477 checksum += buffer[0]; 478 // Address 479 buffer[1] = address; 480 checksum += buffer[1]; 481 // Data0 482 buffer[2] = 0x00; 483 checksum += buffer[2]; 484 // Data1 485 buffer[3] = 0x00; 486 checksum += buffer[3]; 487 // Data2 488 buffer[4] = MM_READ_STATUS; 489 checksum += buffer[4]; 490 // Data3 491 buffer[5] = 0x00; 492 checksum += buffer[5]; 493 //Checksum 494 buffer[6] = checksum; 495} 496 497void MotionMind::makeSetOdomReq(uint8_t* buffer, unsigned int address, float position) 498{ 499 unsigned int checksum = 0; 500 uint32_t posInt = robot2abspos(position); 501 printf("Setting position register to %0.6f : %d\n",position,posInt); 502 char* pos = (char*)&posInt; 503 // Command 504 buffer[0] = MM_WRITE_REG; 505 checksum += buffer[0]; 506 // Address 507 buffer[1] = address; 508 checksum += buffer[1]; 509 // Index 510 buffer[2] = MM_REG_POSITION; 511 checksum += buffer[2]; 512 // Data0 513 buffer[3] = pos[0]; 514 checksum += buffer[3]; 515 // Data1 516 buffer[4] = pos[1]; 517 checksum += buffer[4]; 518 // Data2 519 buffer[5] = pos[2]; 520 checksum += buffer[5]; 521 // Data3 522 buffer[6] = pos[3]; 523 checksum += buffer[6]; 524 //Checksum 525 buffer[7] = checksum; 526} 527 528int MotionMind::robot2abspos(const float position) 529{ 530 assert(this->gear_ratio != 0.0); 531 int abspos = static_cast<int> ((double)position*(double)this->cpr*this->gear_ratio); 532 return abspos; 533} 534 535// Currently this alternates between reading the pedal position and reading the steering position 536void MotionMind::FindCurrentPos() 537{ 538 long elapsed; 539 struct timeval curr; 540 if (!this->pos_request_sent) 541 { 542 uint8_t * buffer = new uint8_t[MESSAGE_LENGTH]; 543 makeReadPositionCommand(buffer, this->address); 544 player_opaque_data_t mData; 545 mData.data_count = MESSAGE_LENGTH; 546 mData.data = buffer; 547 this->MsgWait(); 548 opaque->PutMsg(this->InQueue, PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD_DATA, reinterpret_cast<void*>(&mData),0,NULL); 549 GlobalTime->GetTime(&(this->msg_sent)); 550 delete [] buffer; 551 this->pos_request_sent = true; 552 GlobalTime->GetTime(&(this->time_sent_pos)); 553 usleep(MM_DATA_WAIT); 554 } 555 while (this->pos_request_sent) 556 { 557 ProcessMessages(); 558 GlobalTime->GetTime(&curr); 559 elapsed = (curr.tv_sec - this->time_sent_pos.tv_sec)*1e6 + (curr.tv_usec - this->time_sent_pos.tv_usec); 560 if ((curr.tv_sec - this->time_sent_pos.tv_sec)*1e6 + (curr.tv_usec - this->time_sent_pos.tv_usec) > MSG_TIMEOUT) 561 { 562 printf("%d %ld mm pos request message timeout triggered\n",opaque_id.index,elapsed); 563 this->pos_request_sent = false; 564 break; 565 } 566 // Ensures that you are reading from the right point in the stream 567 while( (rx_count > 0) && (rx_buffer[0] != this->address)) 568 { 569 memmove(rx_buffer, rx_buffer+1, rx_count - 1); 570 rx_count--; 571 } 572 if (rx_count >= 6) 573 { 574 assert (rx_buffer[0] == this->address); 575 int position; 576 unsigned char* pos = (unsigned char*)&position; 577 unsigned int checksum = rx_buffer[0]; 578 unsigned char* csum = (unsigned char*)&checksum; 579 pos[0] = rx_buffer[1]; 580 checksum += rx_buffer[1]; 581 pos[1] = rx_buffer[2]; 582 checksum += rx_buffer[2]; 583 pos[2] = rx_buffer[3]; 584 checksum += rx_buffer[3]; 585 pos[3] = rx_buffer[4]; 586 checksum += rx_buffer[4]; 587 if (csum[0] != rx_buffer[5]) 588 { 589 // Checksums don't match - not necessarily bad as it can be due to the address header being in the middle of a responce 590 // to another driver with a different address e.g. the responce is 02 01 xx xx xx xx, the data being sent to device 2 591 // with the info 01 xx xx xx. 592 memmove(rx_buffer, rx_buffer+1, rx_count - 1); 593 rx_count--; 594 } 595 else 596 { 597 GlobalTime->GetTime(&curr); 598 float robot_pos = (float)((double)position/(this->gear_ratio*((double)cpr))); 599 this->pos_data.pos = robot_pos; 600 rx_count -= 6; 601 memmove(rx_buffer+6, rx_buffer, rx_count); 602 this->pos_request_sent = false; 603 } 604 } 605 else 606 { 607 // Prevents the CPU from becoming overloaded 608 usleep(MM_CPU_WAIT); 609 } 610 } 611 GlobalTime->GetTime(&curr); 612 elapsed = (curr.tv_sec - this->time_sent_pos.tv_sec)*1e6 + (curr.tv_usec - this->time_sent_pos.tv_usec); 613} 614 615void MotionMind::FindCurrentStatus() 616{ 617 long elapsed; 618 struct timeval curr; 619 if (!this->status_request_sent) 620 { 621 // printf("%d MotionMind FindCurrentStatus sending status request\n",opaque_id.index); 622 uint8_t * buffer = new uint8_t[MESSAGE_LENGTH]; 623 makeReadStatusCommand(buffer, this->address); 624 player_opaque_data_t mData; 625 mData.data_count = MESSAGE_LENGTH; 626 mData.data = buffer; 627 this->MsgWait(); 628 opaque->PutMsg(this->InQueue, PLAYER_MSGTYPE_CMD, PLAYER_OPAQUE_CMD_DATA, reinterpret_cast<void*>(&mData),0,NULL); 629 GlobalTime->GetTime(&(this->msg_sent)); 630 delete [] buffer; 631 this->status_request_sent = true; 632 GlobalTime->GetTime(&(this->time_sent_status)); 633 usleep(MM_DATA_WAIT); 634 } 635 while (this->status_request_sent) 636 { 637 ProcessMessages(); 638 GlobalTime->GetTime(&curr); 639 elapsed = (curr.tv_sec - this->time_sent_status.tv_sec)*1e6 + (curr.tv_usec - this->time_sent_status.tv_usec); 640 if ((curr.tv_sec - this->time_sent_status.tv_sec)*1e6 + (curr.tv_usec - this->time_sent_status.tv_usec) > MSG_TIMEOUT) 641 { 642 printf("%d %ld mm status message request timeout triggered\n",opaque_id.index,elapsed); 643 this->status_request_sent = false; 644 break; 645 } 646 // Ensures that you are reading from the right point in the stream 647 while( (rx_count > 0) && (rx_buffer[0] != this->address)) 648 { 649 memmove(rx_buffer, rx_buffer+1, rx_count - 1); 650 rx_count--; 651 } 652 if (rx_count >= 4) 653 { 654 assert (rx_buffer[0] == this->address); 655 int32_t status = 0; 656 unsigned char* stat = (unsigned char*)&status; 657 unsigned int checksum = rx_buffer[0]; 658 unsigned char* csum = (unsigned char*)&checksum; 659 stat[0] = rx_buffer[1]; 660 checksum += rx_buffer[1]; 661 stat[1] = rx_buffer[2]; 662 checksum += rx_buffer[2]; 663 if (csum[0] != rx_buffer[3]) 664 { 665 // Checksums don't match - not necessarily bad as it can be due to the address header being in the middle of a responce 666 // to another driver with a different address e.g. the responce is 02 01 xx xx xx xx, the data being sent to device 2 667 // with the info 01 xx xx xx. 668 memmove(rx_buffer, rx_buffer+1, rx_count - 1); 669 rx_count--; 670 } 671 else 672 { 673 this->pos_data.status = 674 (status & MM_STATUS_NEGLIMIT ? 0x01 : 0x00) // NEGLIMIT -> lim min 675 | (status & MM_STATUS_POSLIMIT ? 0x04 : 0x00) // POSLIMIT -> lim max 676 | (status & MM_STATUS_CURRENTLIMIT ? 0x08 : 0x00) // CURRENTLIMIT -> over current 677 | (status & MM_STATUS_INPOSITION ? 0x10 : 0x00) // INPOSITION -> trajectory complete 678 | (status & MM_STATUS_BRAKE ? 0x00 : 0x20); // BRAKE -> is enabled (inverted) 679 rx_count -= 4; 680 memmove(rx_buffer+4, rx_buffer, rx_count); 681 status_request_sent = false; 682 } 683 } 684 else 685 { 686 // Prevents the CPU from becoming overloaded 687 usleep(MM_CPU_WAIT); 688 } 689 } 690 elapsed = (curr.tv_sec - this->time_sent_status.tv_sec)*1e6 + (curr.tv_usec - this->time_sent_status.tv_usec); 691} 692 693int MotionMind::MsgWait() 694{ 695 struct timeval curr; 696 long elapsed; 697 GlobalTime->GetTime(&curr); 698 elapsed = (curr.tv_sec - this->msg_sent.tv_sec)*1e6 + (curr.tv_usec - this->msg_sent.tv_usec); 699 while (elapsed < MM_MSG_WAIT) 700 { 701 usleep(MM_MSG_WAIT-elapsed); 702 GlobalTime->GetTime(&curr); 703 elapsed = (curr.tv_sec - this->msg_sent.tv_sec)*1e6 + (curr.tv_usec - this->msg_sent.tv_usec); 704 } 705 return 0; 706} 707 708 709