Can a teensy 3.5 handle 5 encoders and 5 PIDs?

Status
Not open for further replies.

laptophead

Well-known member
I am powering a robotic arm with 5 points of inflections, all feed back by encoders.
Moving 2 axis at the same time still works good, but when I ad the 3rd and fourth, the PIDs overshoot.

I looked at the encoder readings and it skips readings, sometimes 100 pulses at the time... Therefore erroneous PID outputs...

Should I go for the teensy 3.6? Would it make a difference?

Thanks
 

Attachments

  • MitsuTeensy_10_Michael_Inputs.ino
    22.9 KB · Views: 138
Thanks for the advice, the highest count rate is 42000, not too big. I tried the AltEncoder.h library from you -(luni) but I have a hard time with the conversion of his encoder values to integers.

Here I what I tried but it did not convert.

How do you convert your encoder values to integers?

What kind of numbers are your readings?

Thanks

I tried this but it did not work.
void loop()
{
for(int i = 0; i< 6; i++)
{
Serial.println(encoderList->counter);
Serial.print(" BaseEncoder= " );
BaseRotEncoderVal = (encoderList[0]);

}
Serial.println();

delay(100);
}
 
The values are stored as int, there is no need to convert. You have an error in the loop function. It should read

Code:
BaseRotEncoderVal = encoderList[0]->counter;
I had a look at the code you posted in #1, here an usage example which is closer to your application:
Code:
#include "AltEncoder.h"
using namespace AltEncoder;

Encoder KnobEncoder(24, 25);      // Control knob for testing
Encoder BaseRotEncoder(12, 11);   // Encoder for base Rotation

Encoder ElbowEncoder(33, 34);     // Elbow
Encoder ShoulderEncoder(28, 29);  //Encoder for Shoulder

Encoder Wrist_REncoder(30, 31);   //
Encoder Wrist_LEncoder(26, 27);   //


Encoder* encoderList[] =
{
   &KnobEncoder,
   &BaseRotEncoder,
   &ElbowEncoder,
   &ShoulderEncoder,
   &Wrist_REncoder,
   &Wrist_LEncoder,
   nullptr
};

void setup()
{
   Serial.begin(0);
   Controller::begin(encoderList, 25/*µs*/);   // choose a sampling period which is at least a factor of two smaller than the shortest time between two encoder signal edges. 
}

void loop()
{
   int BaseRotEncoderVal = BaseRotEncoder.counter; 
   Serial.println(BaseRotEncoderVal);
   delay(100);
}

the highest count rate is 42000, not too big.
Is this counts per second?
 
Luni,

So nice of you to write the code, it works for all encoders. Viellen Danke ( I lived in Osterreich for a year.)
However we got a problem. I need to Zero the encoders at startup. The old library allowed for KnobEncoder.write(0); for example , but of course that does not work anymore....

Here is my calibration function, without it I am dead. It is based on the robot hitting switches at the end of motion.

