Welcome to RDS Team 2N’s documentation!¶

Indices and tables¶

Doxygen Documentation¶

struct Axis¶: #include <utils.h>

Nested struct for handling axis-based data and states.

struct AxisState¶: #include <utils.h>

Holds constants representing possible ODrive axis states.

class Controller¶

#include <controller.h>

Provides torque control functionality using a PID algorithm.

This class calculates joint torques by comparing current and desired joint states, accumulating joint errors, and adjusting PID gains accordingly.

Public Functions

Controller()¶

Constructs a new Controller object.

Initializes the PID gains and transformation matrix used for converting joint torques to motor torques.

Initializes the PID gains, pulley dimensions, and transformation matrix used for motor torque conversion. Note: D_MOTOR_PULLEY, D_FINGER_PULLEY_1, and D_FINGER_PULLEY_2 should be defined elsewhere.

std::pair<std::vector<double>, std::vector<double>> torque_control(std::vector<double> joint_states, std::vector<double> joint_states_desired, std::vector<double> joint_error_sum, std::vector<double> motor_velocities)¶

Computes motor torques based on PID control.

Computes motor torques using PID control.

Calculates the difference between the desired and current joint states, updates the error sum, and computes the derivative of the error from motor velocities. It then applies a PID control algorithm to determine the joint torques, which are transformed into motor torques.

This method calculates the error between desired and current joint states, updates the accumulated error, and computes the derivative of the error from the motor velocities. Then, it applies a PID algorithm to compute the joint torques and converts these into motor torques using a transformation matrix.

Parameters:

joint_states – Vector of current joint states.
joint_states_desired – Vector of desired joint states.
joint_error_sum – Vector containing the accumulated joint error over time.
motor_velocities – Vector of current motor velocities.
joint_states – Vector of current joint states.
joint_states_desired – Vector of desired joint states.
joint_error_sum – Vector containing the accumulated joint error.
motor_velocities – Vector of current motor velocities.

Returns:

A std::pair consisting of:

A vector of computed motor torques.
The updated joint error sum vector.

Returns:

A std::pair containing:

A vector of computed motor torques.
The updated joint error sum vector.

void check_error(std::vector<double> joint_error)¶

Adjusts PID gains based on the current joint error.

Adjusts the PID gains based on the current joint error.

Depending on whether the joint errors are negative or positive, the PID gains (kp, ki, kd) are updated to use either lower or higher predefined values.

The function selects between low and high PID gains depending on whether each joint error is positive or negative.

Parameters:

joint_error – Vector containing the error for each joint.
joint_error – Vector containing the error for each joint.

Private Members

Finger finger¶: Instance of the Finger class used for finger control.

double low_kp¶: Low and high proportional gains.

double high_kp¶

double low_kd¶: Low and high derivative gains.

double high_kd¶

double low_ki¶: Low and high integral gains.

double high_ki¶

double r¶: Motor and finger pulley dimensions (expected to be defined elsewhere).

double R1¶

double R2¶

std::array<std::array<double, 2>, 2> kp¶: 2x2 matrices for PID gains.

std::array<std::array<double, 2>, 2> kd¶

std::array<std::array<double, 2>, 2> ki¶

std::array<std::array<double, 4>, 2> trans_mat¶: 2x4 transformation matrix converting joint torques to motor torques.

class Finger¶

#include <finger.h>

Provides functionality to convert between motor states and finger joint states.

This class defines methods to compute joint states from motor states and vice versa.

Public Functions

Finger()¶

Constructs a new Finger object.

The constructor currently does not perform additional initialization.

float *getJointStates(const float motor_states[NUM_MOTORS])¶

Computes the finger joint states based on motor states.

Computes the finger joint states from motor states.

Given an array of motor states, this function calculates the positions of the finger joints.

This function calculates the positions of two joints by converting motor states using the specified pulley diameters. The first joint is computed as the difference between two positions (averaged), while the second joint is a combination of four positions.

Parameters:

motor_states – An array of motor states with length NUM_MOTORS.
motor_states – An array of motor states (length NUM_MOTORS).

Returns:

A pointer to an array of computed joint states (length NUM_JOINTS).

Returns:

Pointer to an array containing the computed joint states (length NUM_JOINTS).

float *getMotorCommands(const float joint_inputs[NUM_JOINTS])¶

Computes motor commands based on desired joint inputs.

Given an array of desired joint inputs, this function calculates the motor commands required.

