Help with setting up ODrive to work with Magic Pie motor

I did some tests for the Z encoder 2500 changing the starting position and motor_dir and that’s what behavior I was able to detect:

  1. .encoder_offset =-6263, .motor_dir = -1 looks like that it can’t find z pulse
    https://youtu.be/urRpeb1Ho8I

  2. .encoder_offset =-6263, .motor_dir = 1 there are several stable motor positions per round (not by z pulse)
    https://youtu.be/2NyKRk6UsS0

  3. .encoder_offset =-6263, .motor_dir = 1 in some positions there is that strange behavior. I tried to damp the oscillations with my hand
    https://youtu.be/qBpkqKWuDM0

Settings:
Motor - N5065
.calibration_current = 10.0f,
.current_lim = 10.0f,
.use_index =true,
.index_found = false,
.manually_calibrated = true,
.encoder_cpr = (2500 * 4),
.encoder_offset =-6263,


I understand that you may have many other things to do, but may I suggest to send this assembly (encoder,N5065 and Odrive) to you for verification ?
I will not disassemble this assembly so you could see this behavior.
Also i want to send the E6B2CWZ6C encoder with which ODrive does not work either (I ordered 3 such encoders, i checked all of them, and I wan’t to know - the problem is in them or in ODrive).
I can pay some money for checking and setting up.
Anyway, I’m going to order the new version 3.5 and I do not need version 3.4, but I need to be sure that these errors will not be in the new version.
Because this behavior with a large motor and current of 40 A can be dangerous.

It looks to me like the correct setting is motor_dir = -1, but something has changed (a phase wire is plugged in differently, some mechanical coupling has shifted, etc). Can you run the encoder calibration again?

Sure if you like you can send your rig to me and I can set it up correctly for you. You don’t have to pay for the time for me to set it up if everything is wired up and I only have to change firmware settings, just pay for the shipping.

Hello.
I’ve tested the Odrive 3.5 with the Z index encoder OMRON 2500 cpr and motor N5065.

Strange behavior repeats.
startup_motor_calibration - is ok
startup_encoder_index_search - is ok
But if after startup send
odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
then there is uncontrolled N5065 rotation
And after this odrv0.axis0.error returns code 49

Here is the video

Here is the json i used as backup-config
{
“config”: {
“brake_resistance”: 2,
“enable_uart”: true,
“enable_i2c_instead_of_can”: false,
“enable_ascii_protocol_on_usb”: true,
“dc_bus_undervoltage_trip_level”: 8,
“dc_bus_overvoltage_trip_level”: 51.840003967285156
},
“axis0”: {
“config”: {
“startup_motor_calibration”: true,
“startup_encoder_index_search”: true,
“startup_encoder_offset_calibration”: false,
“startup_closed_loop_control”: false,
“startup_sensorless_control”: false,
“enable_step_dir”: false,
“counts_per_step”: 2,
“ramp_up_time”: 0.4000000059604645,
“ramp_up_distance”: 12.566370964050293,
“spin_up_current”: 10,
“spin_up_acceleration”: 400,
“spin_up_target_vel”: 400
},
“motor”: {
“config”: {
“pre_calibrated”: false,
“pole_pairs”: 7,
“calibration_current”: 10,
“resistance_calib_max_voltage”: 1,
“phase_inductance”: 0,
“phase_resistance”: 0,
“direction”: 1,
“motor_type”: 0,
“current_lim”: 10,
“requested_current_range”: 70
}
},
“controller”: {
“config”: {
“control_mode”: 3,
“pos_gain”: 20,
“vel_gain”: 0.0005000000237487257,
“vel_integrator_gain”: 0.0010000000474974513,
“vel_limit”: 20000
}
},
“encoder”: {
“config”: {
“mode”: 0,
“use_index”: true,
“pre_calibrated”: true,
“idx_search_speed”: 10,
“cpr”: 10000,
“offset”: 0,
“offset_float”: 0,
“calib_range”: 0.019999999552965164
}
}
},
“axis1”: {
“config”: {
“startup_motor_calibration”: false,
“startup_encoder_index_search”: false,
“startup_encoder_offset_calibration”: false,
“startup_closed_loop_control”: false,
“startup_sensorless_control”: false,
“enable_step_dir”: false,
“counts_per_step”: 2,
“ramp_up_time”: 0.4000000059604645,
“ramp_up_distance”: 12.566370964050293,
“spin_up_current”: 10,
“spin_up_acceleration”: 400,
“spin_up_target_vel”: 400
},
“motor”: {
“config”: {
“pre_calibrated”: false,
“pole_pairs”: 7,
“calibration_current”: 10,
“resistance_calib_max_voltage”: 1,
“phase_inductance”: 0,
“phase_resistance”: 0,
“direction”: 1,
“motor_type”: 0,
“current_lim”: 10,
“requested_current_range”: 70
}
},
“controller”: {
“config”: {
“control_mode”: 3,
“pos_gain”: 20,
“vel_gain”: 0.0005000000237487257,
“vel_integrator_gain”: 0.0010000000474974513,
“vel_limit”: 20000
}
},
“encoder”: {
“config”: {
“mode”: 0,
“use_index”: false,
“pre_calibrated”: false,
“idx_search_speed”: 10,
“cpr”: 8192,
“offset”: 0,
“offset_float”: 0,
“calib_range”: 0.019999999552965164
}
}
}
}

