Is Teensy Blocking during Serial communication?

laptophead · Nov 22, 2019

I am using a 3.5 to run a robotic arm, 6 points of inflection, controlled by 3 DC motor controller with Quadrature encoders.

It is essential to read the encoders all the time, so I know my XYZ and when a motion is completed...
So I use 3 Serial ports on the teensy to read these values every 20 mS , they all run at 115200.

Despite it power, the teensy is acting sluggish, is overshooting some commands , etc

So I pulled up the Scope to see what is going on.

On the red (top) I made an oscillator in the loop to see how long to refresh.

digitalWriteFast(35, !digitalReadFast(35)); // toggle

When there is no Serial talk, the loop is takes about 250 micro Sec. Not bad,

However , look at the yellow line.
During the ask - tell of the 6 encoders the loop stops for about 7 Mili Sec.

The commands look like:
BaseEncGlobal = roboclaw129.ReadEncM1(adr129, &status3, &valid3);
ShoEncGlobal = roboclaw128.ReadEncM2(adr128, &status2, &valid2);
ElbowEncGlobal = roboclaw129.ReadEncM2(adr129, &status4, &valid4);
GripEncGlobal = roboclaw128.ReadEncM1(adr128, &status1, &valid1);

VertRotEnc = roboclaw130.ReadEncM1(adr130, &status5, &valid5);
HorRotEnc = roboclaw130.ReadEncM2(adr130, &status6, &valid6);

they are part of the ROBOCLAW controller library.

So does that mean that I am stuck during the Serial Talk?
Things get even slower later. I am outputting about 20 numbers to a tablet that I use to see and input commands to the robot.
I am using processing for that and the AP Sync library.

ck it out if you have time
https://youtu.be/t_-1qefwpJQ

That resulted in a 75 mS blockage.... No wonder the arm goes berserk.

Is there a better way to handle my serial "needs"?

Thanks everyone,

Mitch

KurtE · Nov 22, 2019

It is hard to say what is going on.

I am not sure what Nathan does these days in that library.

Do you have a link to the library? Which RoboClaws? It has been a while since I pulled out one of mine I used earlier on a rover (or two).

But yes if the library code does something in functions like: ReadEncM1 that sends a command over the Serial port to the Roboclaw and then waits for a response, than it could take awhile especially at 115200...

From past experience with things like using RC servos over something like an SSC-32 or with the Roboclaw, doing things like a constant polling of the servo/motor for their current position and then tell it to stop or change, often did not give you the best results. Instead of possible it is best to tell the controller, when you get to position X, stop or the like.

But again might be able to give you better idea if I knew which library you are using and were to get it.
I know it has gone from Basic Micro -> Orion Robotics -> ION Control to I think -> Basic Micro ?

laptophead · Nov 22, 2019

Thanks for the reply,

Yes the roboclaw is now basic micro
https://www.basicmicro.com/downloads

Yes, I am sending commands to be finished by the controller, but I have to know when they are done to move to the next step.
For that I need encoder readings.

I guess I'll have to live with it. However I heard about wizardry that allows multithreading in a way that Serial Read Does not stop the show.

Thanks
Mitch

Elf · Nov 22, 2019

Have you tried a higher baud rate?

KurtE · Nov 22, 2019

There is lots of stuff involved here. Note: there are many up here who are probably a lot better to answer how best to do this setup with PID setup and the like.

The functions like: ReadEncM1, boil down to reading logical registers, which require writing out a command and waiting for a response.

Code:

uint32_t RoboClaw::ReadEncM1(uint8_t address, uint8_t *status,bool *valid){
	return Read4_1(address,GETM1ENC,status,valid);
}

Code:

uint32_t RoboClaw::Read4_1(uint8_t address, uint8_t cmd, uint8_t *status, bool *valid){
	uint8_t crc;

	if(valid)
		*valid = false;
	
	uint32_t value=0;
	uint8_t trys=MAXRETRY;
	int16_t data;
	do{
		flush();

		crc_clear();
		write(address);
		crc_update(address);
		write(cmd);
		crc_update(cmd);

		data = read(timeout);
		crc_update(data);
		value=(uint32_t)data<<24;

		if(data!=-1){
			data = read(timeout);
			crc_update(data);
			value|=(uint32_t)data<<16;
		}

		if(data!=-1){
			data = read(timeout);
			crc_update(data);
			value|=(uint32_t)data<<8;
		}

		if(data!=-1){
			data = read(timeout);
			crc_update(data);
			value|=(uint32_t)data;
		}
	
		if(data!=-1){
			data = read(timeout);
			crc_update(data);
			if(status)
				*status = data;
		}
				
		if(data!=-1){
			uint16_t ccrc;
			data = read(timeout);
			if(data!=-1){
				ccrc = data << 8;
				data = read(timeout);
				if(data!=-1){
					ccrc |= data;
					if(crc_get()==ccrc){
						if(valid)
							*valid = true;
						return value;
					}
				}
			}
		}
	}while(trys--);

	return false;
}

Couple of issues for you here. First off this is these commands are synchronous. That is you issue the command and then wait for the response.

So it outputs 2 bytes and then I believe try to retrieve 6 byes. Issues with this is:
a) It is synchronous. That is you won't continue, until you receive 6 bytes (4 bytes data 2 bytes CRC) . So assuming everything else you will have to wait for 2 bytes to output and I believe 4 btyes to be returned.
So Serial of 6 bytes. So if no overhead... then maybe about .7ms assuming best case no delays... You do this 6 times so min 4ms

b) wonder how well the timings are as it does multiple reads in a row without delays and the like, so timing may be such as it does not have the next byte available and then does a SerialX.read(), which returns -1, which it thinks is error and starts over...

c) Again having multiple Serial ports is wasted here as each of these calls waits to complete before the next one starts.

One could rewrite the library such that the requests are built into 2 halves. Output the command to read, and then another that sees if enough characters have been received , and extract the data...

So again some of the issues is how the library is setup. You could easily split reading registers like this in to two parts. Send command, and call to see if we have the response. With the 3 serial ports you could cut this time to be one third the time.

But again the drive a motor until encoder returns a value X, inplies by the time we receive it, we may have moved beyond our desired location.

To get around this, the controller may have other controls that may work better for you. Like the ability to set destination position and velocity to get there.

Although the last time I tried this (maybe 3-5 years ago), when you said stop a count X, this is maybe where it started to de-accelerate, so you had to figure out where to set the desitination such that actually stopped at the desired location...

Alternatively sometimes you simply had to guess. That is instead of asking are your there yet, instead you computed that the last command you issued should take time X to complete and after X time has elapsed you assume it completed. At least for RC servos this was more accurate than doing the query and then respond... Not sure if I explained this very well...

laptophead · Nov 23, 2019

Kurt,
Thanks for the detailed explanation.
I think this is as good as it gets. I can call the read every 10 ms and that means I refresh 100 times a sec.
I heard that a guided mislle refreshes the algorithm 2000 times a sec.

At 10 mS refresh 7 will be blocked for communication, and 3 will be left to compute.

You said,
To get around this, the controller may have other controls that may work better for you. Like the ability to set destination position and velocity to get there.

I do send this kind of commands already, but sometimes they overshoot the target and come back, sometimes they stop short and the PID moves them in slowly...

It is normal. When the Arm is extended the inertia of the rotation will overshoot, and the opposite is true when the arm is close to the 0 Axis.

I wonder how Kuka or Fanuc do it. The send various PID settings with the command based on a physics calculator that inputs the load, and other stuff...

Regarding the AP Sync library.
I replaced that with a long string to my tablet.
That reduced the transmission time, ( blocking) from 75mS to 15.
I send it at 57600 baud because it goes through a Bluetooth dongle.

I call that function every 300mS to refresh the tablet. It overlaps the read encoders at times, not good.

Can I do a Serial.print or Serial.write without blocking?

Thanks