This function derives motor commands from the provided joint inputs. For now, it only uses the first joint input to compute commands for motor 0 and motor 2, with motor 1 and motor 3 set to zero.

Parameters:

joint_inputs – An array of desired joint inputs with length NUM_JOINTS.
joint_inputs – An array of desired joint inputs (length NUM_JOINTS).

Returns:

A pointer to an array of computed motor commands (length NUM_MOTORS).

Returns:

Pointer to an array containing the computed motor commands (length NUM_MOTORS).

Private Members

float _joint_states[NUM_JOINTS]¶: Array storing the current joint states.

float _motor_commands[NUM_MOTORS]¶: Array storing the computed motor commands.

struct ODrive¶

#include <utils.h>

Namespace-like struct containing ODrive axis state values.

The ODrive struct is used to hold various enumerations that represent different axis states in an ODrive motor driver.

class R1806¶

#include <r1806.h>

Provides methods to encode and decode CAN messages for controlling an R1806 motor via ODrive.

This class contains utility functions to extract node/command IDs, encode state and torque commands, and decode feedback such as position and velocity.

Public Functions

R1806()¶

Constructs a new R1806 object.

Constructs a new R1806 object and sets the gear reduction ratio.

Initializes internal parameters, including the gear reduction ratio.

uint32_t extract_node_id(uint32_t can_id)¶

Extract the node ID from a given CAN identifier.

Extract the node ID from a CAN ID.

Node ID occupies bits [10..5] of a standard 11-bit ID. We mask and shift to retrieve it.

Parameters:

can_id – 29-bit or 11-bit CAN identifier.
can_id – The raw CAN identifier.

Returns:

Node ID (extracted bits from can_id).

Returns:

Extracted node ID.

uint32_t extract_command_id(uint32_t can_id)¶

Extract the command ID from a given CAN identifier.

Extract the command ID from a CAN ID.

Command ID occupies bits [4..0] of the CAN identifier. We mask out the lower 5 bits to retrieve it.

Parameters:

can_id – 29-bit or 11-bit CAN identifier.
can_id – The raw CAN identifier.

Returns:

Command ID (extracted bits from can_id).

Returns:

Extracted command ID.

CAN_message_t encodeRequestedStateCommand(uint32_t node_id, uint32_t requested_state)¶

Encode a “requested state” command for ODrive.

Encode a “requested state” command into a CAN message.

Creates a CAN message instructing ODrive to enter a specific axis state.

This implementation forces the state to CLOSED_LOOP_CONTROL for demonstration. You can modify it to use the passed-in requested_state argument fully.

Parameters:

node_id – ID of the node (motor) on the CAN bus.
requested_state – The requested axis state (e.g., CLOSED_LOOP_CONTROL).
node_id – The target node (motor) ID.
requested_state – The desired axis state (not fully used in this example).

Returns:

A CAN_message_t struct with the encoded command.

Returns:

A CAN_message_t configured with the appropriate ID and payload.

CAN_message_t encodeTorqueCommand(uint32_t node_id, float torque)¶

Encode a torque command for the ODrive.

Encode a torque command into a CAN message.

Creates a CAN message to set a torque value for a given node. The torque is automatically adjusted by the gear reduction ratio.

The gear reduction ratio is factored into the torque command so that the motor sees the correct torque value. The 4-byte float is written in little-endian format into the CAN frame buffer.

Parameters:

node_id – ID of the node (motor) on the CAN bus.
torque – Desired torque in Nm (or consistent units).
node_id – The target node (motor) ID.
torque – Desired torque before gear reduction is accounted for.

Returns:

A CAN_message_t struct with the encoded torque command.

Returns:

A CAN_message_t with the correct ID and payload to set torque on ODrive.

float decodePositionFeedback(CAN_message_t frame)¶

Decode position feedback from a CAN message.

Interprets the first 4 bytes of the CAN message payload as a float for the number of revolutions, then converts it to a position (radians) factoring in the gear reduction ratio.

The first 4 bytes of the CAN frame are interpreted as a float representing the number of revolutions. This is then converted to radians, factoring in the gear reduction ratio.

Parameters:

frame – A CAN_message_t containing position feedback data.
frame – A CAN_message_t containing position data in its first 4 bytes.

Returns:

Position in radians.

Returns:

Position in radians.

float decodeVelocityFeedback(CAN_message_t frame)¶

Decode velocity feedback from a CAN message.

