ODrives - umrover/mrover-ros Wiki

Table of Contents

Project Overview
System Explaiend
Top-Level Code
Configuration
Topics - Subscriber
Topics - Publisher
Watchdog Timeout Period
Installing odrivetool
Setting up a New ODrive
Getting the ID
Changing the USB Permissions
Calibrating the ODrive
Testing the Motor using odrivetool
Tuning Control Loops
Program Behavior if ODrive Errors
Program Behavior if USB Disconnects \ Checking if There are Errors
Common Errors
TODO \


Project Overview

The ODrive codebase deals with controlling the ODrive motor controllers to drive the wheels. For the 2022 Mars Rover Rosie, there are 3 ODrive motor controllers to drive the wheels. Each ODrive connects to the Jetson via USB. Each ODrive has two axes and each axis is assigned a wheel. To simplify wiring and code, the axis of each ODrive are either all left or all right. Also, the wheels that are connected to one singular ODrive are opposite each other (i.e. an ODrive is assigned either the front, middle, or back wheels).


System Explained

The Rosie '22 rover has 6 brushless motors. These brushless motors are controlled by 3 ODrive motor controllers. Each ODrive motor controller controls two brushless motors. The ODrive motor controller connects to the Jetson Xavier via USB.

The way the ODrives are mapped to each of the wheels is that each ODrive controls either the front pair, middle pair, or back pair of wheels. The ODrive itself receives battery voltage. It is connected to the Hall Effect sensors and the power of the wheels.

The ODrives can be controlled via the ODrive Python library. Our script is able to control the three ODrives, which can be identified by its ODrive ID.

Our program and the ODrive firmware also have watchdog capabilities. If the ODrive disconnects from the Jetson, then the wheels will stop moving. If the Jetson no longer receives new comms from the base station, then the wheels will stop moving. In all states, our program is able to recover (whether there be a watchdog error, ODrive error, a disconnection error, and more).

In our system, if there is an error in one axis, then the corresponding axis of that same ODrive will not run (e.g. if encoder does not work for left front wheel, then the front ODrive will run into an error state, and the right front wheel will not process commands either).


Top Level Code

odrive_control.py

This program controls the behavior of three ODrives. Various threads run simultaneously: One thread to process incoming commands, three threads to publish ODrive data for the three ODrives, and three thread to manage the watchdog for the three ODrives.

The code contains a basic differential drive controller. All wheels on the left have the same speed and all wheels on the right have the same speed. It expects Twist commands to have its linear velocity be in m/s and the rotational velocity to be in rad/s. If the rover receives too high commands, then it will adjust the ratios of the wheels and adjust the max.

ODriveBridge

One ODriveBridge object is created in main to store all the configuration data and manage the states of the ODrive and various other behavior.

States

The State object is kept track of in ODriveBridge. There are various types of States: ArmedState, DisconnectedState, and ErrorState. The State may change depending on the current ODriveEvent.

Disconnected State

In this state the program is searching for the ODrive by its ID number. Once it has found the ODrive, the state will immediately change to Armed.

Armed State

In this state the ODrive will respond to velocity commands until instructed otherwise or errors out.

Error State

In this state the ODrive has detected a system error and will reboot, going from Disconnected to Armed immediately.

Event

Dictates the behavior of the changing of States. The ODriveEvents are the following: When the ODrive is disconnected, trying to arm, or if there is an error.

Modrive

The Modrive object abstracts the behavior of the ODrive. The ODriveBridge creates the same Modrive object every time it tries to connect to an ODrive. The Modrive object has functions that allows it to get encoder data, get state data, set velocity, and much more.


Configuration

drive.yaml

This configuration file allows you to configure various things.

Axis - odrive/axis/

You may choose to configure what axis corresponds to either left or right. This must be consistent with which axes the left and right wheel are connected to electrically. This must only be 0 or 1 (and both should not share the same number).

ODrive Config - odrive/config/

You may choose to configure the ODrive configuration data such as current limit and watchdog timeout.

ODrive IDs - odrive/ids/

You may choose to configure which ODrive controls which wheel pair by listing its ID. The ODrive IDs of each ODrive can be found using odrivetool. Follow the steps here. This must be consistent with which ODrive is connected to which wheel pairs electrically.

Ratio - odrive/ratio/

You may choose to configure the direction of the ODrive for the wheel. It must be either -1.0 or 1.0. 1.0 means that the positive direction of the motor corresponds to the wheel moving forward.

ROS - odrive/ros/

You may choose to configure the ros specific stuff such as the rate at which we publish encoder data in Hz.