What to do? ( regarding the 42000 count, that is the range, and it takes 5 seconds to go that far, so freq is low, 10KHz, is that OK?

void Calibration()
{ ZeroBaseVal = digitalRead (ZeroBasePin);
ZeroShoulderVal = digitalRead (ZeroShoulderPin);
ZeroElbowVal = digitalRead (ZeroElbowPin);
ZeroWrist_RVal = digitalRead (ZeroWrist_RPin);
ZeroWrist_LVal = digitalRead (ZeroWrist_LPin);

// Serial.print(" ZeroWrist_RVal= ");
// Serial.print(ZeroWrist_RVal);
// Serial.print(" ZeroWrist_LVal= ");
// Serial.print(ZeroWrist_LVal);
// Serial.println();


if ( ZeroBaseVal == 1) // ZeroBase val is 1 when Switch is not pressed
{ analogWrite(BaseLeftPin, 256); // Shoulder rising till hits the sw
analogWrite(BaseRightPin, 200);
}
else // Now base SW is pressed and goes to 0
{ StopBaseRot(); // it hit the SW
BaseRotEncoder.write(0);
delay (100);
}


if ( ZeroShoulderVal == 1) // ZeroShoulder val is 1 when Switch is not pressed
{ analogWrite(ShoulderUpPin, 256); // Shoulder rising till hits the sw
analogWrite(ShoulderDnPin, 200);
}
else // Now ShoulderSW is pressed goes to 0.
{ StopShoulder(); // it hit the SW
ShoulderEncoder.write(0);
// KnobEncoder.write(10);
delay (100);
}

if ( ZeroElbowVal == 1) // ZeroShoulder val is 1 when Switch is not pressed
{ analogWrite(ElbowUpPin, 256 ); // Shoulder Descending to hit sw
analogWrite(ElbowDnPin, 180 );
}
else //
{ StopElbow(); // it hit the SW
ElbowEncoder.write(0);
delay (100);
}

if ( ZeroWrist_RVal == 1) // ZeroShoulder val is 1 when Switch is not pressed
{ analogWrite(Wrist_RUpPin, 256);
analogWrite(Wrist_RDnPin, 200);
}
else //
{ StopWrist_R(); // it hit the SW
Wrist_REncoder.write(0);
delay (100);
}

if ( ZeroBaseVal == 0 && ZeroShoulderVal == 0 && ZeroElbowVal == 0 && ZeroWrist_RVal == 0)
{ CalibrateTrigger = 1; // flip val of the trigger to 1 till the next reboot
}

}
 
Good to hear that it works.

I need to Zero the encoders at startup.

This is easy, you can assign any value at any time to the counters. Just do

Code:
 BaseRotEncoder.counter = 0;

... regarding the 42000 count, that is the range, and it takes 5 seconds to go that far, so freq is low, 10KHz,
That's quite slow, you will not run into any problems with 25µs sampling period. Would be interesting to see a video of the running system...

Viel Spaß mit dem Roboter :)
 
Last edited:
The Zero procedure worked, but I still have bad readings when 4 PIDs are working

Here is a sample, see how the numbers skip?

This does not happen if I just run your reading sketch and I just move the arm by hand...

You can see the PIDs in the original sketch, they dont allow uploading the sketch again...

Here is a video of the arm
https://youtu.be/yQxl888nHJQ

I am on teensy 3.5, I wonder if 3.6 will be better?

Thanks


