Skip to content

Commit ecccd3b

Browse files
samuko-thingssamuko-things
committed
update serial comm code for two and four motor support
1 parent dbfb2fa commit ecccd3b

5 files changed

Lines changed: 313 additions & 96 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,11 @@
11
cmake_minimum_required(VERSION 3.16)
22
project(epmc_cpp LANGUAGES CXX)
33

4+
include_directories(include)
45

6+
add_executable( two_motor_control examples/two_motor_control.cpp )
7+
target_link_libraries( two_motor_control serial )
58

6-
include_directories(include)
7-
add_executable( motor_control examples/motor_control.cpp )
8-
target_link_libraries( motor_control serial )
9+
add_executable( four_motor_control examples/four_motor_control.cpp )
10+
target_link_libraries( four_motor_control serial )
911

‎README.md‎

Lines changed: 68 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -6,29 +6,6 @@ This library helps communicate with the **`Easy PID Motor Controller Module`** (
66
A simple way to get started is simply to try out and follow the example code in the example folder
77

88

9-
## How to Use the .deb Package
10-
11-
#### Prequisite
12-
- ensure you've already set up your microcomputer or PC system with ROS2
13-
14-
- download and install the epmc-serial-dev pkg. you can also check the [release](https://github.com/robocre8/epmc_serial_cpp/releases/)
15-
16-
[PC (AMD64)](https://github.com/robocre8/epmc_serial_cpp/tree/amd64-build)
17-
```shell
18-
wget https://github.com/robocre8/epmc_serial_cpp/releases/download/v1.1.1/epmc-serial-dev_1.1.1_amd64.deb
19-
```
20-
```shell
21-
sudo apt install ./epmc-serial-dev_1.1.1_amd64.deb
22-
```
23-
[Raspberry Pi (ARM64)](https://github.com/robocre8/epmc_serial_cpp/tree/arm64-build)
24-
```shell
25-
wget https://github.com/robocre8/epmc_serial_cpp/releases/download/v1.1.1/epmc-serial-dev_1.1.1_arm64.deb
26-
```
27-
```shell
28-
sudo apt install ./epmc-serial-dev_1.1.1_arm64.deb
29-
```
30-
31-
329
## How to Use the Library (build from source)
3310
- install the libserial-dev package
3411
> sudo apt-get update
@@ -57,29 +34,35 @@ sudo apt install ./epmc-serial-dev_1.1.1_arm64.deb
5734
> mkdir build (i.e create a folder named build)
5835
>
5936
> enter the following command in the terminal in the root folder:
60-
````
37+
````shell
6138
cmake -B ./build/
6239
````
63-
````
40+
````shell
6441
cmake --build ./build/
6542
````
43+
````shell
44+
./build/two_motor_control
6645
````
67-
./build/motor_control
46+
````shell
47+
./build/four_motor_control
6848
````
6949

70-
- You can follow the pattern used in the example `motor_control.cpp` in your own code.
71-
50+
- You can follow the pattern used in the example `two_motor_control.cpp` and `four_motor_control.cpp` in your own code.
7251

7352

74-
75-
## Basic Library functions and usage
53+
## Basic Library functions and usage (Two Motor Support Control)
7654

7755
- connect to epmc_driver shield module
7856
> epmc_serial::EPMCSerialClient controller
7957
>
80-
> controller.connect("port_name or port_path")
58+
> _//ensure you set/call **supportedNumOfMotors()** before **connect()** as below:_
8159
>
82-
> controller.clearDataBuffer() # returns bool -> success
60+
> controller.supportedNumOfMotors(epmc_serial::SupportedNumOfMotors::TWO);
61+
>
62+
> controller.connect("port_name or port_path")
63+
64+
- clear speed, position, e.t.c data buffer on the EPMC module
65+
> controller.clearDataBuffer() // returns bool -> success
8366

8467
- send target angular velocity command
8568
> controller.writeSpeed(motor0_TargetVel, motor1_TargetVel)
@@ -91,14 +74,62 @@ sudo apt install ./epmc-serial-dev_1.1.1_arm64.deb
9174
> controller.setCmdTimeout(timeout_ms)
9275

9376
- get motor command timeout
94-
> controller.getCmdTimeout() # returns std::tuple -> (success, motor_command_timeout_ms): bool, float
77+
> controller.getCmdTimeout() // returns std::tuple -> bool, float
78+
79+
- read motors angular position and angular velocity
80+
> controller.readMotorData() // returns std::tuple -> bool, std::vector<float> (pos0, pos1, vel0, vel1)
9581

9682
- read motors angular position
97-
> controller.readPos() # returns std::tuple -> (success, angPos0, angPos1): bool, float, float
83+
> controller.readPos() // returns std::tuple -> bool, std::vector<float> (pos0, pos1)
9884

9985
- read motors angular velocity
100-
> controller.readVel() # returns std::tuple -> (success, angVel0, angVel1): bool, float, float
86+
> controller.readVel() // returns std::tuple -> bool, std::vector<float> (vel0, vel1)
10187

102-
- read motorA maximum commandable angular velocity
103-
> controller.getMaxVel(motor_no) # returns std::tuple -> (success, max_vel): bool, float, float
88+
- read motor maximum commandable angular velocity
89+
> controller.getMaxVel(motor_no) // returns std::tuple -> bool, float
10490
> maxVel0 or maxVel1 based on the specified motor number
91+
92+
- while these function above help communicate with the already configure EPMC module, more examples of advanced funtions usage for parameter tuning can be found in the [epmc_setup_application](https://github.com/robocre8/epmc_setup_application) source code
93+
94+
#
95+
96+
## Basic Library functions and usage (Four Motor Support Control)
97+
98+
- connect to epmc_driver shield module
99+
> epmc_serial::EPMCSerialClient controller
100+
>
101+
> _//ensure you set/call **supportedNumOfMotors()** before **connect()** as below:_
102+
>
103+
> controller.supportedNumOfMotors(epmc_serial::SupportedNumOfMotors::FOUR)
104+
>
105+
> controller.connect("port_name or port_path")
106+
107+
- clear speed, position, e.t.c data buffer on the EPMC module
108+
> controller.clearDataBuffer() // returns bool -> success
109+
110+
- send target angular velocity command
111+
> controller.writeSpeed(motor0_TargetVel, motor1_TargetVel, motor2_TargetVel, motor3_TargetVel)
112+
113+
- send PWM command
114+
> controller.writePWM(motor0_PWM, motor1_PWM, motor2_PWM, motor3_PWM)
115+
116+
- set motor command timeout
117+
> controller.setCmdTimeout(timeout_ms)
118+
119+
- get motor command timeout
120+
> controller.getCmdTimeout() // returns std::tuple -> bool, float
121+
122+
- read motors angular position and angular velocity
123+
> controller.readMotorData() // returns std::tuple -> bool, std::vector<float> (pos0, pos1, pos2, pos3, vel0, vel1, vel2, vel3)
124+
125+
- read motors angular position
126+
> controller.readPos() // returns std::tuple -> bool, std::vector<float> (pos0, pos1, pos2, pos3)
127+
128+
- read motors angular velocity
129+
> controller.readVel() // returns std::tuple -> bool, std::vector<float> (vel0, vel1, vel2, vel3)
130+
131+
- read motor maximum commandable angular velocity
132+
> controller.getMaxVel(motor_no) // returns std::tuple -> bool, float
133+
> maxVel0 or maxVel1 based on the specified motor number
134+
135+
- while these function above help communicate with the already configure EPMC module, more examples of advanced funtions usage for parameter tuning can be found in the [epmc_setup_application](https://github.com/robocre8/epmc_setup_application) source code

examples/motor_control.cpp renamed to examples/four_motor_control.cpp

Lines changed: 22 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99

1010
#include "epmc_serial.hpp"
1111

12-
epmc_serial::EPMCSerialClient controller(epmc_serial::SupportedNumOfMotors::FOUR);
12+
epmc_serial::EPMCSerialClient controller;
1313

1414
void delay_ms(unsigned long milliseconds)
1515
{
@@ -19,7 +19,8 @@ void delay_ms(unsigned long milliseconds)
1919
int main(int argc, char **argv)
2020
{
2121
// variable for communication
22-
bool success; float val0, val1, val2, val3, val4, val5, val6, val7;
22+
bool success; float val0;
23+
std::vector<float> val;
2324

2425
float pos0=0.0, pos1=0.0, pos2=0.0, pos3=0.0;
2526
float vel0=0.0, vel1=0.0, vel2=0.0, vel3=0.0;
@@ -41,6 +42,7 @@ int main(int argc, char **argv)
4142
std::string serial_port = "/dev/ttyUSB0";
4243
int serial_baudrate = 115200;
4344
int serial_timeout_ms = 18; // value < 20ms (50 Hz comm)
45+
controller.supportedNumOfMotors(epmc_serial::SupportedNumOfMotors::FOUR);
4446
controller.connect(serial_port, serial_baudrate, serial_timeout_ms);
4547

4648
success = controller.clearDataBuffer();
@@ -88,29 +90,27 @@ int main(int argc, char **argv)
8890
if (readDuration.count() > readTimeInterval)
8991
{
9092

91-
// controller.writeSpeed(v, v);
92-
// std::tie(success, val0, val1, val2, val3, val4, val5, val6, val7) = controller.readMotorData();
93-
std::tie(success, val0, val1, val2, val3) = controller.readPos();
93+
// controller.writeSpeed(v, v, v, v);
94+
std::tie(success, val) = controller.readMotorData();
9495
if (success) { // only update if read was successfull
95-
pos0 = val0; pos1 = val1; pos2 = val2; pos3 = val3;
96+
pos0 = val.at(0); pos1 = val.at(1); pos2 = val.at(2); pos3 = val.at(3);
97+
vel0 = val.at(4); vel1 = val.at(5); vel2 = val.at(6); vel3 = val.at(7);
98+
99+
std::cout << "----------------------------------" << std::endl;
100+
std::cout << "motor0_readings: [" << pos0 << "," << vel0 << "]" << std::endl;
101+
std::cout << "motor1_readings: [" << pos1 << "," << vel1 << "]" << std::endl;
102+
std::cout << "motor2_readings: [" << pos2 << "," << vel2 << "]" << std::endl;
103+
std::cout << "motor3_readings: [" << pos3 << "," << vel3 << "]" << std::endl;
104+
std::cout << "----------------------------------" << std::endl;
105+
std::cout << std::endl;
96106
}
97-
else{
98-
std::cout << "error1" << std::endl;
107+
else {
108+
std::cout << "----------------------------------" << std::endl;
109+
std::cout << "error reading motor data" << std::endl;
110+
std::cout << "----------------------------------" << std::endl;
111+
std::cout << std::endl;
99112
}
100-
std::tie(success, val0, val1, val2, val3) = controller.readVel();
101-
if (success) { // only update if read was successfull
102-
vel0 = val0; vel1 = val1; vel2 = val2; vel3 = val3;
103-
}
104-
else{
105-
std::cout << "error2" << std::endl;
106-
}
107-
std::cout << "----------------------------------" << std::endl;
108-
std::cout << "motor0_readings: [" << pos0 << "," << vel0 << "]" << std::endl;
109-
std::cout << "motor1_readings: [" << pos1 << "," << vel1 << "]" << std::endl;
110-
std::cout << "motor2_readings: [" << pos2 << "," << vel2 << "]" << std::endl;
111-
std::cout << "motor3_readings: [" << pos3 << "," << vel3 << "]" << std::endl;
112-
std::cout << "----------------------------------" << std::endl;
113-
std::cout << std::endl;
113+
114114

115115
readTime = std::chrono::system_clock::now();
116116
}

‎examples/two_motor_control.cpp‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
2+
#include <sstream>
3+
#include <iostream>
4+
#include <unistd.h>
5+
6+
#include <chrono>
7+
8+
#include <iomanip>
9+
10+
#include "epmc_serial.hpp"
11+
12+
epmc_serial::EPMCSerialClient controller;
13+
14+
void delay_ms(unsigned long milliseconds)
15+
{
16+
usleep(milliseconds * 1000);
17+
}
18+
19+
int main(int argc, char **argv)
20+
{
21+
// variable for communication
22+
bool success; float val0;
23+
std::vector<float> val;
24+
25+
float pos0=0.0, pos1=0.0;
26+
float vel0=0.0, vel1=0.0;
27+
28+
// [4 rev/sec, 2 rev/sec, 1 rev/sec, 0.5 rev/sec]
29+
float targetVel[] = {1.571, 3.142, 6.284, 12.568}; // in rad/sec
30+
float vel = targetVel[1]; // in rad/sec
31+
float v = 0.0;
32+
33+
auto cmdTime = std::chrono::system_clock::now();
34+
std::chrono::duration<double> cmdDuration;
35+
float cmdTimeInterval = 5.0;
36+
37+
auto readTime = std::chrono::system_clock::now();
38+
std::chrono::duration<double> readDuration;
39+
float readTimeInterval = 0.02; // 50Hz
40+
41+
// 50Hz comm setup
42+
std::string serial_port = "/dev/ttyACM0";
43+
int serial_baudrate = 115200;
44+
int serial_timeout_ms = 18; // value < 20ms (50 Hz comm)
45+
controller.supportedNumOfMotors(epmc_serial::SupportedNumOfMotors::TWO);
46+
controller.connect(serial_port, serial_baudrate, serial_timeout_ms);
47+
48+
success = controller.clearDataBuffer();
49+
controller.writeSpeed(v, v);
50+
51+
int motor_cmd_timeout_ms = 10000;
52+
controller.setCmdTimeout(motor_cmd_timeout_ms); // set motor command timeout
53+
std::tie(success, val0) = controller.getCmdTimeout();
54+
if (success) { // only update if read was successfull
55+
motor_cmd_timeout_ms = val0;
56+
std::cout << "motor command timeout: " << motor_cmd_timeout_ms << " ms" << std::endl;
57+
} else {
58+
std::cerr << "ERROR: could not read motor command timeout" << std::endl;
59+
}
60+
61+
bool sendHigh = true;
62+
63+
cmdTime = std::chrono::system_clock::now();
64+
readTime = std::chrono::system_clock::now();
65+
66+
while (true)
67+
{
68+
69+
cmdDuration = (std::chrono::system_clock::now() - cmdTime);
70+
if (cmdDuration.count() > cmdTimeInterval)
71+
{
72+
if (sendHigh)
73+
{
74+
v = vel;
75+
controller.writeSpeed(v, v);
76+
vel *= -1;
77+
sendHigh = false;
78+
}
79+
else
80+
{
81+
v = 0.0;
82+
controller.writeSpeed(v, v);
83+
sendHigh = true;
84+
}
85+
86+
cmdTime = std::chrono::system_clock::now();
87+
}
88+
89+
readDuration = (std::chrono::system_clock::now() - readTime);
90+
if (readDuration.count() > readTimeInterval)
91+
{
92+
93+
// controller.writeSpeed(v, v);
94+
std::tie(success, val) = controller.readMotorData();
95+
if (success) { // only update if read was successfull
96+
pos0 = val.at(0); pos1 = val.at(1);
97+
vel0 = val.at(2); vel1 = val.at(3);
98+
99+
std::cout << "----------------------------------" << std::endl;
100+
std::cout << "motor0_readings: [" << pos0 << "," << vel0 << "]" << std::endl;
101+
std::cout << "motor1_readings: [" << pos1 << "," << vel1 << "]" << std::endl;
102+
std::cout << "----------------------------------" << std::endl;
103+
std::cout << std::endl;
104+
}
105+
else {
106+
std::cout << "----------------------------------" << std::endl;
107+
std::cout << "error reading motor data" << std::endl;
108+
std::cout << "----------------------------------" << std::endl;
109+
std::cout << std::endl;
110+
}
111+
112+
113+
readTime = std::chrono::system_clock::now();
114+
}
115+
}
116+
}

0 commit comments

Comments
 (0)