// Script to read from and command to all 6 motors in Real-Time followed
// by sending to host-pc throuh UDP Packets
#include <iostream>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <SPI.h>
// Includes needed for Ethernet
#include <Ethernet.h>
#include <EthernetUdp.h>
// Includes needed for motor controller
#include <ODriveArduino.h>
// The baudrate must match the baudrate configured on the ODrive.
// Note that the default clock speed for Teensy 4.1 is 2Mhz but can reach 600MHz without cooling.
// Therefore max theoretical baudrate without cooling on Teensy side is 37500000
// Min clock period for SPI to read encoder is 100 ns which implies max baudrate to be 625000
// Therefore max theoretical baudrate is 625000
float baudrate = 625000; //max theoretical is 625000
// Visit Pjrc website for serial pin inputs // RX (ODrive TX), TX (ODrive RX)
HardwareSerial& odrive_serial1 = Serial1;
HardwareSerial& odrive_serial2 = Serial2;
HardwareSerial& odrive_serial3 = Serial3;
HardwareSerial& odrive_serial4 = Serial5;
HardwareSerial& odrive_serial5 = Serial7;
HardwareSerial& odrive_serial6 = Serial8;
// Initialize Odrive Object
ODriveArduino odrive1(odrive_serial1);
ODriveArduino odrive2(odrive_serial2);
ODriveArduino odrive3(odrive_serial3);
ODriveArduino odrive4(odrive_serial4);
ODriveArduino odrive5(odrive_serial5);
ODriveArduino odrive6(odrive_serial6);
// ******************************//
// Homing Parameters
//*******************************//
//float Motor_home[6] = {1.5708, 1.5708, 1.5708, 0.7854, 0.7854, 1.5708}; // [Rad] - Home Positions at Joint Level - TEMP
float Motor_home[6] = {1.0472, 1.3962, 1.5708, 1.1519, 1.0035, 1.5708}; // [Rad] - for Right Arm
// Motors 1, 2, 3 (upper 5-bar), 4,5,6 (lower 5-bar)
float Motor_offset[6] = {3.14159, 3.14159, 1.5708, 1.5708, 1.5708, 1.5708}; // [Rad] - offset to match kinematic model in MATLAB
float Capstan_Ratio = 20.0 / 80.0; // Gear Ratio For motors 1,2,4 and 5
float Belt_Ratio = 16.43 / 68.0; // Gear Ratio (MISUMI Belts) For moto 3 and 6
float Encoder_Ratio = 6.2832; // [rad/turn]
float Encoder_Home = 0.5; // [Turns] For all 6 motors
double q_enc[6]; // [Turns] - Current Reading at Motor Level
double q_curr[6]; // [Rad] - Current Reading at Joint Level (Output Level)
double Motor_Torque[6];
int Kv = 330.0; // [rpm/V] Specified to D5312S Motor
double Torque_Const = 8.27 / Kv; //[Nm/A]
// ******************************//
// Flags //
// Flag to Enable Closed Loop Mode for all motors. If 0 disabled.
int enable_CL_mode = 0;
// ******************************//
// Ethernet vars
//*******************************//
byte mac[] = {0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xFE};
IPAddress ip(192, 168, 1, 135); // local ip - Teensy 192.168.1.134 for left arm
IPAddress sendIP(192, 168, 1, 101); // remote ip - Host PC
unsigned int localPort = 2003; // local port to listen on
unsigned int sendPort = 2002; // remote port to send to // 2001 for left arm
const int num_values_in_data_array = 6;
const int num_bytes_in_message = num_values_in_data_array * 4; // we are sending doubles (4 bytes per float)
union {
float values[num_values_in_data_array];
char data_buffer[num_bytes_in_message];
} send_data;
union {
float values[num_values_in_data_array];
char data_buffer[num_bytes_in_message];
} recv_data;
// Declare the UDP object
EthernetUDP Udp;
// ************* Void Setup ************** //
void setup() {
odrive_serial1.begin(baudrate);
odrive_serial2.begin(baudrate);
odrive_serial3.begin(baudrate);
odrive_serial4.begin(baudrate);
odrive_serial5.begin(baudrate);
odrive_serial6.begin(baudrate);
Serial.begin(625000);
delay(10);
// Initialize Odrives' Axis State
Serial.println("Waiting for ODrives...");
while (odrive1.getState() == AXIS_STATE_UNDEFINED || odrive2.getState() == AXIS_STATE_UNDEFINED || odrive3.getState() == AXIS_STATE_UNDEFINED
|| odrive4.getState() == AXIS_STATE_UNDEFINED || odrive5.getState() == AXIS_STATE_UNDEFINED || odrive6.getState() == AXIS_STATE_UNDEFINED) {
odrive1.clearErrors();
if (odrive1.getState() != AXIS_STATE_UNDEFINED) {
Serial.println("ODRIVE1 IS ACTIVE");
};
odrive2.clearErrors();
if (odrive2.getState() != AXIS_STATE_UNDEFINED) {
Serial.println("ODRIVE2 IS ACTIVE");
};
odrive3.clearErrors();
if (odrive3.getState() != AXIS_STATE_UNDEFINED) {
Serial.println("ODRIVE3 IS ACTIVE");
};
odrive4.clearErrors();
if (odrive4.getState() != AXIS_STATE_UNDEFINED) {
Serial.println("ODRIVE4 IS ACTIVE");
};
odrive5.clearErrors();
if (odrive5.getState() != AXIS_STATE_UNDEFINED) {
Serial.println("ODRIVE5 IS ACTIVE");
};
odrive6.clearErrors();
if (odrive6.getState() != AXIS_STATE_UNDEFINED) {
Serial.println("ODRIVE6 IS ACTIVE");
};
delay(100);
}
// Check Step - Read DC Voltage for a motor and print it to Serial
// Serial.print("DC voltage motor 1: ");
if (enable_CL_mode == 1) { // Enable Closed Loop Control
Serial.println("Enabling closed loop control...");
while (odrive1.getState() != AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrive1.clearErrors();
odrive1.setState(AXIS_STATE_CLOSED_LOOP_CONTROL); // AXIS_STATE_CLOSED_LOOP_CONTROL
delay(10);
}
while (odrive2.getState() != AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrive2.clearErrors();
odrive2.setState(AXIS_STATE_CLOSED_LOOP_CONTROL);
delay(10);
}
while (odrive3.getState() != AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrive3.clearErrors();
odrive3.setState(AXIS_STATE_CLOSED_LOOP_CONTROL);
delay(10);
}
while (odrive4.getState() != AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrive4.clearErrors();
odrive4.setState(AXIS_STATE_CLOSED_LOOP_CONTROL);
delay(10);
}
while (odrive5.getState() != AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrive5.clearErrors();
odrive5.setState(AXIS_STATE_CLOSED_LOOP_CONTROL);
delay(10);
}
while (odrive6.getState() != AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrive6.clearErrors();
odrive6.setState(AXIS_STATE_CLOSED_LOOP_CONTROL);
delay(10);
}
}
Serial.println("ODrives are running!");
// setup for Ethernet UDP
Ethernet.begin(mac, ip);
Udp.begin(localPort);
delay(100);
Serial.println("UDP port established");
}
void loop() {
// timer.start();
//
//Recieve via UDP
int packetSize = Udp.parsePacket();
// Serial.println("\n Bytes received:");
// Serial.println(packetSize);
Udp.read(recv_data.data_buffer, sizeof(recv_data.data_buffer));
if (packetSize) {
Serial.println("\n Bytes received:");
Serial.println(packetSize);
Serial.println("Data recieved:");
for (int i = 0; i < (packetSize / 4); i++) {
Serial.print(recv_data.values[i]);
Serial.print(" ");
}
}
Serial.println("Motor Torques:");
double Odrive1_Current = odrive1.getParameterAsFloat("axis0.motor.foc.Iq_setpoint");
double Odrive1_Torque = Odrive1_Current * Torque_Const;
Motor_Torque[0] = {Odrive1_Torque};
Serial.print("Odrive1:");
Serial.println(Odrive1_Torque, 3);
Serial.println();
double Odrive2_Current = odrive2.getParameterAsFloat("axis0.motor.foc.Iq_setpoint");
double Odrive2_Torque = Odrive2_Current * Torque_Const;
Motor_Torque[1] = {Odrive2_Torque};
Serial.print("Odrive2:");
Serial.println(Odrive2_Torque, 3);
Serial.println();
double Odrive3_Current = odrive3.getParameterAsFloat("axis0.motor.foc.Iq_setpoint");
double Odrive3_Torque = Odrive3_Current * Torque_Const;
Motor_Torque[2] = {Odrive3_Torque};
Serial.print("Odrive3:");
Serial.println(Odrive3_Torque, 3);
Serial.println();
double Odrive4_Current = odrive4.getParameterAsFloat("axis0.motor.foc.Iq_setpoint");
double Odrive4_Torque = Odrive4_Current * Torque_Const;
Motor_Torque[3] = {Odrive4_Torque};
Serial.print("Odrive4:");
Serial.println(Odrive4_Torque, 3);
Serial.println();
double Odrive5_Current = odrive5.getParameterAsFloat("axis0.motor.foc.Iq_setpoint");
double Odrive5_Torque = Odrive5_Current * Torque_Const;
Motor_Torque[4] = {Odrive5_Torque};
Serial.print("Odrive5:");
Serial.println(Odrive5_Torque, 3);
Serial.println();
double Odrive6_Current = odrive6.getParameterAsFloat("axis0.motor.foc.Iq_setpoint");
double Odrive6_Torque = Odrive6_Current * Torque_Const;
Motor_Torque[5] = {Odrive6_Torque};
Serial.print("Odrive6:");
Serial.println(Odrive6_Torque, 3);
Serial.println();
// ================================================= //
// Get Motor Position
// Feedback for motor 1
ODriveFeedback feedback1 = odrive1.getFeedback();
Serial.print("Odrive1:");
Serial.print(feedback1.pos);
Serial.println(" ");
q_enc[0] = feedback1.pos;
// odrive1.setPosition(q_enc[0]+0.01); // [Turns]
// delay(1000);
// Serial.println(" ");
// Feedback for motor 2
ODriveFeedback feedback2 = odrive2.getFeedback();
Serial.print("Odrive2:");
Serial.print(feedback2.pos);
Serial.println(" ");
q_enc[1] = feedback2.pos;
// odrive2.setPosition(q_enc[1]+0.01); // [Turns]
// Feedback for motor 3
ODriveFeedback feedback3 = odrive3.getFeedback();
Serial.print("Odrive3:");
Serial.print(feedback3.pos);
Serial.println(" ");
q_enc[2] = feedback3.pos;
// Feedback for motor 4
ODriveFeedback feedback4 = odrive4.getFeedback();
Serial.print("Odrive4:");
Serial.print(feedback4.pos);
Serial.println(" ");
q_enc[3] = feedback4.pos;
// Feedback for motor 5
ODriveFeedback feedback5 = odrive5.getFeedback();
Serial.print("Odrive5:");
Serial.print(feedback5.pos);
Serial.println(" ");
q_enc[4] = feedback5.pos;
// Feedback for motor 6
ODriveFeedback feedback6 = odrive6.getFeedback();
Serial.print("Odrive6:");
Serial.print(feedback6.pos);
Serial.println(" ");
q_enc[5] = feedback6.pos;
// ============================================================//
// Find motor Readings with homing and transmission gear ratio
// Calculate output angle from encoder readings (Including belt/gear ratio and encoder ratio)
// Serial.println("Motor Position:");
for (int i = 0; i < 6; i++) {
if (i == 0 || i == 1) { //For motors 1 and 2, subtract from 180 deg (in rad) the motor reading to match kinmatic notations
q_curr[i] = Motor_home[i] + Capstan_Ratio * Encoder_Ratio * (q_enc[i] - Encoder_Home);
q_curr[i] = Motor_offset[i] - q_curr[i];
if (i == 0) {
// Serial.print("Odrive1:");
// Serial.println(q_curr[0]);
// Serial.println(" ");
}
else {
// Serial.print("Odrive2:");
// Serial.println(q_curr[1]);
// Serial.println(" ");
}
}
else if (i == 3 || i == 4) {
q_curr[i] = Motor_home[i] + Capstan_Ratio * Encoder_Ratio * (q_enc[i] - Encoder_Home);
q_curr[i] = q_curr[i] + Motor_offset[i]; // Add 90 deg (in rad) to motors 4 and 5 readings to match kinematic notations
if (i == 3) {
// Serial.print("Odrive4:");
// Serial.println(q_curr[3]);
// Serial.println(" ");
}
else {
// Serial.print("Odrive5:");
// Serial.println(q_curr[4]);
// Serial.println(" ");
}
}
else if (i == 2) {
q_curr[i] = Motor_home[i] + Belt_Ratio * Encoder_Ratio * (q_enc[i] - Encoder_Home);
q_curr[i] = q_curr[i]; // Motor 3 - Subtraction of 90 deg (in rad) from motors 3 and 6 readings is done on MATLAB after including
// the fourbar mechanism ratio
// Serial.print("Odrive3:");
// Serial.println(q_curr[2]);
// Serial.println(" ");
}
else {
q_curr[i] = Motor_home[i] + Belt_Ratio * Encoder_Ratio * (q_enc[i] - Encoder_Home);
q_curr[i] = PI - q_curr[i]; //
// Serial.print("Odrive6:");
// Serial.println(q_curr[5]);
// Serial.println(" ");
}
// Send data back to Matlab via UDP
send_data.values[i] = (float) q_curr[i];
}
//Send Data to remote IP address and port
Udp.beginPacket(sendIP, sendPort);
Udp.write(send_data.data_buffer, sizeof(send_data.data_buffer));
Udp.endPacket();
delay(5);
// timer.stop();
// Serial.print("Elapsed Time:");
// Serial.println(timer.read());
}```