TestKnobVal= 10 BaseRot = 2728 Shoulder = 1722 ElbowEnc = 4017 Wrist_R = 325 Wrist_L = -1
TestKnobVal= 10 BaseRot = 2746 Shoulder = 1757 ElbowEnc = 4017 Wrist_R = 326 Wrist_L = -1
TestKnobVal= 10 BaseRot = 2789 Shoulder = 1804 ElbowEnc = 4017 Wrist_R = 327 Wrist_L = -1
TestKnobVal= 10 BaseRot = 2827 Shoulder = 1838 ElbowEnc = 4017 Wrist_R = 328 Wrist_L = -1
TestKnobVal= 10 BaseRot = 2880 Shoulder = 1883 ElbowEnc = 4017 Wrist_R = 329 Wrist_L = -1
TestKnobVal= 10 BaseRot = 2916 Shoulder = 1917 ElbowEnc = 4017 Wrist_R = 330 Wrist_L = -1
TestKnobVal= 10 BaseRot = 2958 Shoulder = 1963 ElbowEnc = 4016 Wrist_R = 331 Wrist_L = -1
TestKnobVal= 10 BaseRot = 2982 Shoulder = 1999 ElbowEnc = 4016 Wrist_R = 331 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3011 Shoulder = 2047 ElbowEnc = 4016 Wrist_R = 332 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3033 Shoulder = 2083 ElbowEnc = 4016 Wrist_R = 333 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3064 Shoulder = 2139 ElbowEnc = 4015 Wrist_R = 334 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3089 Shoulder = 2185 ElbowEnc = 4014 Wrist_R = 334 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3115 Shoulder = 2233 ElbowEnc = 4013 Wrist_R = 336 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3150 Shoulder = 2298 ElbowEnc = 4012 Wrist_R = 338 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3175 Shoulder = 2347 ElbowEnc = 4012 Wrist_R = 339 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3201 Shoulder = 2395 ElbowEnc = 4012 Wrist_R = 340 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3236 Shoulder = 2444 ElbowEnc = 4012 Wrist_R = 343 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3265 Shoulder = 2476 ElbowEnc = 4011 Wrist_R = 345 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3294 Shoulder = 2507 ElbowEnc = 4011 Wrist_R = 346 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3333 Shoulder = 2545 ElbowEnc = 4011 Wrist_R = 347 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3359 Shoulder = 2571 ElbowEnc = 4011 Wrist_R = 348 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3384 Shoulder = 2594 ElbowEnc = 4011 Wrist_R = 350 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3412 Shoulder = 2622 ElbowEnc = 4011 Wrist_R = 355 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3433 Shoulder = 2642 ElbowEnc = 4011 Wrist_R = 357 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3452 Shoulder = 2662 ElbowEnc = 4010 Wrist_R = 359 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3479 Shoulder = 2689 ElbowEnc = 4010 Wrist_R = 362 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3501 Shoulder = 2711 ElbowEnc = 4010 Wrist_R = 366 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3526 Shoulder = 2734 ElbowEnc = 4010 Wrist_R = 369 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3560 Shoulder = 2764 ElbowEnc = 4010 Wrist_R = 370 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3586 Shoulder = 2787 ElbowEnc = 4010 Wrist_R = 372 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3611 Shoulder = 2811 ElbowEnc = 4010 Wrist_R = 374 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3640 Shoulder = 2843 ElbowEnc = 4010 Wrist_R = 378 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3660 Shoulder = 2869 ElbowEnc = 4010 Wrist_R = 382 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3680 Shoulder = 2894 ElbowEnc = 4010 Wrist_R = 386 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3706 Shoulder = 2928 ElbowEnc = 4010 Wrist_R = 394 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3727 Shoulder = 2952 ElbowEnc = 4010 Wrist_R = 401 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3748 Shoulder = 2975 ElbowEnc = 4010 Wrist_R = 413 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3777 Shoulder = 3004 ElbowEnc = 4010 Wrist_R = 435 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3800 Shoulder = 3024 ElbowEnc = 4010 Wrist_R = 456 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3823 Shoulder = 3045 ElbowEnc = 4010 Wrist_R = 480 Wrist_L = -1
TestKnobVal= 10 BaseRot = 3846 Shoulder = 3065 ElbowEnc = 4010 Wrist_R = 506 Wrist_L = -1
 
Wow, that's a big robot for such a small Teensy :)

I am on teensy 3.5, I wonder if 3.6 will be better?
I don't believe so and I wouldn't do that before you understand what is going wrong here. The only reason I could imagine at the moment is that you (or the PID library) disable interrupts or spend a lot of time in I high prioritized interrupt. Which PID library do you use?
 
I had a look at the code you posted in #1. Looks like you use this https://github.com/br3ttb/Arduino-PID-Library/. That lib doesn't use interrupts at all. I don't see any obvious reason why the encoder lib should miss counts. Especially as you observed the same behavior with two different libraries now.

Did you test if the encoder values are really wrong? An easy test would be to move an axis back to its reference position and read out the counter. Should be around zero if everything is OK.
Another thing: Are you sure that you don't get noise on your encoder lines? Maybe just from one motor? You wrote that two axes work fine, problems start with the third. Maybe the motor of the third axis generates more noise than the others? Do you have access to a scope to check the lines?
 
Signal is good

The signal from the encoders is fine. If the PIDs are not working and I move the arm by hand, all readings are there. I used the scope. The reason that some encoders are acurate is because they move slow, being close to the target.


I Wonder what to do? How many encoders you're using on your teensy? You have PIDs? Do they work at the same time?
 
I Wonder what to do?

Remote diagnosis with so little information is rather difficult. I would do the following:

First I would try to find out if the problem is really related to the encoders or to something else. For this I would reference the encoders, move them around until you see the strange behavior and move them back close to the reference position. (Do not move them to counter-zero but physically to the reference switch). Do this by manually moving the encoders first and compare results to moving them with the motors. If the encoders show values close to zero if you go back to the ref position they work ok and the error is somewhere else.

If the problem is really related to the encoders I'd carefully check the signal on the input pins of the Arduino. Do you see any noise on the signal? Is the phase between A and B about 90°? Can you send a picture of your scope measurement (both A and B signals, with running motors)? Can you send a schematic of the encoder part? Do your encoders have differential outputs?

And: can you post the actual code you are using?

How many encoders you're using on your teensy? You have PIDs? Do they work at the same time?
I tested with 6 encoders, I tested with encoder frequencies up to a few 100 kHz, I don't use PIDs (I use steppers) but I do a lot more stuff in parallel to reading my encoders.
I'm absolutely confident that the code you posted in #1 does not push your teensy to any limit at all.
 
Luni, thanks for the help,

I attached the file I am using. I still get jumpy readings, but the PIDs seem to respond well. I think is the serial monitor that is not keeping up, or is slowing the processor.

I think your solution is responding better than the Encoder.H so I will keep it.
Noch eine frage:

Can you make your library put out a frequency count? I would like to have the 2 pids (For the Wrist) be based on the motion speed.

Meaning the power to the DC motors will increase - decrease to attain a certain speed, and the motor will stop at the end of range based on if-else.
A sampling speed of 100mS would be good to start, but I might need it faster...

The two motors on the wrist assy have to move at the same speed to give me rotation...

Vielen Danke,

Mitch
 

Attachments

  • MitsuTeensy_13_WrRot_WrAngle.ino
    21.3 KB · Views: 85
  • Screen Shot 2017-08-16 at 7.19.16 PM.png
    Screen Shot 2017-08-16 at 7.19.16 PM.png
    32.5 KB · Views: 143
I attached the file I am using. I still get jumpy readings, but the PIDs seem to respond well. I think is the serial monitor that is not keeping up, or is slowing the processor.

They look quite OK for me?!? The encoders simply change faster than your loop displays them. Nothing to worry about I'd say.

Can you make your library put out a frequency count?
No, that would be a different concept, not fitting into a simple encoder counter lib. Anyway, you can simply do that yourself here a code snipped you could use.

Code:
 int curPos = Wrist_REncoder.counter;
  unsigned curTime = millis();
  Wrist_REncoderFreq = (curPos-Wrist_REncoderVal)/(curTime - Wrist_REncoderTime);
  Wrist_REncoderTime = curTime;
  Wrist_REncoderVal = curPos;
  
  curPos = Wrist_LEncoder.counter;
  curTime = millis();
  Wrist_LEncoderFreq = (curPos-Wrist_LEncoderVal)/(curTime - Wrist_LEncoderTime);
  Wrist_LEncoderTime = curTime;
  Wrist_LEncoderVal = curPos;
Note that this will only work if you have enough counts between two calls of this code. Currently your loop takes about 15µs (120MHz, Optimization: fast) without input from serial.

----------
EDIT:
Are you sure that your idea to lock the frequencies of the two motors with your PID will work at all? My understanding is that the PID will adjust the output variable to zero if the target is reached, i.e. speed will go up first, if the motor approaches target speed the PID wil regulate the output down -> motors slows down again -> oscillations?

Sorry, the last paragraph was nonsense, the I-Part of the PID should be able to do that...
--------

Do you really need to REGULATE the speed? Couldn't you simply disable the PIDs, set the speed of both motors to the desired value, and enable the PIDs if you reached the target position?
 
Last edited:
Help with the code!

Regarding
Do you really need to REGULATE the speed? Couldn't you simply disable the PIDs, set the speed of both motors to the desired value, and enable the PIDs if you reached the target position?