Interprets bytes [4..7] of the CAN message payload as a float for the number of revolutions per second, then converts it to a velocity (rad/s), factoring in the gear reduction ratio.

Bytes [4..7] of the CAN frame are interpreted as a float representing the number of revolutions per second, then converted to rad/s with gear reduction considered.

Parameters:

frame – A CAN_message_t containing velocity feedback data.
frame – A CAN_message_t containing velocity data in bytes [4..7].

Returns:

Velocity in rad/s.

Returns:

Velocity in rad/s.

Private Members

float _gear_reduction_ratio¶: Internal gear reduction ratio used for torque and feedback calculations.

class SerialProtocols¶

#include <serial_protocols.h>

Provides functions to encode and decode serial packets for feedback and joint commands.

This class includes methods to compute packet checksums, encode feedback packets, and decode joint command packets received via serial communication.

Public Functions

SerialProtocols()¶: Constructs a new SerialProtocols object.

uint8_t computeChecksum(uint8_t msgType, uint8_t payloadLen, const uint8_t *payload)¶

Computes a checksum for a serial packet.

The checksum is computed as the 8-bit sum of the message type, payload length, and each byte of the payload.

The checksum is calculated by summing the message type, payload length, and every byte in the payload. The result is truncated to an 8-bit value.

Parameters:

msgType – The message type byte.
payloadLen – The length of the payload.
payload – Pointer to the payload data.
msgType – The message type.
payloadLen – Length of the payload.
payload – Pointer to the payload data.

Returns:

Computed 8-bit checksum.

Returns:

An 8-bit checksum value.

uint8_t *encodeFeedbackPacket(const float positions[NUM_JOINTS])¶

Encodes a feedback packet from joint positions.

The feedback packet is constructed using a start byte, message type, payload length, the payload containing two float positions, a checksum, and an end byte.

The packet is structured as follows:

Byte 0: START_BYTE
Byte 1: MSG_FEEDBACK
Byte 2: FEEDBACK_PAYLOAD_LENGTH
Bytes 3-10: Payload (2 floats representing positions)
Byte 11: Checksum (computed over bytes 1-10)
Byte 12: END_BYTE

Parameters:

positions – An array of joint positions (2 floats).
positions – An array of two float positions.

Returns:

Pointer to the encoded feedback packet.

Returns:

Pointer to the encoded feedback packet.

float *decodeJointCommandPacket(const uint8_t *data)¶

Decodes a joint command packet.

This method validates the packet structure (start byte, message type, payload length, checksum, and end byte) and decodes the payload into joint command values.

The expected packet layout is:

Byte 0: START_BYTE
Byte 1: MSG_JOINT_COMMAND
Byte 2: JOINT_COMMAND_PAYLOAD_LENGTH
Bytes 3-10: Payload (4 float position commands, 16 bytes total)
Byte 11: Checksum (computed over bytes 1-10)
Byte 12: END_BYTE

The method validates each part of the packet and returns a pointer to the decoded joint commands. If any validation fails, the method returns nullptr.

Parameters:

data – Pointer to the received packet data.
data – Pointer to the received packet data.

Returns:

Pointer to the decoded joint command array, or nullptr if validation fails.

Returns:

Pointer to the decoded joint command array, or nullptr if the packet is invalid.

Private Members

uint8_t _encoder_feedback_packet[FEEDBACK_PACKET_SIZE]¶: Buffer for storing the encoded feedback packet.

uint8_t _joint_command_packet[JOINT_COMMAND_PACKET_SIZE]¶: Buffer for storing the received joint command packet.

float _joint_command[NUM_MOTORS]¶: Array to hold decoded joint commands (size NUM_MOTORS).

file controller.h: #include <stdint.h>

#include “finger/finger.h”

#include <iostream>

#include <vector>

#include <array>

#include <chrono>

#include <math.h>

Declaration of the Controller class for torque control.

file finger.h

#include <stdint.h>

Declaration of the Finger class and related constants.

Defines

NUM_JOINTS¶: Number of finger joints.

NUM_MOTORS¶: Number of motors controlling the finger.

D_MOTOR_PULLEY¶: Diameter of the motor pulley in millimeters.

D_FINGER_PULLEY_1¶: Diameter of the first finger pulley in millimeters.

D_FINGER_PULLEY_2¶: Diameter of the second finger pulley in millimeters.

file r1806.h

#include <FlexCAN_T4.h>

