Line data Source code
1 : /*!
2 : * @file PeripheralController.cpp
3 : * @brief Implementation of the PeripheralController class.
4 : * @version 0.1
5 : * @date 2025-02-12
6 : * @details This file contains the implementation of the PeripheralController class,
7 : * which is responsible for controlling the peripherals of the car.
8 : * @author Félix LE BIHAN (@Fle-bihh)
9 : * @author Tiago Pereira (@t-pereira06)
10 : * @author Ricardo Melo (@reomelo)
11 : * @author Michel Batista (@MicchelFAB)
12 : *
13 : * @copyright Copyright (c) 2025
14 : */
15 :
16 : #include "PeripheralController.hpp"
17 :
18 : /*!
19 : * @union i2c_smbus_data
20 : * @brief Represents data formats for I2C SMBus communication.
21 : * - `byte`: Single byte of data.
22 : * - `word`: 16-bit (2 byte) data.
23 : * - `block`: Array for up to 34-byte block transfers.
24 : */
25 : union i2c_smbus_data {
26 : uint8_t byte;
27 : uint16_t word;
28 : uint8_t block[34]; // Block size for SMBus
29 : };
30 :
31 : /* ------------------------------------ */
32 :
33 : #define I2C_SMBUS_WRITE 0
34 : #define I2C_SMBUS_READ 1
35 : #define I2C_SMBUS_BYTE_DATA 2
36 :
37 : /*!
38 : * @brief Clamps a value to a given range.
39 : *
40 : * @param value Value to be clamped.
41 : * @param min_val Minimum value of the range.
42 : * @param max_val Maximum value of the range.
43 : * @return The clamped value, or the original value if it is within the range.
44 : */
45 0 : template <typename T> T clamp(T value, T min_val, T max_val) {
46 0 : return (value < min_val) ? min_val : ((value > max_val) ? max_val : value);
47 : }
48 :
49 : /*!
50 : * @brief Constructor for the PeripheralController class.
51 : *
52 : * @param servo_addr The address of the servo controller.
53 : * @param motor_addr The address of the motor controller.
54 : *
55 : * @details Initializes the I2C buses and sets the device addresses.
56 : * Throws an exception if either the servo or motor controller cannot be opened
57 : * or if the address cannot be set.
58 : */
59 0 : PeripheralController::PeripheralController(int servo_addr, int motor_addr)
60 0 : : servo_addr_(servo_addr), motor_addr_(motor_addr) {
61 : // Initialize I2C buses
62 0 : servo_bus_fd_ = open("/dev/i2c-1", O_RDWR);
63 0 : motor_bus_fd_ = open("/dev/i2c-1", O_RDWR);
64 :
65 0 : if (servo_bus_fd_ < 0 || motor_bus_fd_ < 0) {
66 0 : throw std::runtime_error("Failed to open I2C device");
67 : return;
68 : }
69 :
70 : // Set device addresses
71 0 : if (ioctl(servo_bus_fd_, I2C_SLAVE, servo_addr_) < 0 ||
72 0 : ioctl(motor_bus_fd_, I2C_SLAVE, motor_addr_) < 0) {
73 0 : throw std::runtime_error("Failed to set I2C address");
74 : return;
75 : }
76 : }
77 :
78 : /*!
79 : * @brief Destructor for the PeripheralController class.
80 : *
81 : * @details Closes the file descriptors for the servo and motor I2C buses,
82 : * ensuring that resources are properly released when the object is
83 : * destroyed.
84 : */
85 0 : PeripheralController::~PeripheralController() {
86 0 : close(servo_bus_fd_);
87 0 : close(motor_bus_fd_);
88 0 : }
89 :
90 : /*!
91 : * @brief Writes a byte of data to a specific register.
92 : *
93 : * @param file The file descriptor for the I2C bus.
94 : * @param command The register address to write to.
95 : * @param value The byte of data to write to the register.
96 : * @return The result of the write operation (0 on success, -1 on failure).
97 : * @throws std::runtime_error if the I2C write operation fails.
98 : */
99 0 : int PeripheralController::i2c_smbus_write_byte_data(int file, uint8_t command,
100 : uint8_t value) {
101 : union i2c_smbus_data data;
102 0 : data.byte = value;
103 :
104 : struct i2c_smbus_ioctl_data args;
105 0 : args.read_write = I2C_SMBUS_WRITE;
106 0 : args.command = command;
107 0 : args.size = I2C_SMBUS_BYTE_DATA;
108 0 : args.data = &data;
109 :
110 0 : return ioctl(file, I2C_SMBUS, &args);
111 : }
112 :
113 : /*!
114 : * @brief Reads a byte of data from a specific register.
115 : *
116 : * @param file The file descriptor of the I2C bus.
117 : * @param command The register address to read from.
118 : * @return The byte of data read from the register, or -1 if the operation fails.
119 : */
120 0 : int PeripheralController::i2c_smbus_read_byte_data(int file, uint8_t command) {
121 : union i2c_smbus_data data;
122 :
123 : struct i2c_smbus_ioctl_data args;
124 0 : args.read_write = I2C_SMBUS_READ;
125 0 : args.command = command;
126 0 : args.size = I2C_SMBUS_BYTE_DATA;
127 0 : args.data = &data;
128 :
129 0 : if (ioctl(file, I2C_SMBUS, &args) < 0) {
130 0 : return -1;
131 : }
132 0 : return data.byte;
133 : }
134 :
135 : /*!
136 : * @brief Writes a byte of data to a specific register.
137 : *
138 : * @param fd The file descriptor for the I2C bus.
139 : * @param reg The register address to write to.
140 : * @param value The byte of data to write to the register.
141 : * @throws std::runtime_error if the I2C write operation fails.
142 : */
143 0 : void PeripheralController::write_byte_data(int fd, int reg, int value) {
144 0 : if (i2c_smbus_write_byte_data(fd, reg, value) < 0) {
145 0 : throw std::runtime_error("I2C write failed");
146 : }
147 0 : }
148 :
149 : /*!
150 : * @brief Reads a byte of data from a specific register.
151 : *
152 : * @param fd The file descriptor for the I2C bus.
153 : * @param reg The register address to read from.
154 : * @return The byte of data read from the register.
155 : * @throws std::runtime_error if the I2C read operation fails.
156 : */
157 :
158 0 : int PeripheralController::read_byte_data(int fd, int reg) {
159 0 : int result = i2c_smbus_read_byte_data(fd, reg);
160 0 : if (result < 0) {
161 0 : throw std::runtime_error("I2C read failed");
162 : }
163 0 : return result;
164 : }
165 :
166 : /*!
167 : * @brief Sets the PWM of a servo motor.
168 : *
169 : * @param channel The channel number of the servo motor to control.
170 : * @param on_value The on-time value of the PWM signal (in range 0-4095).
171 : * @param off_value The off-time value of the PWM signal (in range 0-4095).
172 : *
173 : * @details The on-time value is stored in the first two bytes of the register,
174 : * and the off-time value is stored in the second two bytes. The actual PWM
175 : * frequency is 50 Hz.
176 : */
177 0 : void PeripheralController::set_servo_pwm(int channel, int on_value,
178 : int off_value) {
179 0 : int base_reg = 0x06 + (channel * 4);
180 0 : write_byte_data(servo_bus_fd_, base_reg, on_value & 0xFF);
181 0 : write_byte_data(servo_bus_fd_, base_reg + 1, on_value >> 8);
182 0 : write_byte_data(servo_bus_fd_, base_reg + 2, off_value & 0xFF);
183 0 : write_byte_data(servo_bus_fd_, base_reg + 3, off_value >> 8);
184 0 : }
185 :
186 : /*!
187 : * @brief Sets the PWM value for a motor.
188 : *
189 : * @param channel The motor channel to set.
190 : * @param value The desired PWM value.
191 : *
192 : * @details The value is clamped to [0, 4095] and then written to the motor
193 : * controller.
194 : */
195 0 : void PeripheralController::set_motor_pwm(int channel, int value) {
196 0 : value = clamp(value, 0, 4095);
197 0 : write_byte_data(motor_bus_fd_, 0x06 + (4 * channel), 0);
198 0 : write_byte_data(motor_bus_fd_, 0x07 + (4 * channel), 0);
199 0 : write_byte_data(motor_bus_fd_, 0x08 + (4 * channel), value & 0xFF);
200 0 : write_byte_data(motor_bus_fd_, 0x09 + (4 * channel), value >> 8);
201 0 : }
202 :
203 : /*!
204 : * @brief Initializes the servo controller.
205 : *
206 : * @details Configures the servo controller with specific register settings.
207 : * The method writes a sequence of commands to the servo bus to prepare the
208 : * controller for operation. It includes setting the mode, setting the prescale
209 : * value, and enabling the output. Each command is followed by a delay to
210 : * ensure proper initialization.
211 : */
212 :
213 0 : void PeripheralController::init_servo() {
214 0 : write_byte_data(servo_bus_fd_, 0x00, 0x06);
215 0 : usleep(100000);
216 :
217 0 : write_byte_data(servo_bus_fd_, 0x00, 0x10);
218 0 : usleep(100000);
219 :
220 0 : write_byte_data(servo_bus_fd_, 0xFE, 0x79);
221 0 : usleep(100000);
222 :
223 0 : write_byte_data(servo_bus_fd_, 0x01, 0x04);
224 0 : usleep(100000);
225 :
226 0 : write_byte_data(servo_bus_fd_, 0x00, 0x20);
227 0 : usleep(100000);
228 0 : }
229 :
230 : /*!
231 : * @brief Initializes the motor controllers.
232 : *
233 : * @details Sets up the motor controllers, configuring them for 100 Hz PWM
234 : * operation. The motors are then enabled.
235 : */
236 0 : void PeripheralController::init_motors() {
237 0 : write_byte_data(motor_bus_fd_, 0x00, 0x20);
238 :
239 0 : int prescale = static_cast<int>(std::floor(25000000.0 / 4096.0 / 100 - 1));
240 0 : int oldmode = read_byte_data(motor_bus_fd_, 0x00);
241 0 : int newmode = (oldmode & 0x7F) | 0x10;
242 :
243 0 : write_byte_data(motor_bus_fd_, 0x00, newmode);
244 0 : write_byte_data(motor_bus_fd_, 0xFE, prescale);
245 0 : write_byte_data(motor_bus_fd_, 0x00, oldmode);
246 0 : usleep(5000);
247 0 : write_byte_data(motor_bus_fd_, 0x00, oldmode | 0xa1);
248 0 : }
|