Yes, I think I have to regulate speed, but I am open to suggestions from you. I sent a pic of the assembly. The motors actuate the 2 side gears in opposite directions. If the speed is identical, the middle gear stays in one place but rotates around its own axis. If the speed varies, the middle gear will spin but also bounce up and down ,... Mitsubishi made is good enough to put a nut on a screw... I am trying to get close...
If the two motors have identical output, the in-equal frictions in the mechanism will drive the speed all over the place....

Regarding the code,
I implemented it for the Base encoder, (I can move by hand,) but it does not put out any values. I sent the code attached,

I also solved another issue.
The Knob, did not work with your library but worked fine with Encoder.h
I had to ad
pinMode (24, INPUT_PULLUP);
pinMode (25, INPUT_PULLUP);
Now is fine, I love your library...

You might want to document that for other folks. (Encoder.h did that from the library)

Thanks
 

Attachments

  • IMG_7926.jpg
    IMG_7926.jpg
    100.8 KB · Views: 93
  • Mitsu5EncodersAltEncoder.ino
    1.9 KB · Views: 94
Thanks for the video! That helps understanding your problem.

Summarizing what I understood from your posts and the video (please correct if I'm wrong):

  • The left motor runs a bit faster than the right one. The middle "gear" (how do you call this?) therefore slowly tilts back. Since the left motor is faster it reaches its target earlier and stops while the right motor is still moving.
  • Obviously both motors finally reach their targets correctly because at the end position the tilt of the middle gear is the same as it was at the beginning.
To me it looks like the encoders and PIDs do what they are supposed to do (moving the motors to the target positions) which is good.
The only problem left is that the PIDs move the motors at a slightly different speed.

The video shows that the motors are moving nice and smooth and with constant speed. So instead of trying to regulate the speed of the motors I'd start with trying to calibrate them. If that doesn't work you can still try to implement a speed regulation.

To check if a calibration will work, you can do the following simple experiment:
  1. Set both motor outputs to some constant value (manually setting the pin not with the PID) so that they move slowly.
  2. After some time stop both motors
  3. Adjust the output value for one of the motors until the middle gear shows no tilt at the end of the movement.
  4. Calculate the quotient of the motor settings which lead to no tilt.
Repeat for a faster speed. If you get about the same quotient for each speed a simple calibration should be good enough.
A video of the experiment would be very helpful
 
Luni,
I played with it yesterday, tried that too. Yes I could come closer to the desired effect, but I dont see it as a long term solution.
The middle gear will be connected to a grip, small hand, and that is meant to grab things and move them. At times the center of gravity of those things will not be in the middle. Than my adjustment for the different friction will be off.
The same will happen as the belts and gears and grease wear out in time.
Even right now, I have different behavior at different angles of the assembly.

I am sure the Mitsubishi engineers had an intelligent solution to deal with this based on keeping the 2 lateral gears in sync dynamically.

https://www.youtube.com/watch?v=BN2QS67ZEUs

shows a video of this in motion.

I would still like to try the "constant speed PID" solution, I wonder if you had a look at my code. Aslo I am thinking to write something that would make one motor the lead and the other the follower. So the second motor would just try to reach the same encoder reading all the time throughout the motion.
Or write a function that would constantly look at the EncoderR minus EncoderL and give or subtract power to each motor, till that number is close to zero...
 
Ok, understand, would have been an easy solution. Moving those motors in sync would be easy with steppers, but might turn out to be quite difficult with your servos.

Maybe somebody having more experience with PID regulators / servo drives can chime in here?

Yes, I had a quick look at your code but didn't find time yet to check whats wrong. I'll be on the road in an hour, but I can try something on the weekend if nobody else can help you here.
 
Last edited:
Found two old maxon dc motors with attached encoders in my junk box and connected them to a Teensy 3.1 board with a couple of TLE-5205 H-Bridges. With this setup I did a few experiments to synchronize the movement of two dc motors. The left motor was used as master and the right motor as slave. I.e., I set up the master PID to simply run 15000 encoder steps and the slave PID to minimize the difference of the encoder values of master and slave. I.e., the slave tries to follow the master motor as close as possible. Below the used sketch, it should be easy to convert if you want to repeat this experiment with your robot.

Code:
#include <arduino.h>
#include "MaxonMot.h"
#include "AltEncoder.h"
#include "pid\PID_v1.h"

using namespace AltEncoder;

// Motors & Encoders -------------------------------------
// Motor: Maxon 2008808, with included encoders (100 cts/rev)
// Motors driven by to TLE52005 Bridges, 12V

MaxonMotor<5, 7> master;  
Encoder encMaster(14, 15);

MaxonMotor<10, 12> slave;
Encoder encSlave(16, 17);

Encoder* encoderList[] =
{
	&encMaster,
	&encSlave,
	nullptr
};

// PID----------------------------------------------------

float masterTarget, masterInput, masterOutput;
float slaveTarget, slaveInput, slaveOutput;

float m_Kp = 0.6, m_Ki = 0.00, m_Kd = 0.0; // master: just move the motor, p only -> will not reach target, doesn't matter for this experiment
float s_Kp = 2.1, s_Ki = 7.0, s_Kd = 1.8;  // slave: try to follow master as close as possible, note: settings are close to instable

PID pidLeft(&masterInput, &masterOutput, &masterTarget, m_Kp / 1000.0, m_Ki / 1000.0, m_Kd / 1000.0, DIRECT);
PID pidRight(&slaveInput, &slaveOutput, &slaveTarget, s_Kp / 1000.0, s_Ki / 1000.0, s_Kd / 1000.0, DIRECT);


void setup()
{
	Serial.begin(0);

	pinMode(LED_BUILTIN, OUTPUT);

	Controller::begin(encoderList, 20);

	master.begin();
	slave.begin();

	pidLeft.SetOutputLimits(-75, 75);
	pidLeft.SetMode(AUTOMATIC);
	pidLeft.SetSampleTime(20);

	pidRight.SetOutputLimits(-75, 75);
	pidRight.SetMode(AUTOMATIC);
	pidRight.SetSampleTime(20);

	masterTarget = 15000;
	slaveTarget = 0;        // we will regulate the slave to zero difference in the encoders
                        
}

elapsedMillis stopwatch;

void loop()
{	
	masterInput = encMaster.counter;
	slaveInput = encSlave.counter - encMaster.counter;  

	pidLeft.Compute();
	pidRight.Compute();

	master.speed = (int)masterOutput;
	slave.speed = (int)slaveOutput;

	Serial.printf("%d\t %d\t%d\n", micros(), encMaster.counter, encSlave.counter); // tab seperated for easy import into excel
	
	if (stopwatch < 4000) return;  // stop after 4s

	// stop motors
	master.speed = 0;
	slave.speed = 0;

	while (1)
	{
		digitalWriteFast(LED_BUILTIN, !digitalReadFast(LED_BUILTIN));
		delay(50);
	}
}


Here a short video of the movement


It works in principle but it is not really perfect. Since a PID always needs a difference between target and current value to generate the output the slave motor has a significant lag relative to the master.

Here is about the best I could achieve by playing around with the PID parameters. To analyze the quality of the regulation I printed the values of both encoders and the elapsed microseconds in loop and copy/pasted the result to Excel.

encodervalues.PNG

Here the same settings but I disturbed the master by touching the gear two times with the finger. Slave follows nicely.

encodervalues2.PNG

I'm not sure if this is the right way to synchronize two servos. Anyway, it is fun to play around with this setup.
 
Last edited:
This looks very promising, I will try it on Mon. Thanks for the nice work, it's better than synchronizing the speed, I think.

I see is not perfect, but I think the original Mitsubishi is not perfect either, just very close.
I wonder why there is still an "Encoder difference" to the end of the motion.
Did the graph reach the end of the motion?

To keep them closer maybe we can offset for the difference such as:
slaveTarget = 600;
and then always deduct that from the command line.

This is fun,
I will let you know soon.
 
I wonder why there is still an "Encoder difference" to the end of the motion.
Did the graph reach the end of the motion?

My motor control is a rather primitive PWM. I.e, for slow speed the motor practically has no momentum and stops at speed settings from -5 ... +5. Thus the PID has a hard time to reach the target. Increasing KI will help of course but it will lead to oscillations. However, I would say this is an effect of my setup only, should be much better with yours.

I had an additional idea: In #19 both motors had the same speed limit so the slave can never reach the master if it lags behind. I therefore reduced the max speed of the master to give the slave a chance to catch up. I also made the slave PID settings more agressive and added a significant differential response (Kd) so that it will respond faster to speed changes of the master. The output of the slave PID (slave speed setting) starts to oscillate but due to the large inertia of my system this doesn't do any harm.

Here the recording of a undisturbed and a disturbed (2x stopped motor by hand) measurement:

freerunning.PNG disturbed.PNG

Starts to make sense :)