The problem is the your have set “pre_calibrated”: true, but not written the offset.

If i set
“pre_calibrated”: false,
then
odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL doesn’t work.

I tried folowing:

  1. set in json
    “startup_motor_calibration”: true,
    “startup_encoder_index_search”: true,
    “startup_encoder_offset_calibration”: false,
    “startup_closed_loop_control”: false,
    “startup_sensorless_control”: false,
    “encoder”: {
    “config”: {
    “pre_calibrated”: false

  2. After startup index search run the commands
    1
    AXIS_STATE_CLOSED_LOOP_CONTROL doesn’t work

  3. What is the meaning of offset if after
    odrv0.axis0.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH offset is always 0
    2
    if in the json set
    "startup_motor_calibration": true,
    "startup_encoder_index_search": true,
    after startup_encoder_index_search offset equals 0 always

  4. What is the meaning of offset_float ?

Please follow the steps in the documentation exactly.
Specifically you need to:

  1. run AXIS_STATE_ENCODER_INDEX_SEARCH
  2. run AXIS_STATE_ENCODER_OFFSET_CALIBRATION

in that order. But yeah just follow all the steps in the docs.

I did as you wrote but there is some strange behavior to me.
If i change the start position of the motor when Odrive is switched on, the motor CLOSED_LOOP is fixed not to the Z index position.
But if in JSON set "startup_encoder_index_search": true then why after found z index motor is shifted and does not remain in the position Z index?

Does this mean that I can’t change the starting position of the motor despite the fact that every time there is a search for z index?

Here is the video

On the video i did folowing

  1. the motor is on in the position in which I calibrated the encoder with index signal. Therefore when i switched on everything is fine and CLOSED_LOOP retains Z index position.
  2. I changed the initial position of the motor and turned on the Odrive - the motor has shifted relative to the Z index and CLOSED_LOOP retains NON Z index position

Here is json

{
“config”: {
“brake_resistance”: 2,
“enable_uart”: true,
“enable_i2c_instead_of_can”: false,
“enable_ascii_protocol_on_usb”: true,
“dc_bus_undervoltage_trip_level”: 8,
“dc_bus_overvoltage_trip_level”: 51.840003967285156
},
“axis0”: {
“config”: {
“startup_motor_calibration”: true,
“startup_encoder_index_search”: true,
“startup_encoder_offset_calibration”: false,
“startup_closed_loop_control”: true,
“startup_sensorless_control”: false,
“enable_step_dir”: false,
“counts_per_step”: 2,
“ramp_up_time”: 0.4000000059604645,
“ramp_up_distance”: 12.566370964050293,
“spin_up_current”: 10,
“spin_up_acceleration”: 400,
“spin_up_target_vel”: 400
},
“motor”: {
“config”: {
“pre_calibrated”: false,
“pole_pairs”: 7,
“calibration_current”: 10,
“resistance_calib_max_voltage”: 1,
“phase_inductance”: 0.000016686473827576265,
“phase_resistance”: 0.03275086358189583,
“direction”: 1,
“motor_type”: 0,
“current_lim”: 10,
“requested_current_range”: 70
}
},
“controller”: {
“config”: {
“control_mode”: 3,
“pos_gain”: 20,
“vel_gain”: 0.0005000000237487257,
“vel_integrator_gain”: 0.0010000000474974513,
“vel_limit”: 20000
}
},
“encoder”: {
“config”: {
“mode”: 0,
“use_index”: true,
“pre_calibrated”: true,
“idx_search_speed”: 10,
“cpr”: 10000,
“offset”: 9519,
“offset_float”: 0.7360302209854126,
“calib_range”: 0.019999999552965164
}
}
},
“axis1”: {
“config”: {
“startup_motor_calibration”: false,
“startup_encoder_index_search”: false,
“startup_encoder_offset_calibration”: false,
“startup_closed_loop_control”: false,
“startup_sensorless_control”: false,
“enable_step_dir”: false,
“counts_per_step”: 2,
“ramp_up_time”: 0.4000000059604645,
“ramp_up_distance”: 12.566370964050293,
“spin_up_current”: 10,
“spin_up_acceleration”: 400,
“spin_up_target_vel”: 400
},
“motor”: {
“config”: {
“pre_calibrated”: false,
“pole_pairs”: 7,
“calibration_current”: 10,
“resistance_calib_max_voltage”: 1,
“phase_inductance”: 0,
“phase_resistance”: 0,
“direction”: 1,
“motor_type”: 0,
“current_lim”: 10,
“requested_current_range”: 70
}
},
“controller”: {
“config”: {
“control_mode”: 3,
“pos_gain”: 20,
“vel_gain”: 0.0005000000237487257,
“vel_integrator_gain”: 0.0010000000474974513,
“vel_limit”: 20000
}
},
“encoder”: {
“config”: {
“mode”: 0,
“use_index”: false,
“pre_calibrated”: false,
“idx_search_speed”: 10,
“cpr”: 8192,
“offset”: 0,
“offset_float”: 0,
“calib_range”: 0.019999999552965164
}
}
}
}

We currently only uses Z index to calibrate the commutation of the motor, but not the “home” or 0 position. We intend to use home switches for that instead.

If you want the index to see the home position, You can go to the encoder.cpp and where we do set_circular_count(0), you can also zero the linear count.

Sorry, but i need help with this.
I downloaded the latest version from GitHub master branch.
I see that all Visual Studio tools are still working and I can build and flash ODrive as before by calling task / build and task / flash

How exactly can I zero the linear count?

Here is the function from encoder.cpp

void Encoder::enc_index_cb() {
if (config_.use_index && !index_found_) {
set_circular_count(0);
if (config_.pre_calibrated) {
offset_ = config_.offset;
is_ready_ = true;
}
index_found_ = true;
}
}

I hope the situation with the home position to become clear. The length of the shoulders of my manipulator is about 80 cm and encoder resolution is 17500 (7:1 => 2500*7) so it will be difficult for me to make such a home switch that will allow accurately starts from calibrating position.

I also want to ask your advice about a very important problem that I am trying to solve.
In short - how to make a smooth stop/start of the motor? May be you need to add a maximum speed and acceleration limit to command odrive.SetPosition

The motor stops and starts very sharply. This becomes clearly visible on a powerful motor.
Here in this video is shown as motor 1500 W with current 30 A moves by commands smth like this

    odrive.SetPosition(0, 3000);
    delay(500);
    odrive.SetPosition(0, 5000);
    delay(500);

  1. One of solutions is to brake in this way:
    odrive.SetPosition(0, 4500);
    delay(5);
    odrive.SetPosition(0, 4600);
    delay(5);
    odrive.SetPosition(0, 4800);
    delay(5);
    odrive.SetPosition(0, 5000);
    If we started from position 1000 and want to stop at position 5000 then this method works.
    But in order to work correctly, you need to know the speed of the motor at the time of start braking.
    Also if the motor lifts the load, then for the correct choice of the braking interval, you need to know the weight of the load.
    Otherwise you will not get a smooth braking/starting.

  2. Another solution is getting the position by CTRL_MODE_VELOCITY_CONTROL.
    In this case, ODrive is controlled by speed commands, the encoder position is monitored, and the speed is changed.
    For some reason during the tests the torque on the motor is much less than when controlled via CTRL_MODE_POSITION_CONTROL

Perhaps you have some suggestion what approach to choose?
May be you already have something embedded in the FirmWare that can be used for this purpose?


Here some video from Automate 2017 which shows the typical movements of the manipulators and many of them - moving to a point at a given speed, breaking at given acceleration


We intend to implement trajectory planning into ODrive, that will give you your smooth motion (mostly, anyway. Robot arms have a lot of non-linear characteristics, so you’re really better off using something like ROS for this).

As Oskar already mentioned, the index search is only for internal ODrive requirements for the field oriented control of the motor. There are a couple of ways to use this information to know your arm’s world position though.

  1. Mount the encoder in such a way that the index pulse occurs exactly at the intended home position
  2. Find the offset from index to home, and always use that.

Thank you very much for the answer. I’d like to know a little more. Please answer a few more questions so that I understand which way to move.

About the smoothed motion and movement of the motor at the desired position with a constant current and different speeds -> I think that the most suitable way would still be to use SetPosition and delay commands.
Since the movement to the positive (pushing) load the motor will try to overtake its position and we will have to brake it.
The variant that was proposed in the cos-cycle Arduino demonstrates very clearly that it is possible to change the speed at a constant current.

for (float ph = 0.0f; ph < 6.28318530718f; ph += 0.01f) {
float pos_m0 = 20000.0f * cos(ph);
odrive.SetPosition(0, pos_m0);
delay(5);
}

The only question is how to know the current speed of the motor to calculate the stop/acceleration curve or track the desired speed to move.
It can be measured by obtaining a position at a specified interval

Please could you answer the question is it possible?

  1. I’ll call the drive command odrive.GetParameter (motor, drive.PARAM_FLOAT_ENCODER_PL_POS)
  2. I’ll call the odrive odrive.GetParameter again after some time
    What is the minimum (or constant) time interval can be when working through сom-port via Teensy?

I have a Teensy 3.5 and I will work through com port with Odrive, because Arduino does not give stable results. I did some tests with Arduino and got this

I run the command odrive.SetPosition(0, 10000);
After process finished I run this commands for a few seconds
odrive.GetParameter(0, odrive.PARAM_FLOAT_ENCODER_PLL_POS)
odrive.GetParameter(0, odrive.PARAM_FLOAT_POS_SETPOINT)
And such unstable values was ​​returned

PARAM_FLOAT_ENCODER_PLL_POS 9.0000	
PARAM_FLOAT_ENCODER_PLL_POS 9976.8818
PARAM_FLOAT_ENCODER_PLL_POS 9.0000	
PARAM_FLOAT_ENCODER_PLL_POS 18007.4003
PARAM_FLOAT_ENCODER_PLL_POS 9.0000	
PARAM_FLOAT_ENCODER_PLL_POS 9.0000	
PARAM_FLOAT_ENCODER_PLL_POS 9.0000	
PARAM_FLOAT_ENCODER_PLL_POS 18810.0000
PARAM_FLOAT_ENCODER_PLL_POS 9.0000	
PARAM_FLOAT_ENCODER_PLL_POS 18006.0996

So , Oscar advised me to use Teensy.
He wrote

I assume you are using SoftwareSerial on the Arduino? SoftwareSerial is rally crap, and it’s better if you use a more powerful arduino like Teensy (recommended), or Arduino Mega.`


Another very important question for me is whether I need to start rewriting the FirmWare by myself or whether I can solve it with your help.

Here is on this video with test motor D5065 and Zindex encoder Omron 2500 cpr. I done the calibration successful.
And then changed randomly the initial position of the motor in the off and after turning on the index was found, and then the motor was taken to the position in which it was when turned on.


On the video i did folowing

  1. the motor is on in the position in which I calibrated the encoder with index signal. Therefore when i switched on everything is fine and CLOSED_LOOP retains Z index position.
  2. I changed the initial position of the motor and turned on the Odrive - the motor has shifted relative to the Z index and CLOSED_LOOP retains NON Z index position

What should be done to prevent this from happening ? How to reset (set to 0) or change offset so when the index is found, the motor automatically remains in the index position, even if the positions when turned on are different?
Oscar gave me a link to a function

void Encoder::enc_index_cb() {
if (config_.use_index && !index_found_) {
set_circular_count(0);
if (config_.pre_calibrated) {
offset_ = config_.offset;
is_ready_ = true;
}
index_found_ = true;
}
}

I still hope that I do not have to disassemble the FirmWare to rewrite the initialization algorithm encoder or maybe you can tell me how best. Because with further changes FirmWare will have to transfer all the code in to the new version.

Let me show you the picture of my manipulator.


Shoulders very long, the resolution of the encoder 17500.
It will be very difficult to make such a Home switch that the initial position is always exactly the same. Therefore i can’t Mount the encoder in such a way that the index pulse occurs exactly at the intended home position
You wrote Find the offset from index to home, and always use that. How to do it?
Without an absolute encoder i never know the position in which the motor when turned on. I know only position of the Z index. The absolute encoder is not yet supported by Odrive but its resolution is up to 4096 and you will not be able to use pulleys, so Z index encoder itself is the best option in my opinion.

The easiest way out in my opinion is to make a new parameter in json config so that offset is set to 0 immediately after finding the index and no motor moves after this

“axis0”: {
“config”: {
“startup_motor_calibration”: true,
“startup_encoder_index_search”: true,
“startup_encoder_offset_calibration”: false,
“startup_closed_loop_control”: true,
“startup_sensorless_control”: false,
“enable_step_dir”: false,
“counts_per_step”: 2,
“ramp_up_time”: 0.4000000059604645,
“ramp_up_distance”: 12.566370964050293,
“spin_up_current”: 10,
“spin_up_acceleration”: 400,
“spin_up_target_vel”: 400,

Add smth like this

“set_position0_to_encoder_index”: true

Smooth motion

The best way to solve this is with a trajectory module. @Wetmelon started working on one recently. Maybe he will have something we can test out by the end of the weekend? (what do you say @Wetmelon? :wink: ) .

Encoder index as home

You should go edit the firmware. It’s not difficult to maintain your own version using git. If you don’t want to, then please make the changes in the firmware that you propose: add the configuration variable set_position0_to_encoder_index, and condition the reset on that. Then submit a pull request, and we will make it the official code, and you will help make the project better.

Try something like this:

void Encoder::enc_index_cb() {
    if (config_.use_index && !index_found_) {
        set_circular_count(0);
        if (config_.set_position0_to_encoder_index)
            set_linear_count(0);
        if (config_.pre_calibrated) {
            offset_ = config_.offset;
            is_ready_ = true;
        }
        index_found_ = true;
    }
}

Thank you very much for advice.
I added line set_linear_count(0) in enc_index_cb without new config param because its my only configuration.
Now all works perfect and does initialization what I need.

Should i look for trajectory module appearing in one of GitHub branches ? Or something else?
It would be great if the control of that module would be possible through teensy/arduino

I connected Teensy 3.5 to Odrive instead of Arduino and there are some errors and questions (with arduino there are no errors but p command still returns unstable values)
I listed errors down here:

  1. With teensy s command works, but read commands (like b or p) don’t return the values (its return values 0.000 always). Can I have missed something?
    if I incorrectly connected the outputs, then s command would not work
    This question is most important, because without getposition i can’t control manipulator

    Maybe this is due to the voltage 3 volts at the output of the ODrive?
    I used 31,32 Teensy pins with and without 3-5v level converter but i still can’t read bus voltage and position. s command works good in all cases
    image


2. There are some errors when i check the scketch in calibration sequence for Teensy , but when i comment error lines
odrive.run_state(atoi(c), requested_state, true); then all is normal

Great. I actually took you up on this idea and I added it to the official code, it’s only on devel now, but it will be released later.

We are working on it. You can subscribe to the relevant issue on github to get updated.

Can you check the signals with an oscilloscope or logic analyzer? I don’t think you need a level shifter, 3.3V should be ok.

That’s a good catch, that’s actually an error. I’m surprised it worked before. The issue is that atoi expects string, but we give it char. I’ll make a fix for that.

When i connect pins 8,9 Arduino to Odrive then s,b,p commands work good
and there is no error in lines odrive.run_state(atoi©, requested_state, true);

But when i connect the same pins (8,9) Teensy 3.5 to Odrive then command s works good but b,p don’t work
and there are error in lines odrive.run_state(atoi©, requested_state, true);

I can check the output from ODrive with the oscilloscope, but it seems to me that there is a signal, but there is an error with internal conversion perhaps
There may be a reason for this in the same reason why odrive.run_state (atoi ©, requested_state, true) does not work


Arduino


Teensy

I used for teensy 0 and 1 pins on teensy. Please, try

Thank you very much, hbtousa !
I connected 0,1 pins on teensy and now it works

and most importantly - now I see that the read motor positions loop returns stable results. And now I can finally start assembling the manipulator on large motors with Z index encoder with no initial moves !!
:tada: :grin:

Just in case, here is the pictures

1 Like

Hello. Here is the one of first tests of manipulator and the 2,5 kg load test.
calibration current = 10 A, current limit = 40 A, pos_gain = 100, vel_limit = 20000

It was hard to find the starting point so I made the starting platform.

It seems that inertia strongly affects the stopping at a predetermined position.
I believe that inertia can be reduced by a smooth stop.
For example by commands:
odrive.SetPosition(0, 4500);
delay(5);
odrive.SetPosition(0, 4600);
delay(5);
odrive.SetPosition(0, 4800);
delay(5);
odrive.SetPosition(0, 5000);
Therefore, I look forward to the completion of the work on the trajectory module to use the smooth stop with its help.

The braking trajectory changes and depends on the weight of the load.
Because the effect of inertia is even when moving without load, and with the load it is very noticeable. Thus, to accurately determine the smoothness of braking, we need to know the weight of the load.
But since there are no torque or weight sensors in the system it is hard to do.

There is an idea to trace the path of a stop for example in the first 300 ms and calculate the trajectory for a smooth stop by it. Although there will be a lot of trouble with the code.

Another idea is to simply put a weight sensor in the grasp area, perhaps one will be enough.
For example, this
aliexpress.com/item/1pcs-Miniature-load-cell-2-0MV-V-small-tension-sensor-pull-pressure-load-cell-5-200kg/32812616813.html
or this
aliexpress.com/item/5pcs-x-load-cell-strain-gauge-sensor-50kg-pressure-micro-scale-half-bridge-Weighing-Sensor-Electronic/32669863551.html

2
and knowing the weight of the load to calculate the trajectory of deceleration


There is one strange behavior associated with the calibration of the Z encoder.
I had to rotate one motor by 180 degrees because if set direction = 1 then after AXIS_STATE_ENCODER_OFFSET_CALIBRATION direction itself is set to -1

odrv0.axis0.requested_state = AXIS_STATE_MOTOR_CALIBRATION
odrv0.axis0.encoder.config.use_index = True
odrv0.axis0.motor.config.direction = 1
odrv0.axis0.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH
odrv0.axis0.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION
odrv0.axis0.encoder.config.offset
odrv0.axis0.motor.config.direction => !!! HERE IS direction set as -1 , if change manually to 1 there will behavior like this 2018_05_15 2500 10a z_ - YouTube


Another issue => sometimes after reload teensy scketch then Odrive stops read commands via GPIO.
After turning Odrive OFF/ON all is normal.What could it be?

Yes I think the trajectory module will help. I think like you suggest also using gain scheduling to change the gain tuning when you attach a load is a good idea. The industrial robots do this, and normally you actually have to manually enter the mass of the load. In your case I think you can just hold the load out against gravity and look at the current in the motor needed to hold it there, that will give you an estimate of the weight. You can calibrate the current to mass ratio with a known test weight. Look at .motor.current_control.iq_setpoint to get the current.