Hi
every one!
I am try to build a Bolt by myself.
First of all i want to do some test about single motor.
What i have is a TI board and a motor. I have flashed the firmware using “dual_motor_torque_ctrol_can”,the CAN communication is same as described in Connecting via CAN — ODRI Firmware for uDriver Boards 1.0.0 documentation
Then i modified the demo_1_motor.cpp,make the desired_current is not zero,and recompile blmc_drivers.
When i try to run “demo_1_motor” as a test,there are something wrong,the motor doesn’t spin,error code is 2.
Here is my question:
1 I do not install PREEMPT_RT kernel now,does this cause the problem?
2 Can motor run without encoder? What will happen if there is no encoder?
3 Is there a very detailed motor test guide,which i can follow to do motor test to check if everything is
ok? Even now I am not sure the motor wiring is in right order.
4 What is virtual spring mode about motor? i found this option in Universal Dual Motor GUI.Does there exist docs about this?
Hi, Here are some element of answer. I’m not familiar with the CAN version of the firmware but I know that:
1 - No, PREEMPT_RT is not mandatory for simple tests, it will just reduce the jitter in the execution timings.
2 - No, the motors are controlled using FOC. The motor position is needed for the controller to know how to power the coils. (Here is a nice introduction to FOC if you want to know more Field-Oriented Control - Trinamic)
3 - For CAN, I’m not the best person to answer maybe @felixwidmaier ?
4 - Virtual Spring mode is a mode where the control low applied to the motor is:
i = Kp * (p0 - p) - Kd * v
With :
i the current motor (analogue to torque)
Kp is the stiffness coefficient of the virtual spring
Kd is the damping
p0 the equilibrium motor angle
p the motor angle
v is the motor velocity
The motor is behaving like a spring damper system with this mode.
In CAN firmware, it was implemented as a test setup.
In SPI firmware, this is now used to control the motor in impedance, with ability to change on the fly at 1khz: Kp, Kd, p0, v0 and i0 with the control low being;
i = Kp * (p0 - p) + Kd * (v0 - v) +i0
Error code 2 means the timeout on the board was triggered because no new command was received via CAN for a certain time (see documentation of error codes).
This may indeed be caused by not using the real-time kernel. For a first test you can try with simply increasing that timeout. It should be possible to do this by editing this line of demo_1_motor.cpp from
The last argument is the timeout in milliseconds. The default is 100, increasing it to 1000 (=1s) should hopefully be enough.
If this works, I recommend that you switch to a more real-time capable kernel to get more stable control. However, you can try the “lowlatency” kernel first. It is a bit “less real-time” than the PREEMPT_RT kernel but much (!) easier to install and as long as you don’t overload your CPU it is likely good enough. There is info about it in the documentation of the robot_interfaces package.
I am not really familiar with this demo but from looking at the code it should indeed spin the motor if the desired current passed to set_current_target() is set to a high enough value (I think something around 0.5 should be enough, if there is no load on the motor).
I am currently not in the lab, though, so I cannot test it myself.
Hi,
I also use the Dual Motor GUI that @felixwidmaier implemented for testing the actuator modules via USB with a windows computer. The spring mode is a very nice way to evaluate your setup.
After connecting the motor and encoder and flashing the TI card you want to:
Click and deselect the “IqRef from CAN”
Click the “Run Motor J1” button
the card will send current to the motor to determine the magnetic orientation of the rotor
the motor will move a bit - during the period you should not hold or disturb the rotor
after a couple of seconds the motor is aligned and the GUI will display “Status: Ready!”
you can now select the Torque Controller tab and click the “spring mode” button
the speed control tab will not work with this software - for speed control you’ll have to flash the cards with the “dual_motor_vel_control”
if you displace your rotor now you should feel a torque proportional to the displacement
if you let the rotor go it will start to oscillate
if you don’t get the spring behavior and the rotor seems to get stuck at different angles the motor phase order is wrong
in this case you want to power down the motor driver card, swap two of the motor phases and try again - it doesn’t matter which motor phases you switch - for example A->B and B->A
Thank you very much @fgrimminger.
I am just new to motor control,not familiar with TI development environment.
You those step-by-step guide help me a lot.
Now i have finished another motor test,the result is shown in the video: single motor test.mp4
I modify the code of “demo1_motor.cpp” according to your suggestion and run it.
It drive the motor spin fast when the current is 0.5, and not error occur.
Yes - that looks good - the spring mode is working as expected.
You can change the stiffness and/or the max. current setting to get different behaviors.
Now i use “demo_1_motor” to test the motor.
Most of time everything is ok,but sometimes the motor would suddenly stop soon after the motor start to rotate.The error code is 1.
I check the document,the description is:Encoder error too high.
Then i read the code,there is no more information.I seems the encoder read a value larger than QEP_MAX_INDEX_ERROR whihch is (1./360.)*(5000.0).
The “encoder error” works like this: In addition to the regular ticks the encoder has a special “index tick” which only occurs once per revolution. Since there is only one, it should always appear at the same position when spinning the motor. We use this for some basic fault checking: Whenever this index tick is observed, the firmware checks if it occurred at the expected position, i.e. exactly one revolution since the last time it occurred (+/- QEP_MAX_INDEX_ERROR to allow some minor errors).
If the observed position differs from the expected position by more than QEP_MAX_INDEX_ERROR ticks, the error is triggered.
In practice this means there is either some problem with the encoder itself (maybe a dirty or damaged encoder disc, resulting in some ticks to be missed occasionally) or with the cables connecting the encoder with the board. For example, we often get this error when a cable breaks inside the insulation, resulting in a loose connection (so most of the time the connection is there but sometimes it is interrupted for a moment, resulting in ticks to be missed).
Now i am reading the demo code of blmc_drivers and try to run the demos.
“demo_2motor” seems using PD controller to control motor track the desired pos for hardware slide.
Because i don’t have hardware slide at hand,so i write a simple qt demo,using a virtual slide to provide a desired pos for motor tracking.
The motor doesn’t rotate smoothly.
There are vibration and oscillation when the motor reach the desired pos,as show in the video:
I don’t install real time kernel now, and the CAN communation frequency is 4000 per second.
How can i make the motor rotate smoothly?
Does there exist example code show me the way to solve this problem?
I can’t solve this problem this problem for two days.
I also read other demo codes,like python_blmc,teststand…,try the PD params in those codes,but no work.
According to the information i know,i thing i dont’ need to know implements of udriver_firwmware,just compile it,and flash to the TI board.
Does i need to know about the implements of udriver_firwmware?
I find the bug.
I use a QT timer(10ms) to control motor track the virtual slide pos,the motor oscillate hard.
If i do the motor control almost same as “demo_2_motos”,the motor wouldn’t oscillate,this is the video: motor no oscillate
But there is sitll one problem i don’t sovle,when do motor test ,it is very easy to recv a error which error code is 2(means CAN receive timeout).
Sorry for the late response, I’m just back from a longer leave.
I highly recommend using a real-time kernel, this will likely solve the CAN receive timeout issue and allow for more robust control of the motors.
You can try with the “lowlatency” kernel (instead of PREEMPT_RT), which is very easy to install on Ubuntu and will likely be good enough. See the documentation here: Real Time Setup — robot_interfaces documentation.