Wheel Specs - wheels

You must configure the wheel stuff to the actual specs of the rover. This should only be changed based on the hardware and mechanical properties of the rover. The base represents the width of the rover in meters. The radius represents the radius of each wheel. The ratio represents the gear ratio of the motor to the wheel. The max speed represents the max allowed speed of the rover.


Topics - Subscriber

Drive Velocity Commands

Message: JointState.msg "/drive_cmd/wheels/{left/right}"
Publisher: teleop
Subscriber: odrive_control

Topics - Publisher

Wheel Data

Message: WheelData.msg "/drive_data/joint/{front/middle/back}/{left/right}"
Publisher: odrive_control
Subscriber: gui

ODrive State

Message: ODriveState.msg "/drive_data/odrive/{front/middle/back}"
Publisher: odrive_control
Subscriber: gui


Watchdog Timeout Period

1 second


Installing odrivetool

View the Getting Started guide here.

Make sure to set up USB device permissions by running the following:

sudo bash -c "curl https://cdn.odriverobotics.com/files/odrive-udev-rules.rules > /etc/udev/rules.d/91-odrive.rules && udevadm control --reload-rules && udevadm trigger"

Setting up a New ODrive

Guide for the Nanotec/Maxon motors

  1. Open up odrivetool. On the Jetson run source ~/.mrover/build_env/bin/activate and type odrivetool. This should open up and say we connected to ODrive. We want (I think) version 0.5.4.
  2. If it's an ODrive we used before, just double check these parameters. Otherwise, set the following anyway (numbers in parentheses were used for Rosie '22):
  • odrvX.axisY.motor.config.pole_pairs which depends on the motor and is on the data sheet (was 7),
  • odrvX.axisY.motor.config.resstance_calib_max_voltage which should be either 1.2 or 0.6 (was 0.6),
  • odrvX.axisY.motor.config.requested_current_range which should probably be 8.0 (was 8.0),
  • odrvX.axisY.motor.config.torque_constant which it should say on the motor's data sheet and the units are Nm per amp and it might be 52.5/1000 (was 52.5/1000)
  • odrvX.axisY.encoder.config.cpr which should be 6 x pole_pairs (was 42)
  • odrvX.axisY.controller.config.pos_gain which should be 0.01 (was 0.01)
  • odrvX.axisY.controller.config.vel_gain which should be 0.01 (was 0.01)
  • odrvX.axisY.controller.config.vel_integrator_gain which should be 0 (was 0)
  • odrvX.axisY.controller.config.vel_limit which is speed and it could be 1000 or so unit is turns/second and we once wanted 5300 rpm (was 1000)
  • odrvX.axisY.motor.config.calibration_current which should be 1.45 or 0.7 (was 0.7).
  1. Save configuration and reboot by running odrvX.save_configuration(). If this returns false, make sure all axes are in the IDLE state by doing odrvX.axisY.requested_state = AXIS_STATE_IDLE. Then reboot by running odrvX.reboot.

Getting the ID

Each ODrive has a unique serial ID. In order to determine that this ID is, follow the steps on this website on the hoverboard guide page. To activate odrivetool follow the steps below. You can do this on the base station or the Jetson. On the Jetson make sure the
odrive_bridge program on the Jetson is deactivated.
Open up odrivetool. On the Jetson run source ~/.mrover/build_env/bin/activate and type odrivetool. This will start up odrivetool, and after a few seconds Connected to ODrive [ID number] as odrvX should appear on the screen.
Type
$ quit()
$ deactivate
to get out of this state.
In the config/drive.yaml, look at the line that sets the IDs. Depending on which ODrive you replaced, change its ID to the new one. Rebuild the program. \


Changing the USB Permissions

USB permissions when connecting the ODrive to the Jetson have to be modified before you can successfully communicate with the odrive. This only has to be done once.
Make sure the ODrive is connected via USB and ssh into the Jetson. Type
$ lsusb . From list find the idVendor, idProduct, and MODE of the odrive. It will be listed under the info for the InterBiometrics device.
$ sudo vi /etc/udev/rules.d/50-myusb.rules
add
SUBSYSTEMS=="usb", ATTRS{idVendor}=="1209", ATTRS{idProduct}=="0d32", GROUP="mrover", MODE="0666"
to the file
If this is on your virtual machine change the group name to be vagrant.
Restart the Jetson.


Calibrating the ODrive

