Skip to content

Commit df2fb78

Browse files
GodeffroyGodeffroy
committed
Add Dynamixel AX12 Servo Example
1 parent 80fdc6f commit df2fb78

6 files changed

Lines changed: 613 additions & 1 deletion

File tree

‎examples/Example8_AX12/Example8_AX12.cpp‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
/*
2+
Start the serial communication at default baud rate speed : 921600
3+
SERIAL_DEBUG is the Serial port used for communication with the PC
4+
You can redefine it to change the Serial port used
5+
*/
6+
7+
#include "ESP32_Helper.h"
8+
using namespace Printer;
9+
10+
void setup(void)
11+
{
12+
delay(2000);
13+
ESP32_Helper::Initialisation();
14+
int8_t rxPin = 26;
15+
int8_t txPin = 27;
16+
int8_t dirPin = 14;
17+
18+
ServoAX12::Initialisation(Serial1, rxPin, txPin, dirPin);
19+
20+
ServoAX12::AddServo(1, "Up", 0, 300);
21+
ServoAX12::AddServo(2, "Down", 0, 300);
22+
ServoAX12::AddServo(11, "Left", 0, 300);
23+
ServoAX12::AddServo(12, "Right", 0, 300);
24+
}
25+
26+
int32_t cpt = 0;
27+
28+
void loop(void)
29+
{
30+
/*
31+
ServoAX12::SetServoPosition(1, 100);
32+
while (ServoAX12::IsServoMoving(1))
33+
{
34+
//ServoAX12::PrintPosition();
35+
ServoAX12::TeleplotPosition();
36+
delay(5);
37+
}
38+
ServoAX12::SetServoPosition(1, 200);
39+
while (ServoAX12::IsServoMoving(1))
40+
{
41+
//ServoAX12::PrintPosition();
42+
ServoAX12::TeleplotPosition();
43+
delay(5);
44+
}*/
45+
delay(3000);
46+
print(".");
47+
ServoAX12::Scan(ServoAX12::DxlProtocolVersion::PROTOCOL_1, BaudRate::BAUD_RATE_1000000);
48+
49+
//ServoAX12::UpdateAllServo();
50+
//ServoAX12::TeleplotPosition();
51+
}

‎include/ESP32_Helper.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
#include "Printer.h"
2929
#include "Debugger.h"
3030
#include "FileSystem_Helper.h"
31+
#include "ServoAX12.h"
3132

