Skip to content

Commit 0cc228f

Browse files
actions-useractions-user
committed
autosync
1 parent 0550a59 commit 0cc228f

File tree

3 files changed

+292
-39
lines changed

3 files changed

+292
-39
lines changed

‎src/ODriveCAN.cpp‎

Lines changed: 127 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -3,51 +3,135 @@
33
// Documentation: https://docs.odriverobotics.com/v/latest/guides/arduino-can-guide.html
44

55
#include "ODriveCAN.h"
6+
67
#include <Arduino.h> // needed for debug printing
78

89
bool ODriveCAN::clearErrors() {
9-
Clear_Errors_msg_t clear_errors_msg;
10-
return send(clear_errors_msg);
10+
Clear_Errors_msg_t msg;
11+
return send(msg);
1112
}
1213

1314
bool ODriveCAN::setPosition(float position, float velocity_feedforward, float torque_feedforward) {
14-
Set_Input_Pos_msg_t input_pos_msg;
15-
input_pos_msg.Input_Pos = position;
16-
input_pos_msg.Vel_FF = velocity_feedforward;
17-
input_pos_msg.Torque_FF = torque_feedforward;
18-
return send(input_pos_msg);
15+
Set_Input_Pos_msg_t msg;
16+
17+
msg.Input_Pos = position;
18+
msg.Vel_FF = velocity_feedforward;
19+
msg.Torque_FF = torque_feedforward;
20+
21+
return send(msg);
1922
}
2023

2124
bool ODriveCAN::setVelocity(float velocity, float torque_feedforward) {
22-
Set_Input_Vel_msg_t input_vel_msg;
23-
input_vel_msg.Input_Vel = velocity;
24-
input_vel_msg.Input_Torque_FF = torque_feedforward;
25-
return send(input_vel_msg);
25+
Set_Input_Vel_msg_t msg;
26+
27+
msg.Input_Vel = velocity;
28+
msg.Input_Torque_FF = torque_feedforward;
29+
30+
return send(msg);
2631
}
2732

2833
bool ODriveCAN::setControllerMode(uint8_t control_mode, uint8_t input_mode) {
29-
Set_Controller_Mode_msg_t ctrl_mode_msg;
30-
ctrl_mode_msg.Control_Mode = control_mode;
31-
ctrl_mode_msg.Input_Mode = input_mode;
32-
return send(ctrl_mode_msg);
34+
Set_Controller_Mode_msg_t msg;
35+
36+
msg.Control_Mode = control_mode;
37+
msg.Input_Mode = input_mode;
38+
39+
return send(msg);
3340
}
3441

3542
bool ODriveCAN::setTorque(float torque) {
36-
Set_Input_Torque_msg_t input_torque_msg;
37-
input_torque_msg.Input_Torque = torque;
38-
return send(input_torque_msg);
39-
}
43+
Set_Input_Torque_msg_t msg;
44+
45+
msg.Input_Torque = torque;
4046

41-
bool ODriveCAN::trapezoidalMove(float position) {
42-
Set_Input_Pos_msg_t input_pos_msg;
43-
input_pos_msg.Input_Pos = position;
44-
return send(input_pos_msg);
47+
return send(msg);
4548
}
4649

