Skip to content

Commit 27ad1a5

Browse files
GitshaoxiangGitshaoxiang
committed
add ODrive example
1 parent a062719 commit 27ad1a5

File tree

3 files changed

+403
-0
lines changed

3 files changed

+403
-0
lines changed

‎examples/Modules/ODrive/ODrive.ino

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
/*
2+
Description:
3+
This case uses the ODrive module to control the high-speed and precise rotation of the servo motor
4+
Press button C to calibrate (do not touch the motor shaft during this period), long press and short button A to control the motor rotation.
5+
Note: The motor parameter configuration in this case is only applicable to the motor model matched with the M5 Odrive kit. When driving other types of motors,
6+
please configure the parameters according to the motor used.
7+
*/
8+
9+
10+
#include "M5Stack.h"
11+
#include "odrive.h"
12+
13+
ODrive odrive(Serial1);
14+
TFT_eSprite canvas = TFT_eSprite(&M5.Lcd);
15+
16+
void showStringCenter(const char* str, uint16_t color) {
17+
canvas.fillScreen(TFT_BLACK);
18+
canvas.setTextColor(color);
19+
canvas.drawString(str, 160, 120, 4);
20+
canvas.pushSprite(0, 0);
21+
}
22+
23+
void setup() {
24+
M5.begin(true, false, true, true);
25+
canvas.setColorDepth(1);
26+
canvas.createSprite(320, 240);
27+
Serial1.begin(115200, SERIAL_8N1, 13, 5);
28+
29+
canvas.setTextDatum(MC_DATUM);
30+
showStringCenter("ODrive", TFT_GREEN);
31+
}
32+
33+
void loop() {
34+
M5.update();
35+
if (M5.BtnA.wasReleased()) {
36+
odrive.setPosition(10);
37+
}
38+
39+
if (M5.BtnA.wasReleasefor(800)) {
40+
odrive.setPosition(0);
41+
}
42+
43+
if (M5.BtnB.wasReleased()) {
44+
showStringCenter("Save default config", TFT_GREEN);
45+
odrive.setDefaultConfig();
46+
odrive.reboot();
47+
}
48+
49+
if (M5.BtnB.wasReleasefor(800)) {
50+
showStringCenter("Clear config", TFT_GREEN);
51+
odrive.eraseConfig();
52+
}
53+
54+
if (M5.BtnC.wasReleased()) {
55+
showStringCenter("Motor CALIBRATION", TFT_WHITE);
56+
odrive.runState(odrive.AXIS_STATE_MOTOR_CALIBRATION, 10000);
57+
if (odrive.checkError()) {
58+
showStringCenter("Motor CALIBRATION", TFT_RED);
59+
return ;
60+
}
61+
62+
showStringCenter("Encoder CALIBRATION", TFT_WHITE);
63+
odrive.runState(odrive.AXIS_STATE_ENCODER_INDEX_SEARCH, 10000);
64+
if (odrive.checkError()) {
65+
showStringCenter("Encoder Failed", TFT_RED);
66+
return ;
67+
}
68+
69+
showStringCenter("Offset CALIBRATION", TFT_WHITE);
70+
odrive.runState(odrive.AXIS_STATE_ENCODER_OFFSET_CALIBRATION, 10000);
71+
if (odrive.checkError()) {
72+
showStringCenter("Offset Failed", TFT_RED);
73+
return ;
74+
}
75+
76+
odrive.setGain(211.0, 0.0225, 0.01125);
77+
odrive.setControlMode(odrive.CONTROL_MODE_POSITION_CONTROL);
78+
odrive.runState(odrive.AXIS_STATE_CLOSED_LOOP_CONTROL, 2000);
79+
80+
showStringCenter("Finish", TFT_GREEN);
81+
}
82+
83+
char data_show[100];
84+
sprintf(data_show, "%f, %d, %d\r\n", odrive.getVbusVoltage(), odrive.getEncoderShadowCount(), odrive.checkError());
85+
showStringCenter(data_show, TFT_WHITE);
86+
Serial.printf(data_show);
87+
delay(20);
88+
}

‎examples/Modules/ODrive/odrive.cpp