BTW: I changed all double to float in my code and in the PID library. The math processor of the T3.5 can only handle floats.
 
The graphs look really good, can you post the new code?
You talk a lot about speed settings but we never done this before.
We always shot for the target number of pulses and the speed was decided by PID, friction, and MaxOutput setting.
Did you implement a freq count along the way? Thats the only way to monitor speed, I think.
Also, I am glad you optimized the PID library. My Teensy 3.5 handled it in the original form, I wonder how did it work.
I was not too happy with it, I was becoming unresponsive when it had to do 3 PIDs in the same time, that was the original problem.
I wonder if changing to float will speed calculations. Please post the new library file.

Also , it is said that this PID library updates the output every 200mS even if SetSampleTime(20) for example. 200mS is very slow, I wonder if it is true, and if we can change that...
I like your graphing method, I was struggling with Processing and Serial plotter. How do you do it exactly?

I also have an original Arm like this and I filmed for us how good the motion is as a reference:

Luni, it's great to have a creative partner like you , thank you, it makes it more fun.

Here is the spinning
https://youtu.be/jAHhaewQNw4

Here is the movement of the wrist, (When we change the gear direction)

https://youtu.be/TG5Nl6VGhrc
 
The graphs look really good, can you post the new code?
Sure, attached it to the post. Did a few timing measurements this morning and found that the Serial.printf is limiting the update rate of the PIDs. So I packed the PID calls into an interrupt routine which made another big performance difference, you'll see the effect in the graphs at the end of the post.