Calibration must be done manually. Once again ssh into the Jetson and go to the .mrover folder and start running odrivetool.
Run source ~/.mrover/build_env/bin/activate and type odrivetool. The odrives should automatically connect. Using the odrvX.axisY (0 or 1 depending on the ODrive with the id) in place of m_axis, execute all of the following commands for axis0 and axis1. \

$ odrvX.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE
The motor should beep and start calibrating now.

You can check to see if it completed successfully by running dump_errors(odrvX). If there are errors, you can try doing dump_errors(odrvX, True) to first dump the errors, and then you can retry the calibration sequence.

If there are still errors, you may need to check the Errors section below. Once it has finished, type
$ odrvX.axis0.motor.config.pre_calibrated = True
$ odrvX.axis0.encoder.config.pre_calibrated = True
Repeat these three commands for axis1 as well. Then type
$ odrvX.reboot()


Testing the Motor Using odrivetool

  1. Open up odrivetool. On the Jetson run source ~/.mrover/build_env/bin/activate and type odrivetool.
  2. Put the odrive in closed loop by doing odrvX.axisY.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
  3. Set the velocity by doing odrvX.axisY.controller.input_vel = [insert velocity] where velocity is in turns per second.
  4. To get the actual speed it's going at, run odrvX.axisY.encoder.vel_estimate where it returns a number in turns per second.

Tuning Control Loops

  1. For velocity control you want to plot odrvX.axisY.encoder.vel_estimate and odrvX.axisY.controller.input_vel.
  2. Type in start_liveplotter(lambda:[var1, var]) to get a graph.
  3. If it's saying from matplotlib_image does not exist, then in the odrive tool terminal, run import matplotlib and matplotlib.use('GTKAgg')
  4. Tune the control loops as you please, hopefully you are following a forum and/or know what you're doing (do not kill the ODrives/motors please!)

Program Behavior if ODrive Errors

If the ODrive throws an error, odrive_bridge will tell it to reboot. The error will be displayed in the terminal and the state will be displayed as ErrorState. Depending on which ODrive this happens on, either the front or back motors will be unresponsive for a minute until the ODrive finishes rebooting. The state will automatically change from DisconnectedState to Armed upon completion.


Program Behavior if USB Disconnects

If the ODrive is disconnected from the USB port, odrive_bridge will print "ODrive has been unplugged" to the terminal, and will change the state to DisconnectedState until the connection is reestablished. At that point the state will automantically change to Armed.


Checking if There are Errors

First make sure that the odrives.py is not running. Open up odrivetool. On the Jetson run source ~/.mrover/build_env/bin/activate and type odrivetool. $ dump_errors(odrv0)
$ dump_errors(odrv1)


Common Errors

ODrive Is Not Responding To Calibration

Open up odrivetool. On the Jetson run source ~/.mrover/build_env/bin/activate and type odrivetool. $ dump_errors(odrvX, True)
$ dump_errors(odrvX, True)
If an ENCODER_HALL_ERROR shows up only the first time, you are good to try calibration again. If no errors show up at all, or if the error persists, re-check your wiring.

Unexplained USB Failures

For running on the Jetson nano, if you are getting usb suspend errors,
sudo vi /boot/extlinux/extlinux.conf
At the end of APPEND add usbcore.autosuspend=-1
With a space between the end of APPEND and the start of 'usbcore..'
Then reboot.
Upon reboot check cat /proc/cmdline to verify the change was put into place ** this does not work on the xavier due to linux boot reasons **

Also make sure nothing else is on the usb bus (including the wifi chip)

USB Forwarding on VM

Make sure the ODrive is connected via USB and type
$ lsusb . From list find the idVendor, idProduct, and MODE of the odrive. It will be listed under the info for the InterBiometrics device.
$ sudo vi /etc/udev/rules.d/99-usb-serial.rules
$ SUBSYSTEMS=="usb", ATTRS{idVendor}=="[__idVendor__]", ATTRS{idProduct}=="[__idProduct__]", GROUP="mrover", MODE="[__MODE__]"
Restart the VM.

ERROR_ILLEGAL_HALL_STATE

dump_errors(odrvX, True)
If the error persists, the encoder wires are probaby disconnected. Tell electrical to recheck the wires\connectors. </

Suddenly No Response

In this case, stop and restart the ODrive program. The problem is still being investigated \

Unknown ACK and Failure

In this case, stop and restart the ODrive program. The problem is also still being investigated \

No ODrive Module

Make sure you are connected to wifi.
On the Jetson run source ~/.mrover/build_env/bin/activate. $ pip3 install odrive

Other Errors

Find someone on ESW. Or just go ahead and contact madcowswe himself.