Lines changed: 240 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,240 @@
1+
#include <stdio.h>
2+
#include "odrive.h"
3+
4+
const char *default_config =
5+
"w config.brake_resistance 0\n"
6+
"w config.dc_bus_undervoltage_trip_level 8\n"
7+
"w config.dc_bus_overvoltage_trip_level 34\n"
8+
"w config.dc_max_negative_current -5\n"
9+
"w config.max_regen_current 0\n"
10+
"w axis0.motor.config.pole_pairs 4\n"
11+
"w axis0.motor.config.calibration_current 5\n"
12+
"w axis0.motor.config.resistance_calib_max_voltage 4\n"
13+
"w axis0.motor.config.motor_type 0\n"
14+
"w axis0.motor.config.current_lim 5\n"
15+
"w axis0.motor.config.requested_current_range 20\n"
16+
"w axis0.encoder.config.mode 0\n"
17+
"w axis0.encoder.config.use_index 1\n"
18+
"w axis0.encoder.config.cpr 4000\n"
19+
"w axis0.controller.config.control_mode 3\n"
20+
"w axis0.controller.config.vel_limit 65536000\n"
21+
"w axis0.controller.config.pos_gain 20\n"
22+
"w axis0.motor_thermistor.config.poly_coefficient_0 551.8902587890625\n"
23+
"w axis0.motor_thermistor.config.poly_coefficient_1 -753.2103271484375\n"
24+
"w axis0.motor_thermistor.config.poly_coefficient_2 436.8048095703125\n"
25+
"w axis0.motor_thermistor.config.poly_coefficient_3 -42.3204460144043\n"
26+
"w axis0.motor_thermistor.config.temp_limit_lower 75\n"
27+
"w axis0.motor_thermistor.config.temp_limit_upper 90\n"
28+
"w axis0.motor_thermistor.config.enabled 1\n";
29+
30+
31+
ODrive::ODrive(Stream& serial_in)
32+
: serial(serial_in) {}
33+
34+
void ODrive::setPosition(float position) {
35+
setPosition(position, 0.0f, 0.0f);
36+
}
37+
38+
void ODrive::setPosition(float position, float velocity_feedforward) {
39+
setPosition(position, velocity_feedforward, 0.0f);
40+
}
41+
42+
void ODrive::setPosition(float position, float velocity_feedforward, float current_feedforward) {
43+
serial.printf("p 0 %f %f %f\n", position, velocity_feedforward, current_feedforward);
44+
}
45+
46+
void ODrive::setVelocity(float velocity) {
47+
setVelocity(velocity, 0.0f);
48+
}
49+
50+
void ODrive::setVelocity(float velocity, float current_feedforward) {
51+
serial.printf("v 0 %f %f\n", velocity, current_feedforward);
52+
}
53+
54+
void ODrive::setCurrent(float current) {
55+
serial.printf("c 0 %f\n", current);
56+
}
57+
58+
void ODrive::setGain(float pos, float vel, float vel_integrator) {
59+
writeConfig("axis0.controller.config.pos_gain", pos);
60+
writeConfig("axis0.controller.config.vel_gain", vel);
61+
writeConfig("axis0.controller.config.vel_integrator_gain", vel_integrator);
62+
}
63+
64+
void ODrive::setControlMode(int32_t mode) {
65+
writeConfig("axis0.controller.config.control_mode", mode);
66+
}
67+
68+
void ODrive::setControlInputPos(float pos) {
69+
writeConfig("axis0.controller.input_pos", pos);
70+
}
71+
72+
void ODrive::trapezoidalMove(float position){
73+
serial.printf("t 0 %f\n", position);
74+
}
75+
76+
bool ODrive::runState(int32_t requested_state, uint32_t timeout) {
77+
uint32_t time_start = millis();
78+
int32_t state = -1;
79+
writeConfig("axis0.requested_state", requested_state);
80+
state = readConfigInt("axis0.requested_state");
81+
while (state != requested_state && millis() - time_start < timeout) {
82+
delay(100);
83+
state = readConfigInt("axis0.requested_state");
84+
}
85+
return state == requested_state;
86+
}
87+
88+
float ODrive::getVelocity(){
89+
return readConfigFloat("axis0.encoder.vel_estimate");
90+
}
91+
92+
float ODrive::getVbusVoltage() {
93+
return readConfigFloat("vbus_voltage");
94+
}
95+
96+
float ODrive::getPhaseCurrent() {
97+
return readConfigFloat("axis0.motor.current_control.Iq_measured");
98+
}
99+
100+
float ODrive::getBusCurrent() {
101+
return readConfigFloat("axis0.motor.current_control.Ibus");
102+
}
103+
104+
int32_t ODrive::getEncoderShadowCount() {
105+
return readConfigInt("axis0.encoder.shadow_count");
106+
}
107+
108+
float ODrive::getEncoderPosEstimate() {
109+
return readConfigInt("axis0.encoder.pos_estimate");
110+
}
111+
112+
float ODrive::getMotorTemp() {
113+
return readConfigFloat("axis0.motor_thermistor.temperature");
114+
}
115+
116+
void ODrive::eraseConfig() {
117+
writeToDeive("se\n");
118+
}
119+
120+
void ODrive::saveConfig() {
121+
writeToDeive("ss\n");
122+
}
123+
124+
void ODrive::reboot() {
125+
writeToDeive("sr\n");
126+
}
127+
128+
void ODrive::setDefaultConfig() {
129+
writeToDeive("\n");
130+
writeToDeive(default_config);
131+
saveConfig();
132+
}
133+
134+
bool ODrive::checkError(int32_t* axis, int32_t* motor_thermistor, int32_t* encoder, int32_t* controller) {
135+
int32_t errors[4] = {0, 0, 0, 0};
136+
errors[0] = readConfigInt("axis0.error");
137+
errors[1] = readConfigInt("axis0.motor_thermistor.error");
138+
errors[2] = readConfigInt("axis0.encoder.error");
139+
errors[3] = readConfigInt("axis0.controller.error");
140+
141+
if (axis) {
142+
*axis = errors[0];
143+
}
144+
if (motor_thermistor) {
145+
*motor_thermistor = errors[1];
146+
}
147+
if (encoder) {
148+
*encoder = errors[2];
149+
}
150+
if (controller) {
151+
*controller = errors[3];
152+
}
153+
154+
if (errors[0] || errors[1] || errors[2] || errors[3]) {
155+
return true;
156+
}
157+
158+
return false;
159+
}
160+
161+
void ODrive::readFlush() {
162+
while (serial.available()) {
163+
serial.read();
164+
}
165+
}
166+
167+
String ODrive::readString() {
168+
String str = "";
169+
unsigned long timeout_start = millis();
170+
for (;;) {
171+
while (!serial.available()) {
172+
if (millis() - timeout_start >= READ_TIMEOUT) {
173+
return str;
174+
}
175+
}
176+
char c = serial.read();
177+
if (c == '\n') {
178+
break;
179+
}
180+
str += c;
181+
}
182+
return str;
183+
}
184+
185+
float ODrive::readFloat() {
186+
return readString().toFloat();
187+
}
188+
189+
int32_t ODrive::readInt() {
190+
return readString().toInt();
191+
}
192+
193+
void ODrive::writeToDeive(const char* data) {
194+
if (data == NULL) {
195+
return;
196+
}
197+
serial.write(data);
198+
}
199+
200+
void ODrive::writeConfig(const char* config, float value) {
201+
char *out_string = NULL;
202+
int result = asprintf(&out_string, "w %s %f\n", config, value);
203+
if (result != -1) {
204+
writeToDeive(out_string);
205+
free(out_string);
206+
}
207+
}
208+
209+
void ODrive::writeConfig(const char* config, int32_t value) {
210+
char *out_string = NULL;
211+
int result = asprintf(&out_string, "w %s %d\n", config, value);
212+
if (result != -1) {
213+
writeToDeive(out_string);
214+
free(out_string);
215+
}
216+
}
217+
218+
int32_t ODrive::readConfigInt(const char*config) {
219+
char *out_string = NULL;
220+
int result = asprintf(&out_string, "r %s\n", config);
221+
if (result != -1) {
222+
readFlush();
223+
writeToDeive(out_string);
224+
free(out_string);
225+
}
226+
227+
return readInt();
228+
}
229+
230+
float ODrive::readConfigFloat(const char*config) {
231+
char *out_string = NULL;
232+
int result = asprintf(&out_string, "r %s\n", config);
233+
if (result != -1) {
234+
readFlush();
235+
writeToDeive(out_string);
236+
free(out_string);
237+
}
238+
239+
return readFloat();
240+
}

0 commit comments

Comments
 (0)