You talk a lot about speed settings but we never done this before.
of course you did :) . E.g., the following snippet from your code calculates the new speed for the elbow motor and sets it.
Code:
    ElbowPID.Compute();
    analogWrite(ElbowUpPin, (256 - OutputElbow));

The "speed setting" graphs in my posts just show the PID outputs (which is the current speed setting). Nothing special here.

Did you implement a freq count along the way? Thats the only way to monitor speed, I think.
No, I didn't. I think the frequency is not very important as long as both motors run in sync. I can show you tomorrow how to calculate the frequency if you want to try to regulate it.

My Teensy 3.5 handled it in the original form, I wonder how did it work.
Do yourself a favor and get a logic analyzer (<20$ at Amazon). I always feel blind if I don't have one around. To measure the performance of the PID I simply set a pin (testPin2) to HIGH when I enter the interrupt function and back to LOW before I leave it.

Code:
void isr()
{
	digitalWriteFast(testPin2, HIGH);

	masterInput = encMaster.counter;
	slaveInput = encSlave.counter - encMaster.counter;

	pidLeft.Compute();
	pidRight.Compute();

	master.speed = (int)masterOutput;
	slave.speed = (int)(slaveOutput);

	digitalWriteFast(testPin2, LOW);
}

Here a screenshot from the analyzer:

pid_timing.PNG

Calculating both PIDs only takes about 1.4µs which is quite impressive. I call the function every 15µs which probably is overkill but it doesn't do any harm :)

I like your graphing method, I was struggling with Processing and Serial plotter. How do you do it exactly?

I output the interesting data (tab separated) to Serial in loop and copy and paste to a prepared excel sheet.
I'm using TyCommander for uploading, it has a nice serial monitor which makes this simple (CTRL A selects all, CTRL C copies to the clipboard. In Excel you need to "paste special | txt" otherwise you get all the data into one cell)

