Repository navigation
Expand file tree
/
Copy pathODriveArduino.h
More file actions
268 lines (251 loc) · 10.5 KB
/
Copy pathODriveArduino.h
File metadata and controls
268 lines (251 loc) · 10.5 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
#ifndef ODriveArduino_h
#define ODriveArduino_h
#include "Arduino.h"
#include "ODriveEnums.h"
/** @brief Operador de impresión genérico para Stream/Print. */
template <class T>
inline Print &operator<<(Print &obj, T arg)
{
obj.print(arg);
return obj;
}
/** @brief Impresión de float con 4 decimales para comandos ODrive. */
template <>
inline Print &operator<<(Print &obj, float arg)
{
obj.print(arg, 4);
return obj;
}
class ODriveArduino
{
public:
/** @brief Crea la interfaz usando un Stream UART.
* @param serial Puerto serie usado para comunicarse con ODrive.
*/
ODriveArduino(Stream &serial);
/** @name Configuración rápida */
/** @{ */
/** @brief Guarda configuración en memoria no volátil. */
inline void SaveConfig() { serial_ << "ss\n"; }
/** @brief Borra configuración almacenada. */
inline void EraseConfig() { serial_ << "se\n"; }
/** @brief Reinicia la controladora ODrive. */
inline void Reboot() { serial_ << "sr\n"; }
/** @brief Limpia errores activos. */
inline void ClearErrors() { serial_ << "sc\n"; }
/** @brief Envía un comando ASCII directo.
* @param command Cadena de comando sin salto de línea.
*/
inline void SendCommand(const char *command) { serial_ << command << "\n"; }
/** @} */
/** @name Acceso genérico y espera de estado */
/** @{ */
/** @brief Espera estado IDLE de uno o ambos ejes.
* @param motor_number Eje objetivo (0, 1, -1 ambos).
* @param timeout Tiempo máximo en segundos.
*/
void WaitIdle(int motor_number = -1, float timeout = 10.0f);
/** @brief Escribe propiedad de eje (double).
* @param motor_number Eje (0 o 1).
* @param property Ruta relativa a axis (ej: "controller.config.vel_limit").
* @param value Valor a escribir.
*/
void WriteProperty(int motor_number, const char *property, double value);
/** @brief Escribe propiedad de eje (float).
* @param motor_number Eje (0 o 1).
* @param property Ruta relativa a axis.
* @param value Valor a escribir.
*/
void WriteProperty(int motor_number, const char *property, float value);
/** @brief Escribe propiedad de eje (int).
* @param motor_number Eje (0 o 1).
* @param property Ruta relativa a axis.
* @param value Valor a escribir.
*/
void WriteProperty(int motor_number, const char *property, int value);
/** @brief Lee propiedad de eje en double.
* @param motor_number Eje (0 o 1).
* @param property Ruta relativa a axis.
* @param value Puntero donde guardar el valor leído.
*/
void ReadProperty(int motor_number, const char *property, double *value);
/** @brief Lee propiedad de eje en float.
* @param motor_number Eje (0 o 1).
* @param property Ruta relativa a axis.
* @param value Puntero donde guardar el valor leído.
*/
void ReadProperty(int motor_number, const char *property, float *value);
/** @brief Lee propiedad de eje en int.
* @param motor_number Eje (0 o 1).
* @param property Ruta relativa a axis.
* @param value Puntero donde guardar el valor leído.
*/
void ReadProperty(int motor_number, const char *property, int *value);
/** @} */
/** @name Movimiento de un eje */
/** @{ */
/** @brief Comanda posición sin feedforward.
* @param motor_number Eje (0 o 1).
* @param position Setpoint de posición.
*/
void SetPosition(int motor_number, float position);
/** @brief Comanda posición con feedforward de velocidad.
* @param motor_number Eje (0 o 1).
* @param position Setpoint de posición.
* @param velocity_feedforward Feedforward de velocidad.
*/
void SetPosition(int motor_number, float position, float velocity_feedforward);
/** @brief Comanda posición con feedforward completo.
* @param motor_number Eje (0 o 1).
* @param position Setpoint de posición.
* @param velocity_feedforward Feedforward de velocidad.
* @param current_feedforward Feedforward de corriente.
*/
void SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward);
/** @brief Comanda velocidad sin feedforward de corriente.
* @param motor_number Eje (0 o 1).
* @param velocity Setpoint de velocidad.
*/
void SetVelocity(int motor_number, float velocity);
/** @brief Comanda velocidad con feedforward de corriente.
* @param motor_number Eje (0 o 1).
* @param velocity Setpoint de velocidad.
* @param current_feedforward Feedforward de corriente.
*/
void SetVelocity(int motor_number, float velocity, float current_feedforward);
/** @brief Comanda corriente/torque directo.
* @param motor_number Eje (0 o 1).
* @param current Setpoint de corriente.
*/
void SetCurrent(int motor_number, float current);
/** @brief Movimiento trapezoidal a posición objetivo.
* @param motor_number Eje (0 o 1).
* @param position Posición objetivo.
*/
void TrapezoidalMove(int motor_number, float position);
/** @brief Envía velocidad y lee feedback de un eje en la misma transacción.
* @param motor_number Eje (0 o 1).
* @param set_VM Setpoint de velocidad.
* @param velocity_M Puntero de salida para velocidad estimada.
* @param position_M Puntero de salida para posición estimada.
*/
void SetVelocity_GetFeedback(int motor_number, float set_VM, float *velocity_M, float *position_M);
/** @} */
/** @name Movimiento optimizado para dos ejes */
/** @{ */
/** @brief Comanda velocidad de ambos ejes.
* @param velocity_M0 Setpoint de velocidad eje 0.
* @param velocity_M1 Setpoint de velocidad eje 1.
*/
void SetVelocityBoth(float velocity_M0, float velocity_M1);
/** @brief Comanda ambos ejes y lee feedback de ambos.
* @param set_VM0 Setpoint de velocidad eje 0.
* @param set_VM1 Setpoint de velocidad eje 1.
* @param velocity_M0 Salida velocidad eje 0.
* @param position_M0 Salida posición eje 0.
* @param velocity_M1 Salida velocidad eje 1.
* @param position_M1 Salida posición eje 1.
*/
void SetVelocityBoth_GetFeedback(float set_VM0, float set_VM1, float *velocity_M0, float *position_M0, float *velocity_M1, float *position_M1);
/** @brief Comanda ambos ejes, lee feedback y `vbus`.
* @param set_VM0 Setpoint de velocidad eje 0.
* @param set_VM1 Setpoint de velocidad eje 1.
* @param vbus Salida de tensión de bus.
* @param velocity_M0 Salida velocidad eje 0.
* @param position_M0 Salida posición eje 0.
* @param velocity_M1 Salida velocidad eje 1.
* @param position_M1 Salida posición eje 1.
*/
void SetVelocityBoth_GetFeedback_Vbus(float set_VM0, float set_VM1, float *vbus, float *velocity_M0, float *position_M0, float *velocity_M1, float *position_M1);
/** @brief Comanda ambos ejes y lee `vbus`.
* @param set_VM0 Setpoint de velocidad eje 0.
* @param set_VM1 Setpoint de velocidad eje 1.
* @param vbus Salida de tensión de bus.
*/
void SetVelocityBoth_GetVbus(float set_VM0, float set_VM1, float *vbus);
/** @} */
/** @name Getters y feedback */
/** @{ */
/** @brief Lee velocidad estimada de un eje.
* @param motor_number Eje (0 o 1).
* @return Velocidad estimada.
*/
float GetVelocity(int motor_number);
/** @brief Lee posición estimada de un eje.
* @param motor_number Eje (0 o 1).
* @return Posición estimada.
*/
float GetPosition(int motor_number);
/** @brief Lee tensión del bus DC.
* @return Valor de `vbus_voltage`.
*/
float GetVbus();
/** @brief Lee posición y velocidad de un eje.
* @param motor_number Eje (0 o 1).
* @param velocity_M Salida velocidad estimada.
* @param position_M Salida posición estimada.
*/
void GetFeedback(int motor_number, float *velocity_M, float *position_M);
/** @brief Lee feedback de ambos ejes.
* @param velocity_M0 Salida velocidad eje 0.
* @param position_M0 Salida posición eje 0.
* @param velocity_M1 Salida velocidad eje 1.
* @param position_M1 Salida posición eje 1.
*/
void GetFeedbackBoth(float *velocity_M0, float *position_M0, float *velocity_M1, float *position_M1);
/** @brief Lee feedback de un eje y tensión de bus.
* @param motor_number Eje (0 o 1).
* @param velocity_M Salida velocidad estimada.
* @param position_M Salida posición estimada.
* @param vbus Salida tensión de bus.
*/
void GetFeedback_Vbus(int motor_number, float *velocity_M, float *position_M, float *vbus);
/** @brief Lee feedback de ambos ejes y tensión de bus.
* @param velocity_M0 Salida velocidad eje 0.
* @param position_M0 Salida posición eje 0.
* @param velocity_M1 Salida velocidad eje 1.
* @param position_M1 Salida posición eje 1.
* @param vbus Salida tensión de bus.
*/
void GetFeedbackBoth_Vbus(float *velocity_M0, float *position_M0, float *velocity_M1, float *position_M1, float *vbus);
/** @} */
/** @name Lectura de bajo nivel */
/** @{ */
/** @brief Lee un valor float de UART.
* @param timeout Timeout en microsegundos.
* @return Valor parseado (0 si no hay dato válido).
*/
float readFloat(unsigned long timeout = 100);
/** @brief Lee un valor entero de UART.
* @param timeout Timeout en microsegundos.
* @return Valor parseado (0 si no hay dato válido).
*/
int32_t readInt(unsigned long timeout = 100);
/** @} */
/** @name Control de estado */
/** @{ */
/** @brief Lee estado actual de un eje.
* @param axis Eje (0 o 1).
* @return Estado actual del eje.
*/
ODriveAxisState read_state(int axis);
/** @brief Solicita estado de eje y opcionalmente espera IDLE.
* @param axis Eje (0 o 1).
* @param requested_state Estado solicitado.
* @param wait_for_idle `true` para esperar hasta IDLE.
* @param timeout Timeout en segundos cuando `wait_for_idle` es `true`.
* @return `true` si termina antes de timeout.
*/
bool run_state(int axis, ODriveAxisState requested_state, bool wait_for_idle, float timeout = 10.0f);
/** @} */
/** @brief Vacía bytes pendientes del buffer RX UART. */
void CleanSerial();
private:
/** @brief Lee token ASCII hasta separador o timeout.
* @param timeout Timeout en microsegundos.
* @return Token leído.
*/
String readString(unsigned long timeout = 500);
Stream &serial_;
};
#endif // ODriveArduino_h