PS
I will try a higher baud rate to Roboclaw.

One of the roboclaw controllers drops connectivity already, intermittently. That is another problem. Roboclaw support suggested pull up resistors on the Serial port. Does that make sense?

KurtE · Nov 23, 2019

Again it has been a long time since I played with the Roboclaw. At the time it was back in Lynxmotion/BasicMicro days and I did my own software to talk to the Roboclaw...

If it were me, the steps I would try, would be a set of hacks, to not have the roboclaw code block you...

That is, I would break up the functions called like:

Code:

uint32_t RoboClaw::ReadEncM1(uint8_t address, uint8_t *status,bool *valid){
	return Read4_1(address,GETM1ENC,status,valid);
}

Into either a simple asynch version of the above or a couple of methods... Sorry I don't have time to fully implement, but could create a version, probably using a state table, and some of the state information stored as member variables in the Roboclaw member variables...

Note: I might not handle retries directly in this version, so something like:

Code:

bool RoboClaw::Read4_1_start(uint8_t address, uint8_t cmd){
	// setup to send command and set the state...
	flush();

	crc_clear();
	write(address);
	crc_update(address);
	write(cmd);
	crc_update(cmd);
	// Have a few member variables to remember state 
	value = 0;   // member variable uint32_t
	ccrc = 0;    // member variable uint16_t;
    state = 1;   // some member variable to know what we are looking for...  maybe uint8_t state 0 is not active?
    time_last_char = micros(); // defined in uint32_t part of class
    return true;  // not sure if there are failure states here or not?
}

uint32_t RoboClaw::Read4_1_update(uint8_t *status, bool *valid){
	uint8_t crc;
	uint16_t data;

	if (state == 0 ) return 0;

	if(valid)
		*valid = false;
	

	// Now see if there are any data to process?
	while ((data = read()) != -1) {
		switch (state)  {
			case 1:
			case 2:
			case 3:
			case 4:
			    // read in the 4 bytes of data
				value = (value << 8) | data;
				ccrc_update(data);
				state++;
				break;
			case 5: 
				ccrc_update(data);
				if (status) *status = data;
				state++;
				break;
			case 6:	
				ccrc = data;
				state++;
				break;
			case 7: 
				ccrc = (ccrc << 8) | data;	
				if (ccrc == crc_get()) {
					// we have valid data
					if (valid) *valid = true;
				}
				state = 0; 
				return value;

		}
	    time_last_char = micros(); // defined in uint32_t part of class
	}
	if ((micros() - time_last_char) > timeout) {
		state = 0;
		return (uint32_t)-1;
	}
	return 0;
}

Again this was quick and dirty typed in, did not try to build or add member variables or the like. but the idea was you would call off to start a read and inside your loop or the like you can call the second function to maybe finish the command. If return value is 0, then not done, -1 error, else data.
Probably not best setup for return values, I would probably do it differently if really doing it, like have a pointer to return data, and the return value be some form of state...

But hopefully you get the idea.

In your case I would probably keep another set of states, where says which one of the two encoders on each serial port is active, and when the data comes back on that one, then startup the read for the other other one...

Again sorry still more to flesh out, but should hopefully give you some ideas.

PaulStoffregen · Nov 24, 2019

I recall looking at the RoboClaw code years ago. I believe back then it was using SoftwareSerial, which does indeed block while transmitting.

If you give me a direct link to the code, I could take another quick look.

KurtE · Nov 24, 2019

Hi @PaulStoffregen - The Roboclaw stuff is up at: http://downloads.basicmicro.com/code/arduino.zip

An extract of the code is up in posting #5:

Again what it does is send 2 bytes over serial to the roboclaw and then waits for 7 bytes to be returned (4 bytes for position, 1 byte for state, 2 bytes CRC)

And the code waits until it has the full response.

Which is why it hangs there. The internal functions, that I did not include in the previous post is the read(timeout), which does as you would expect:

Code:

int RoboClaw::read(uint32_t timeout)
{
	if(hserial){
		uint32_t start = micros();
		// Empty buffer?
		while(!hserial->available()){
		   if((micros()-start)>=timeout)
		      return -1;
		}
		return hserial->read();
	}
#ifdef __AVR__
	else{
		if(sserial->isListening()){
			uint32_t start = micros();
			// Empty buffer?
			while(!sserial->available()){
			   if((micros()-start)>=timeout)
				  return -1;
			}
			return sserial->read();
		}
	}
#endif
}

Note: If the read code gets an error like a timeout and/or CRC error it will repeat the whole thing twice...
The timeout value is set in the constructor.

Which is why I suggested you(he) may want to write an alternate version that does not block waiting for responses...

laptophead · Nov 24, 2019

Thank you everyone,

Me writing a new library for Roboclaw? Way beyond my abilities.
I will have to live with it. Would implementing the new teensy 4.0 help? Probably not.

Second issue:
Once every 200 mS I am writing a bunch of values to my tablet so I can monitor the arm.
It is a Serial.println at 57600 baud of a long string with data.

See the tablet setup if you want at: https://youtu.be/t_-1qefwpJQ

I discovered on the scope that also blocks the processor for about 15 mS. Of course it happens during my encoder readings resulting in a dropped connection to roboclaw intermittently.

The question:
Is there a way to do my Serial4.println without blocking?

KurtE · Nov 25, 2019

FYI - I emailed Nathan about it and got a response...

Thought about it yes, Implemented it no.

As long as there is sufficient serial buffer(I assume the teensy has enough) you can just split the write and read as you suggest.

When calculating the CRC you need the written data along with the read data to calculate it properly so you will want to pass the address and command to both the write command and the read command, or keep a global for crc and make sure you dont start another packet before processing the read data.

You can do the same to write only commands and just check the ack byte later as well but not sure its worth the very little time you will save.

Make sure you are using 57600bps or higher though. Roboclaw automatically add a 1ms delay between getting a command and writing back values when the baudrate is 38400 or less. That is so slower processors(or bitbanged serial) have time to switch directions.

If I get a chance I will try to grab one of my older Lynxmotion Rover or Tri... Which I think still has a roboclaw in it and see if I can get a quick and dirty non-blocking version of their API...

EDIT: Did not see your later question.

Serial4.println will only block if there is not sufficient space left in the output queue to store the data. It will wait all of your data is queued up...

You can try to avoid the blocking by calls to Serial4.availableForWrite() which tells you how much room is left in the queue...

So there are ways to minimize this. For example write all of the data into your own buffers, and then each time through the loop, check to see if you still have pending data to put into output queue, call the availableForWrite() and add that many...

OR you can change the size of Serial4.s output buffer.

You can do this by editing ...\cores\teensy3\Serial4.c and change the define SERIAL4_TX_BUFFER_SIZE which defaults to 40.
If you were using a T4, I have code in the HardwareSerial class, that you could define an additional buffer in your sketch
and then call Serial4.addStorageForWrite(buffer, buffer_size);

And it would increase the buffer for TX for only that sketch... But this has not migrated back to T3.x yet...

laptophead · Nov 26, 2019

Kurt
Thanks a lot. About Serial4 writing out, I reduced the size of the string I am sending, and I think is not blocking anymore.
I don't see the blockage on the scope like before.
However, it seems it's blocking the talk between the other 3 serial ports to the 3 roboclaws.

I can live with it, it is all done in 10mS. I learned a lot by hooking the Scope to see when the loop stops.
So everything gets updated at 10mS, thats 100 per sec.
I will see how the robot acts now in the various routines.

Thanks

Is Teensy Blocking during Serial communication?

laptophead

Well-known member

Attachments

KurtE

Senior Member+

laptophead

Well-known member

Elf

Well-known member

KurtE

Senior Member+

laptophead

Well-known member

KurtE

Senior Member+

PaulStoffregen

Well-known member

KurtE

Senior Member+

laptophead

Well-known member

KurtE

Senior Member+

laptophead

Well-known member