API Control
CAN Configuration
Hardware Configuration
Termination resistors must use Split termination. Star connection is prohibited. You must ensure that there are exactly two sets of termination resistors on the same CAN line, and both are at the two ends of the line.
Software Configuration
Our motors use CAN-FD. The arbitration segment is fixed at 1Mbps with a sample point of 0.8. The data segment baud rate is fixed at 5Mbps with a sample point of 0.75.
Recommended CAN configuration command:
ip link set can0 type can bitrate 1000000 sample-point 0.8 dbitrate 5000000 sample-point 0.75 sjw 5 dsjw 3 fd on
Note
The sample point must be set correctly, otherwise sending long frames will definitely cause communication failure. Some USB-to-CAN devices may not correctly support setting these parameters. If you encounter this situation, please use an SOC with hardware CAN-FD support such as RK3576, or consider a Maver-L4 set that includes a main controller.
Configure Motor ID and Parse CAN Messages
Our motors use the CANOpen(CiA301) protocol. CANOpen is a widely used protocol in industrial automation with many tutorials available online, so we won't elaborate here. Some minor modifications have been made to PDO, allowing PDO to have a maximum length of 64 bytes, making full use of CAN-FD bandwidth.
CAN-ID is set through object dictionary 2001h, 1h.
Be Careful About ID Conflict
Each motor module has two motors. Please be careful not to set the same ID for two motors in the same module, otherwise you will not be able to distinguish between the two motors except by opening the cover.
For example, using the canopend tool for configuration, change the ID of the original 1st motor to 3rd:
For how to use the canopend tool, please refer to its Github introduction, here we will not explain it.
New ID Takes Effect After Power Off
After setting a new ID, it will only take effect after power off. Please be careful not to set the wrong ID.
The default TPDO mapping is as follows:
// TPDO1: Position(0x6064, 0, i32(length 0x20)), Timestamp(0x1013 0, u32(length 0x20)), Torque(0x6077, 0, i16(length 0x10)), Error Code(0x603F, 0, u16(length 0x10)). 12 bytes, 1000Hz.
// TPDO2: Status Word(2 bytes), Driver Temperature(2 bytes), Motor Temperature(2 bytes), Control Word(2 bytes), Error Code(2 bytes). 10 bytes(actually sent 12 bytes), 50Hz.
In summary, a typical complete motor initialization and control flow is as follows:
- The host computer program reads once object 1018h 03h to obtain the motor firmware version to ensure compatibility between the host computer program and the motor firmware version.
- The host computer program reads once object 6076h 00h to obtain the motor's peak torque for subsequent calculation and conversion to international unit torque.
- The host computer program reads once object 2003h 07h to obtain the MIT KP and KD scale factors for conversion to KP and KD in international unit dimensions. (Only for firmware version 07, and only if MIT mode needs to be used)
- The host computer program configures heartbeat detection object 1016h 00h to ensure the motor automatically exits enable when the host computer program ends. (Optional, timing starts from the first received heartbeat from the target node)
- The host computer configures TPDO and RPDO mappings (if parameters have been saved through object 1010h, this step is not needed)
- The host computer program generates a CAN receive processing process to parse and handle the motor's TPDO, HEARTBEAT, and optional EMCY
- The host computer sends NMT commands to set the motor to Operational state.
- The host computer correctly configures the CiA402 state machine, selects the mode to enter, and enables the motor.
- The host computer starts sending its own heartbeat (optional, if heartbeat timeout protection is needed. Own heartbeat is recommended to be sent at low frequency.), and starts sending control commands through PDO.
Here is a code example for the parsing part (i.e., step 6):
There are a few details
- Speed is calculated on the host computer using dequeue through hardware timestamp difference, eliminating potential time errors and allowing users to choose their own filtering algorithm.
- Since 6077h has no unit, it is 0-1000, provided in thousandths. Therefore, it needs to be multiplied by 6076h to get the torque value in international units. You should read once 6076h at motor startup (constant for the same motor). Use it for multiplication afterwards.
- About the Q21 data format: Q21 is a fixed-point format for representing decimals. Compared with floating-point formats, this representation has no accumulated error. Its precision is (1 / 2^{21}). When represented with a 32-bit value, the range is -1024 to 1023.99 (0x80000000 to 0x7FFFFFFF) * (1 / 2^21). Conversion:
Q21 = 2^21 * float. For example, if the motor’s actual position is1.52 Rev, the value read from 6064h is3187671 (1.52 × 2097152).
for frame in frames {
let id = frame.id();
if let socketcan::Id::Standard(id) = id {
let id = id.as_raw();
let canopen_id = (id as u8) & 0x7F;
if canopen_id != self.canopen_id {
continue;
}
if let Ok(communication_object_code) =
canopen::canopen_types::CommunicationObject::try_from(id)
{
let constants = self.runtime_constants.get();
match communication_object_code {
canopen::canopen_types::CommunicationObject::TPDO1 => {
if frame.data().len() != 12 {
continue;
}
// TPDO1: Position(4 bytes), Timestamp(4 bytes), Torque(2 bytes), Error Code(2 bytes). 12 bytes, 1000Hz.
let position = i32::from_le_bytes([
frame.data()[0],
frame.data()[1],
frame.data()[2],
frame.data()[3],
]);
// Reduce pi, to map start point from 0-2pi to -pi-pi
let position = position - Self::OFFSET;
let timestamp = u32::from_le_bytes([
frame.data()[4],
frame.data()[5],
frame.data()[6],
frame.data()[7],
]);
let speed = {
let dq = &mut self.runtime_data.lock().unwrap().raw_position;
// Push the new data
dq.push_back((position, timestamp));
if dq.len() > self.speed_average_window {
// Pop the first incoming data
let last_data = dq.pop_front().unwrap();
// In us
let time_diff = timestamp.overflowing_sub(last_data.1).0;
// In seconds
let time_diff = time_diff as f64 / 1_000_000.0f64;
// Q21 Rev
let position_diff = position.overflowing_sub(last_data.0).0;
// In rad/s
position_diff as f64 * 2.0f64 * std::f64::consts::PI
/ time_diff
/ 2_u32.pow(21) as f64
} else {
0.0f64
}
};
let torque = if let Some(peak_torque) = self.runtime_constants.get() {
let peak_torque = peak_torque.peak_torque;
let torque = i16::from_le_bytes([frame.data()[8], frame.data()[9]]);
// Convert mapped torque to Nm
torque as f64 * peak_torque as f64 / 1000.0f64
} else {
0.0f64
};
let error_code =
u16::from_le_bytes([frame.data()[10], frame.data()[11]]);
{
let runtime_data = &mut self.runtime_data.lock().unwrap();
// Raw position is already updated, so no need to update it here
runtime_data.last_contact_time = std::time::Instant::now();
runtime_data.smoothed_speed = speed;
runtime_data.torque = torque;
runtime_data.raw_error_code = error_code;
}
}
canopen::canopen_types::CommunicationObject::TPDO2 => {
if frame.data().len() != 12 {
continue;
}
// TPDO2: Status Word(2 bytes), Driver Temperature(2 bytes), Motor Temperature(2 bytes), Control Word(2 bytes). 8 bytes, 50Hz.
let status_word =
u16::from_le_bytes([frame.data()[0], frame.data()[1]]);
let driver_temperature =
i16::from_le_bytes([frame.data()[2], frame.data()[3]]) as f32
* 0.1f32;
let motor_temperature =
i16::from_le_bytes([frame.data()[4], frame.data()[5]]) as f32
* 0.1f32;
let control_word =
u16::from_le_bytes([frame.data()[6], frame.data()[7]]);
{
let runtime_data = &mut self.runtime_data.lock().unwrap();
runtime_data.status_word = status_word;
runtime_data.driver_temperature = Some(driver_temperature);
runtime_data.motor_temperature = Some(motor_temperature);
runtime_data.read_control_word = control_word;
}
}
canopen::canopen_types::CommunicationObject::Heartbeat => {
if frame.data().len() != 1 {
continue;
}
let data = frame.data()[0];
if let Ok(nmt_state) = NMTState::try_from(data) {
if nmt_state != NMTState::Operational {
{
let mut runtime_data = self.runtime_data.lock().unwrap();
if runtime_data.last_nmt_send_time.elapsed().as_millis()
> 250
{
runtime_data.last_nmt_send_time =
std::time::Instant::now();
}
}
ret.push(canopen::set_nmt_state(
NMTState::Operational,
self.canopen_id,
));
}
self.runtime_data.lock().unwrap().nmt_state = Some(nmt_state);
}
}
_ => {}
}
}
}
}
Motor Control
Control Command Timeout
CANOpen has a built-in timeout mechanism. It is disabled by default, and you need to enable it manually. Please refer to the introduction of object 1016h for details.
Motor Disable Brake
By adjusting object 2040h 00h, you can set whether the motor windings are shorted to maintain braking state after the motor is disabled or a fault occurs.
One-to-Many Control
In CANOpen, PDO is not limited to using specified COB-IDs, but can be freely set. This means you can achieve one-to-many functionality by setting the COB-IDs of RPDO of multiple machines to the same value.
Does it sound abstract? Let's take four motors using force-position hybrid control as an example.
The CANOpenIDs of two motors are 0x01 and 0x02 respectively. Each motor controls their 6072h (Max Torque) and 60FFh (Target Velocity). Set the COB-ID of RPDO1 of both motors to the COB-ID of TPDO1 of node 0x10.
Then each motor has two valid objects. Other bytes can be filled. We assume 6072h is placed first, so for motor one, the mapping should be: 60FFh(4bytes)|6072h(2bytes)|fill(4bytes)|fill(4bytes)|fill(4bytes)|fill(4bytes)|fill(2bytes). Since there are 4 motors in total, 4 * (4 + 2) = 24 bytes are needed to control all motors in one frame, so 18 bytes of fill objects need to be placed after.
Similarly for motor two, it should be: fill(4bytes)|fill(2bytes)|60FFh(4bytes)|6072h(2bytes)|fill(4bytes)|fill(4bytes)|fill(4bytes). And so on.
Save PDO Mapping
PDO Mapping can be saved to avoid re-setting every time on startup. Please refer to object 1010h for details. After saving, you only need to send NMT commands on each startup to directly start control.
MIT Control and Velocity Control
For Powered Castor Wheel, being pushable by humans is basically a must. In many cases, you want the chassis to be freely pushable under specific conditions. At this time, you only need to limit the motor's maximum output torque to 0. All of this only requires using velocity control. But if you want to add some force control, such as dynamic/static friction compensation, etc., you can only use MIT control mode.
It's also worth mentioning that there's no restriction that all motors must use the same mode, so you can freely combine velocity control and MIT control, as long as you ensure your PDO mapping is normal.
Velocity Mode:
- Short data, effectively reduces CAN bus occupancy.
- Combined with 6072h, can achieve force-position hybrid control, allowing the chassis to be pushed. In force-position hybrid control, a single motor requires 6 bytes, and 64 bytes can easily drive 4 steering wheel modules (8 motors)
- Cannot achieve force control
MIT Control Mode:
- Longer data, will significantly increase CAN bus occupancy. If CAN bus signal quality is too poor, it will cause communication failure.
- No need to use with 6072h, just set Kd to 0 directly.
- Can achieve force control, such as dynamic/static friction compensation, etc.
- Length can be 16 bytes or 10 bytes depending on the situation. In either case, 2 COB-IDs are needed to drive all 4 steering wheel modules. Nevertheless, using 10-byte length can effectively reduce CAN bus occupancy, so it is still recommended.
About MIT Control
A single MIT object is 16 bytes long, of which 4 bytes of target position and 2 bytes of Kp, if not used, it is recommended that you directly remove them from the mapping. This can further reduce CAN bus occupancy.
Stall Error
When the motor continuously outputs at peak torque, it will trigger a stall error. If you want to completely avoid stall errors, you need to limit the motor's maximum output torque to 80% of peak torque. That is, write 800 to 6072h.
Control Testing
Please Operate with Caution
Before starting motor control, please ensure the surrounding environment is safe. Since repeatedly sending heartbeat packets for a node in the terminal is not particularly convenient, both examples provided here do not use heartbeat timeout detection.
SDO Control Testing
About SDO Control Testing
In fact, you should use PDO to write objects such as 60FFh, but here we first use SDO for CAN communication demonstration.
Please Operate with Caution
Before starting motor control, please ensure the surrounding environment is safe
Before starting, we power on the motor again to ensure the motor is in its initial state. Assume the motors to be controlled are 0x01 and 0x02. We will use velocity mode for demonstration. Motor 1's torque will be limited to 5% of peak torque, and motor 2 will be limited to 80% of peak torque.
1 w 0x6060 0 i8 3
2 w 0x6060 0 i8 3
1 w 0x60FF 0 i32 0
2 w 0x60FF 0 i32 0
1 w 0x6040 0 u16 6
2 w 0x6040 0 u16 6
1 w 0x6072 0 u16 50
2 w 0x6072 0 u16 800
After all the above writes are completed, write control word 7
Finally, write 0x0f
At this point, the motor is enabled, with a target of 0 Rev/s. Write the speed in Q21 units. Here we prepare to test at 0.1 Rev/s.
0.1 * 2^21 = 209715
At this point, both motors start rotating at 0.1 Rev/s, and the two motors have different peak torques. One motor with less resistance will stop when applied a small force, while the other requires a larger force to stop.
PDO Control Testing
Obviously, using SDO communication wastes bandwidth significantly. PDO should be used for control whenever possible, but as mentioned earlier, PDO mapping needs to be adjusted according to the actual number of motors being operated. Therefore, this demo still only demonstrates controlling 4 motors. For other cases, please adjust according to the actual situation.
Adjust PDO Mapping
To achieve one-frame control of multiple motors, the original RPDO COB-ID cannot meet the requirements. Therefore, not only do we need to adjust the Mapping of all controlled motors, but also adjust the RPDO COB-ID.
Taking velocity control with torque limit as an example, each motor needs to use 6072h (Max Torque) and 60FFh (Target Velocity). Therefore, each motor needs six bytes. There are 4 motors in total, so 24 bytes are needed.
For the sending program, the order of all 24 bytes is as follows:
4-byte fill object is 3000h 03h, 2-byte fill object is 3000h 02h
Motor 1 Max Torque (2 Bytes) | Motor 1 Target Velocity (4 Bytes) | Motor 2 Max Torque (2 Bytes) | Motor 2 Target Velocity (4 Bytes) | Motor 3 Max Torque (2 Bytes) | Motor 3 Target Velocity (4 Bytes) | Motor 4 Max Torque (2 Bytes) | Motor 4 Target Velocity (4 Bytes)
For Motor 1, its RPDO Mapping should be:
Max Torque 6072h | Target Velocity 60FFh | 4-byte fill | 4-byte fill | 4-byte fill | 4-byte fill | 2-byte fill
For Motor 2, its RPDO Mapping should be:
4-byte fill | 2-byte fill | Max Torque 6072h | Target Velocity 60FFh | 4-byte fill | 4-byte fill | 4-byte fill
For Motor 3, its RPDO Mapping should be:
4-byte fill | 4-byte fill | 4-byte fill | Max Torque 6072h | Target Velocity 60FFh | 4-byte fill | 2-byte fill
For Motor 4, its RPDO Mapping should be:
4-byte fill | 4-byte fill | 4-byte fill | 4-byte fill | 2-byte fill | Max Torque 6072h | Target Velocity 60FFh
About Fill Objects
There's no need to stick to this form. Thanks to the flexible nature of PDO, you can arrange it freely. If the target velocity actually needs higher update frequency while max torque doesn't, you can completely put all max torques into RPDO2, sending target velocity at high frequency and max torque at low frequency.
There's also no need to force all motors to be in the same control mode. It's perfectly fine to have some motors in velocity control mode and some using MIT control mode. As long as the Mapping is correct.
About Configuration Automation and Persistence
Remember, essentially canopend just sends some CAN messages. You can completely automate the PDO configuration process. For the specific format of the SDO protocol, please refer to the abundant internet tutorials.
In addition, these configurations will revert to default settings after each power-on unless saved through object 1010h. If you confirm that these configurations won't change, just save them through object 1010h to achieve configuration persistence. Subsequent power-ons won't require reconfiguration, only sending NMT commands and selecting modes, operating control words.
Here we need to choose a COB-ID as the RPDO1 COB-ID for all motors. Here we choose the TPDO1 COB-ID of node 0x10 (i.e., 0x190). You can choose any CAN-ID, as long as you ensure it won't conflict with other nodes. It's recommended to choose the master station's TPDO1, 2, 3, or 4.
- First write
0x8000_0000 | 0x190to 1400h 01h to disable RPDO1 - Set 1400h 02h transmission type to 255
- Set 1600 00h RPDO valid mapping count to 0
- Set 1600 01h application object 1 to 4-byte fill 3000h 03h
- Set 1600 02h application object 2 to 2-byte fill 3000h 02h
- Set 1600 03h application object 3 to Max Torque 6072h 00h
- Set 1600 04h application object 4 to Target Velocity 60FFh 00h
- Set 1600 05h application object 5 to 4-byte fill 3000h 03h
- Set 1600 06h application object 6 to 4-byte fill 3000h 03h
- Set 1600 07h application object 7 to 4-byte fill 3000h 03h
- Set 1600 00h valid mapping count to 7
- Finally write
0x0000_0000 | 0x190to 1400h 01h to enable RPDO1
At this point, RPDO configuration is complete. Next, enter 2 start to send NMT commands, putting the motor into CiA301 Operational state.
But at this point, control via PDO is still not possible because the CiA402 state machine hasn't been configured yet. Since this only needs to be configured once on power-up, we'll also use SDO for demonstration.
- Write
0x80to 6040h 00h to clear errors - Write
0x03to 6060h 00h to set motor to velocity control mode - Write
0x06to 6040h 00h Shutdown - Write
0x07to 6040h 00h Switch On - Write
0x0fto 6040h 00h Operation Enable
At this point, the motor can start to be controlled via PDO. Below is an example of limiting all motors' max torque to 80% and target speed to 1 Rev/s.
If all motors are correctly configured, all motors will start running at this point.
Object Dictionary
Only provide parts different from CiA301 and CiA402. Due to distribution issues, we cannot provide CiA301.pdf and CiA402.pdf. Please use a search engine to find relevant content.
Software Version
By reading 1018h 03h we can obtain the motor firmware version. Object dictionaries may differ between firmware versions, please note.
Version Change History
- 0x07
- First publicly released version