Motor protocols for the R1806 motor used with ODrive Pro.

The R1806 motor is fabricated by Repeat Robotics, and this file defines the R1806 class for CAN-based communication with ODrive.

R1806 motor details: https://repeat-robotics.com/buy/repeat-compact-1806
ODrive CAN Protocol Details: https://docs.odriverobotics.com/v/latest/manual/can-protocol.html

Defines

GEAR_REDUCION_RATIO¶: Gear reduction ratio for the R1806 motor.

file utils.h

#include <stdint.h>

Contains utility data structures and definitions for motor protocols.

Variables

uint32_t UNDEFINED = 0¶: Undefined state.

uint32_t IDLE = 1¶: Idle state.

uint32_t STARTUP_SEQUENCE = 2¶: Startup sequence in progress.

uint32_t FULL_CALIBRATION_SEQUENCE = 3¶: Full calibration sequence.

uint32_t MOTOR_CALIBRATION = 4¶: Motor calibration.

uint32_t ENCODER_INDEX_SEARCH = 6¶: Encoder index search.

uint32_t ENCODER_OFFSET_CALIBRATION = 7¶: Encoder offset calibration.

uint32_t CLOSED_LOOP_CONTROL = 8¶: Closed-loop control.

uint32_t LOCKIN_SPIN = 9¶: Lock-in spin.

uint32_t ENCODER_DIR_FIND = 10¶: Encoder direction find.

uint32_t HOMING = 11¶: Homing procedure.

uint32_t ENCODER_HALL_POLARITY_CALIBRATION = 12¶: Hall polarity calibration.

uint32_t ENCODER_HALL_PHASE_CALIBRATION = 13¶: Hall phase calibration.

uint32_t ANTICOGGING_CALIBRATION = 14¶: Anti-cogging calibration.

struct AxisState

struct Axis

struct ODrive

file utils.h

#include <stdint.h>

Defines constants used in the serial communication protocols.

Defines

NUM_MOTORS: Number of motors controlled via the serial protocol.

START_BYTE¶: Special byte indicating the start of a packet.

END_BYTE¶: Special byte indicating the end of a packet.

MSG_FEEDBACK¶: Message type for feedback packets.

FEEDBACK_PAYLOAD_LENGTH¶: Payload length for feedback packets (contains 2 floats).

FEEDBACK_PACKET_SIZE¶

Total packet size for feedback packets.

Calculated as: START_BYTE + MSG_FEEDBACK + payload length + checksum + END_BYTE.

MSG_JOINT_COMMAND¶: Message type for joint command packets.

JOINT_COMMAND_PAYLOAD_LENGTH¶: Payload length for joint command packets (contains 2 floats).

JOINT_COMMAND_PACKET_SIZE¶

Total packet size for joint command packets.

Calculated as: START_BYTE + MSG_JOINT_COMMAND + payload length + checksum + END_BYTE.

file serial_protocols.h

#include <stdint.h>

#include “serial_protocols/utils.h”

Declaration of the SerialProtocols class for handling serial communication.

Defines

NUM_JOINTS: Number of joints used in the serial communication protocol.

file controller.cpp: #include <controller/controller.h>

file finger.cpp: #include <string.h>

#include “finger/finger.h”

file main.cpp

#include <FlexCAN_T4.h>

#include <math.h>

#include “motor_protocols/r1806.h”

#include “motor_protocols/utils.h”

#include “serial_protocols/serial_protocols.h”

#include “serial_protocols/utils.h”

#include “finger/finger.h”

#include “controller/controller.h”

Main entry point for the motor control and serial feedback system.

This file sets up CAN communication, motor and serial protocols, and implements the main control loop. It integrates functionalities from various modules such as motor protocols (R1806), serial protocols, finger kinematics, and a Controller for torque control.

Functions

void canSniff(const CAN_message_t &msg)¶

Callback function for received CAN messages.

This function processes incoming CAN messages by extracting the motor ID and command ID, updating motor position and velocity states, and applying calibration offsets when needed.

Parameters:: msg – The received CAN message.

void calibrate()¶

Calibrates motor position offsets.

Sends a static torque command for a short duration to settle the system, then records the current motor positions as offsets. After calibration, the system adjusts subsequent motor position readings.

void setup(void)¶

Initializes the system.

Sets up Serial communication, configures the CAN bus (baud rate, RX/TX pins, FIFO, etc.), registers the CAN receive callback, sends closed-loop control requests, and initiates calibration.

