/player-3.0.2/server/drivers/position/motionmind/motionmind.cc

  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