tycommander.PNG


I also have an original Arm like this and I filmed for us how good the motion is as a reference:
Where did you get this from? Would be fun to have one, not that I could use it for something useful but it looks like a lot of fun...


Finally the output of the current setting with the interrupt based PID calculation. I move to target 40000 and back to zero after 2.5s. Note that the deviation of both encoders improved significantly. Also the PIDs are much more responsive now.

result_isr.PNG

Have fun
Lutz
 

Attachments

  • PidTestCodeandExcel.zip
    269.7 KB · Views: 112
Last edited:
Hope you get an arm too

To buy an an arm like that look on ebay for Movemaster rm-501. They come maybe 3-4 a year, a complete system with controller in working order costs about 4000 USD. A broken arm "as is" can be found for about 500 to 1000.
Then you'd have to build your own controller, like me. I would be glad to share the hardware info .

There was one from china , I offered $400 but they declined.
http://www.ebay.com/itm/1pc-Mitsubishi-RM-501/302404808624?ssPageName=STRK:MEBIDX:IT
On ebay scroll down to see pics.

The next Arm I want to revive is the CRS Catalyst 3. I have it, it has only 3 motors with breaks on them to hold the arm at target. Interesting.
We'll use the same technology from the RM501. The CRS is much newer and simpler.
Maybe you can get that and we could work in parallel. You might find non working one for $500 to 1000. Generally the electronics are bad, but we'll make new ones anyways.


I attached a some pics of the Catalyst. Finally I would like to generate a PCB that can be adapted to any arm based on 4 elements: DC motor - encoder - limit switch combo, brake (bremse).
There are a lot of them out there, Epson, Panasonic, scorbot, all gathering dust because the controller died.

Regarding not doing a PID for speed: This is a bit philosophical,
I think we condition the PID on the number of steps to go and what we get out of it is the Force.
Speed means to me: Space/Time in our case Pulses/10mS, for example. Or 1/period of the wave.

Right now, our PID takes in "masterTarget = 40000;" and those are pulses, not Pulse/time.

I am implementing your code in the next hours.

I am a little scared of TYcommander on Mac... I will stick with serial port for right now.
 

Attachments

  • CRS Catalyst4.jpg
    CRS Catalyst4.jpg
    120 KB · Views: 122
  • CRS Catalyst3.jpg
    CRS Catalyst3.jpg
    110.1 KB · Views: 81
  • CRS Catalyst2.jpg
    CRS Catalyst2.jpg
    96.8 KB · Views: 87
Getting no Output from the PIDs

I adapted the code to my controllers, but something is wrong.

I attached the sketch.

This is what I get on the serial:

micros E_master E_slave Sp_master Sp_slave
612968 0 0 0 0
618088 0 0 27 27
623233 0 0 27 27
628378 0 0 27 27
633523 0 0 27 27
638668 0 0 27 27
643812 0 0 27 27
648959 0 0 27 27
654103 0 0 27 27
659248 0 0 27 27
664392 0 0 27 27
669538 0 0 27 27
674683 0 0 27 27
679828 0 0 27 27

then they all go to 0
2501428 0 0 27 27
2506572 0 0 27 27
2511718 0 0 27 27
2516863 0 0 0 0
2521993 0 0 0 0
2527123 0 0 0 0
2532253 0 0 0 0
2537384 0 0 0 0
2542513 0 0 0 0
2547642 0 0 0 0
2552773 0 0 0 0
2557903 0 0 0 0

The encoders read well, I moved them by hand:
5962101 40 469 0 0
5967261 40 469 0 0
5972421 40 469 0 0
5977581 40 469 0 0
5982741 40 469 0 0
5987901 40 469 0 0
5993061 40 469 0 0
5998221 40 469 0 0
 

Attachments

  • PID_Interrupt_Luni.ino
    3.3 KB · Views: 88
Status
Not open for further replies.
Back
Top