void loop()¶

Main control loop.

Processes incoming CAN messages and serial commands, performs torque control using a PID controller, updates joint states, checks safety conditions (e.g., velocity limits), and sends feedback packets over Serial.

Variables

FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can1¶: CAN bus object.

CAN_message_t msg¶: Temporary CAN message storage.

R1806 r1806_protocols¶: R1806 motor protocol handler.

SerialProtocols serial_protocols¶: Serial protocol handler.

Finger finger¶: Finger kinematics handler.

Controller controller¶: Controller for torque control.

float *joint_states¶: Pointer to the current joint states.

float motor_offsets[4] = {0.0, 0.0, 0.0, 0.0}¶: Offsets for motor positions.

float print[2] = {0.0, 0.0}¶: Array used for serial feedback (positions).

float *joint_commands¶: Pointer to decoded joint command values.

float motor_position_states[4] = {0.0, 0.0, 0.0, 0.0}¶: Current motor position states.

float motor_velocity_states[4] = {0.0, 0.0, 0.0, 0.0}¶: Current motor velocity states.

float motor_position_offsets[4] = {0.0, 0.0, 0.0, 0.0}¶: Calibration offsets for motor positions.

float joint_targets[2] = {0.0, 0.0}¶: Desired joint targets.

float static_torque = 0.065¶: Static torque used during calibration.

float velocity_limit = 50.0¶: Velocity limit for safety.

bool calibrated = false¶: Flag indicating whether calibration is complete.

std::vector<double> motor_states = {0.0, 0.0, 0.0, 0.0}¶: Current motor positions (std::vector format).

std::vector<double> motor_torques = {0.0, 0.0, 0.0, 0.0}¶: Computed motor torques.

std::vector<double> joint_error_sum = {0.0, 0.0}¶: Accumulated joint error sum.

std::vector<double> joint_states_desired = {0.0, 0.0}¶: Desired joint states.

std::vector<double> motor_velocities = {0.0, 0.0, 0.0, 0.0}¶: Current motor velocities.

uint8_t *packet¶: Pointer to a serial packet.

static uint32_t timeout = millis()¶: Timeout marker for CAN message sending.

int danger_mode = false¶: Flag indicating if danger mode is active.

bool received_joint_commands = false¶: Flag indicating if valid joint commands were received.

float j1_input = 0.0¶: Raw input for joint 1.

float j2_input = 0.0¶: Raw input for joint 2.

CAN_message_t motor1_torque_cmd = r1806_protocols.encodeTorqueCommand(1, 0.0)¶

CAN_message_t motor1_close_loop_request_cmd = r1806_protocols.encodeRequestedStateCommand(1, ODrive.Axis.AxisState.CLOSED_LOOP_CONTROL)¶

CAN_message_t motor2_torque_cmd = r1806_protocols.encodeTorqueCommand(1, 0.0)¶

CAN_message_t motor2_close_loop_request_cmd = r1806_protocols.encodeRequestedStateCommand(2, ODrive.Axis.AxisState.CLOSED_LOOP_CONTROL)¶

CAN_message_t motor3_torque_cmd = r1806_protocols.encodeTorqueCommand(1, 0.0)¶

CAN_message_t motor3_close_loop_request_cmd = r1806_protocols.encodeRequestedStateCommand(3, ODrive.Axis.AxisState.CLOSED_LOOP_CONTROL)¶

CAN_message_t motor4_torque_cmd = r1806_protocols.encodeTorqueCommand(1, 0.0)¶

CAN_message_t motor4_close_loop_request_cmd = r1806_protocols.encodeRequestedStateCommand(4, ODrive.Axis.AxisState.CLOSED_LOOP_CONTROL)¶

const uint32_t POSITION_TIMEOUT_MS = 100¶: Maximum allowed delay for receiving joint commands.

uint32_t lastpositionCmdTime = millis()¶: Timestamp of the last valid joint command received.

file r1806.cpp: #include <cmath>

#include <cstring>

#include “motor_protocols/r1806.h”

#include “motor_protocols/utils.h”

file serial_protocols.cpp: #include <stdint.h>

#include <string.h>

#include “serial_protocols/serial_protocols.h”

dir include/controller

dir src/controller

dir include/finger

dir src/finger

dir include

dir include/motor_protocols

dir src/motor_protocols

dir include/serial_protocols

dir src/serial_protocols

dir src