3233
namespace ESP32_Helper
3334
{

‎include/ServoAX12.h‎

Lines changed: 138 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,138 @@
1+
#ifndef SERVO_AX12_H
2+
#define SERVO_AX12_H
3+
4+
#include <Dynamixel2Arduino.h>
5+
#include <unordered_map>
6+
7+
#include "ESP32_Helper.h"
8+
9+
namespace ServoAX12
10+
{
11+
// https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/tree/master
12+
13+
constexpr size_t MAX_BAUD = 5;
14+
const BaudRate dxlBaud[MAX_BAUD] = {BaudRate::BAUD_RATE_57600,
15+
BaudRate::BAUD_RATE_115200,
16+
BaudRate::BAUD_RATE_1000000,
17+
BaudRate::BAUD_RATE_2000000,
18+
BaudRate::BAUD_RATE_3000000};
19+
20+
enum class DxlProtocolVersion
21+
{
22+
PROTOCOL_1 = 1,
23+
PROTOCOL_2 = 2
24+
};
25+
26+
constexpr size_t MAX_PROTOCOL = 2;
27+
const DxlProtocolVersion dxlProtocol[MAX_PROTOCOL] = {DxlProtocolVersion::PROTOCOL_1,
28+
DxlProtocolVersion::PROTOCOL_2};
29+
30+
// id vitesse acceleration position command_position ledState
31+
/**
32+
* @brief Structure représentant l'état d'un servo moteur.
33+
* @param id Identifiant du servo moteur.
34+
* @param vitesse Vitesse maximale du servo moteur en degrés par seconde.
35+
* @param positionMin Position minimale du servo moteur.
36+
* @param positionMax Position maximale du servo moteur.
37+
* @param command_position Position cible du servo moteur.
38+
* @param ledState État de la LED du servo moteur (allumée en mouvement, éteinte
39+
* sinon).
40+
*/
41+
struct ServoMotion
42+
{
43+
uint8_t id;
44+
String name;
45+
float position;
46+
uint16_t positionMin;
47+
uint16_t positionMax;
48+
float command_position;
49+
bool IsMoving;
50+
bool ledState;
51+
52+
ServoMotion()
53+
{
54+
id = (uint8_t)DXL_BROADCAST_ID;
55+
name = "";
56+
position = 0;
57+
positionMin = 0;
58+
positionMax = 0;
59+
command_position = 0;
60+
IsMoving = false;
61+
ledState = false;
62+
}
63+
64+
/**
65+
* @brief Construct a new Servo Motion object
66+
*
67+
* @param _id Identifiant du servo moteur
68+
* @param _vitesse Vitesse maximale du servo moteur en degrés par seconde
69+
* @param _acceleration Accélération maximale du servo moteur en degrés par
70+
* seconde carrée
71+
*/
72+
ServoMotion(uint8_t _id,
73+
String _name,
74+
uint16_t _positionMin,
75+
uint16_t _positionMax)
76+
{
77+
// Initialisation des valeurs
78+
id = _id;
79+
name = _name;
80+
position = 0;
81+
positionMin = _positionMin;
82+
positionMax = _positionMax;
83+
command_position = 0;
84+
IsMoving = false;
85+
ledState = false;
86+
}
87+
88+
bool operator==(const ServoMotion &other) const
89+
{
90+
return id == other.id;
91+
}
92+
};
93+
94+
void Initialisation(HardwareSerial &serial, int8_t rxPin, int8_t txPin, int8_t dirPin, BaudRate baudRate = BaudRate::BAUD_RATE_1000000, DxlProtocolVersion dxlProtocolVersion = DxlProtocolVersion::PROTOCOL_1);
95+
96+
[[noreturn]] void TaskUpdateServo(void *pvParameters);
97+
98+
void InitAllServo();
99+
void InitServo(ServoMotion &servo);
100+
101+
void AddServo(uint8_t id, String name, uint16_t positionMin, uint16_t positionMax);
102+
103+
void StopAllServo();
104+
void StopServo(ServoMotion &servo);
105+
106+
void StartAllServo();
107+
void StartServo(ServoMotion &servo);
108+
109+
void UpdateAllServo();
110+
void UpdateServo(ServoMotion &servo);
111+
112+
bool AreAllServoMoving();
113+
bool IsServoMoving(uint8_t id);
114+
115+
void SetServoPosition(uint8_t id, float position);
116+
117+
void HandleCommand(Command cmd);
118+
const void PrintCommandHelp();
119+
int16_t Scan();
120+
int16_t Scan(DxlProtocolVersion _protocol, BaudRate _dxlBaud);
121+
void PrintDxlInfo(uint8_t id = DXL_BROADCAST_ID);
122+
123+
void TeleplotPosition();
124+
void PrintPosition();
125+
} // namespace ServoAX12
126+
127+
namespace std
128+
{
129+
template <> struct hash<ServoAX12::ServoMotion>
130+
{
131+
std::size_t operator()(const ServoAX12::ServoMotion &k) const
132+
{
133+
// Hash only the name, since operator== only compares id
134+
return std::hash<uint8_t>()(k.id);
135+
}
136+
};
137+
} // namespace std
138+
#endif

‎platformio.ini‎

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -94,4 +94,15 @@ build_flags =
9494
-std=gnu++17
9595
-D NO_WIFI
9696
-D SIMULATOR
97-
-D WOKWI
97+
-D WOKWI
98+
99+
[env:Example8_AX12]
100+
build_unflags =
101+
-std=gnu++11
102+
build_flags =
103+
-std=c++17
104+
-std=gnu++17
105+
-D NO_WIFI
106+
-D SIMULATOR
107+
lib_deps =
108+
robotis-git/Dynamixel2Arduino@^0.8.0

‎src/ESP32_Helper.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -163,6 +163,8 @@ namespace ESP32_Helper
163163
}
164164
else if (cmdTmp.cmd.startsWith("SPIFFS"))
165165
FileSystem_Helper::HandleCommand(cmdTmp);
166+
else if (cmdTmp.cmd.startsWith("AX12"))
167+
ServoAX12::HandleCommand(cmdTmp);
166168
else
167169
{
168170
// If command is not for Lib, we send it to the main

0 commit comments

Comments
 (0)