4750
bool ODriveCAN::setState(ODriveAxisState requested_state) {
48-
Set_Axis_State_msg_t axist_state_msg;
49-
axist_state_msg.Axis_Requested_State = requested_state;
50-
return send(axist_state_msg);
51+
Set_Axis_State_msg_t msg;
52+
53+
msg.Axis_Requested_State = (uint32_t)requested_state;
54+
55+
return send(msg);
56+
}
57+
58+
bool ODriveCAN::setLimits(float velocity_limit, float current_soft_max) {
59+
Set_Limits_msg_t msg;
60+
61+
msg.Velocity_Limit = velocity_limit;
62+
msg.Current_Limit = current_soft_max;
63+
64+
return send(msg);
65+
}
66+
67+
bool ODriveCAN::setPosGain(float pos_gain) {
68+
Set_Pos_Gain_msg_t msg;
69+
70+
msg.Pos_Gain = pos_gain;
71+
72+
return send(msg);
73+
}
74+
75+
bool ODriveCAN::setVelGains(float vel_gain, float vel_integrator_gain) {
76+
Set_Vel_Gains_msg_t msg;
77+
78+
msg.Vel_Gain = vel_gain;
79+
msg.Vel_Integrator_Gain = vel_integrator_gain;
80+
81+
return send(msg);
82+
}
83+
84+
bool ODriveCAN::setAbsolutePosition(float abs_pos) {
85+
Set_Absolute_Position_msg_t msg;
86+
87+
msg.Position = abs_pos;
88+
89+
return send(msg);
90+
}
91+
92+
bool ODriveCAN::setTrapezoidalVelLimit(float vel_limit) {
93+
Set_Traj_Vel_Limit_msg_t msg;
94+
95+
msg.Traj_Vel_Limit = vel_limit;
96+
97+
return send(msg);
98+
}
99+
100+
bool ODriveCAN::setTrapezoidalAccelLimits(float accel_limit, float decel_limit) {
101+
Set_Traj_Accel_Limits_msg_t msg;
102+
103+
msg.Traj_Accel_Limit = accel_limit;
104+
msg.Traj_Decel_Limit = decel_limit;
105+
106+
return send(msg);
107+
}
108+
109+
bool ODriveCAN::getCurrents(Get_Iq_msg_t& msg, uint16_t timeout_ms) {
110+
return request(msg, timeout_ms);
111+
}
112+
113+
bool ODriveCAN::getTemperature(Get_Temperature_msg_t& msg, uint16_t timeout_ms) {
114+
return request(msg, timeout_ms);
115+
}
116+
117+
bool ODriveCAN::getError(Get_Error_msg_t& msg, uint16_t timeout_ms) {
118+
return request(msg, timeout_ms);
119+
}
120+
121+
bool ODriveCAN::getVersion(Get_Version_msg_t& msg, uint16_t timeout_ms) {
122+
return request(msg, timeout_ms);
123+
}
124+
125+
bool ODriveCAN::getFeedback(Get_Encoder_Estimates_msg_t& msg, uint16_t timeout_ms) {
126+
return request(msg, timeout_ms);
127+
}
128+
129+
bool ODriveCAN::getBusVI(Get_Bus_Voltage_Current_msg_t& msg, uint16_t timeout_ms) {
130+
return request(msg, timeout_ms);
131+
}
132+
133+
bool ODriveCAN::getPower(Get_Powers_msg_t& msg, uint16_t timeout_ms) {
134+
return request(msg, timeout_ms);
51135
}
52136

53137
void ODriveCAN::onReceive(uint32_t id, uint8_t length, const uint8_t* data) {
@@ -57,31 +141,38 @@ void ODriveCAN::onReceive(uint32_t id, uint8_t length, const uint8_t* data) {
57141
Serial.print(" id: 0x");
58142
Serial.println(id, HEX);
59143
Serial.print(" data: 0x");
60-
while (byte_index >= 0) Serial.print(msg.data[byte_index--], HEX);
144+
while (byte_index >= 0)
145+
Serial.print(msg.data[byte_index--], HEX);
61146
Serial.println("");
62147
#endif // DEBUG
63-
if (node_id_ != (id >> ODriveCAN::kNodeIdShift)) return;
64-
switch(id & ODriveCAN::kCmdIdBits) {
148+
if (node_id_ != (id >> ODriveCAN::kNodeIdShift))
149+
return;
150+
switch (id & ODriveCAN::kCmdIdBits) {
65151
case Get_Encoder_Estimates_msg_t::cmd_id: {
66152
Get_Encoder_Estimates_msg_t estimates;
67153
estimates.decode_buf(data);
68-
if (feedback_callback_) feedback_callback_(estimates, feedback_user_data_);
154+
if (feedback_callback_)
155+
feedback_callback_(estimates, feedback_user_data_);
69156
break;
70157
}
71158
case Heartbeat_msg_t::cmd_id: {
72159
Heartbeat_msg_t status;
73160
status.decode_buf(data);
74-
if (axis_state_callback_ != nullptr) axis_state_callback_(status, axis_state_user_data_);
75-
else Serial.println("missing callback");
161+
if (axis_state_callback_ != nullptr)
162+
axis_state_callback_(status, axis_state_user_data_);
163+
else
164+
Serial.println("missing callback");
76165
break;
77166
}
78167
default: {
79-
if (requested_msg_id_ == REQUEST_PENDING) return;
168+
if (requested_msg_id_ == REQUEST_PENDING)
169+
return;
80170
#ifdef DEBUG
81171
Serial.print("waiting for: 0x");
82172
Serial.println(requested_msg_id_, HEX);
83173
#endif // DEBUG
84-
if ((id & ODriveCAN::kCmdIdBits) != requested_msg_id_) return;
174+
if ((id & ODriveCAN::kCmdIdBits) != requested_msg_id_)
175+
return;
85176
memcpy(buffer_, data, length);
86177
requested_msg_id_ = REQUEST_PENDING;
87178
}
@@ -92,7 +183,8 @@ bool ODriveCAN::awaitMsg(uint16_t timeout) {
92183
uint64_t start_time = millis();
93184
while (requested_msg_id_ != REQUEST_PENDING) {
94185
can_intf_.pump_events(); // pump event loop while waiting
95-
if ((millis() - start_time) > 1000 * timeout) return false;
186+
if ((millis() - start_time) > 1000 * timeout)
187+
return false;
96188
}
97189
return true;
98190
}

0 commit comments

Comments
 (0)