amikhalev/uaspire_poc
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Get servos to work

Browse Source
This commit is contained in:
Alex Mikhalev 2019-11-08 14:56:58 -08:00
parent 4b08a77552
commit f91b510e9d
2 changed files with 26 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/uaspire_controls/CMakeLists.txt
View File

@ -29,15 +29,8 @@ find_package(std_msgs REQUIRED)
find_package(uaspire_msgs REQUIRED) find_package(uaspire_msgs REQUIRED)
if (BUILD_RASPBERRY_PI) if (BUILD_RASPBERRY_PI)
find_library(WIRINGPI_LIB NAMES wiringPi HINTS ${CMAKE_SOURCE_DIR}/../WiringPi/wiringPi) find_library(PIGPIO_LIB NAMES pigpiod_if2 HINTS ${CMAKE_SOURCE_DIR}/../pigpio)
find_path(WIRINGPI_INCLUDE_DIR NAMES wiringPi.h HINTS ${CMAKE_SOURCE_DIR}/../WiringPi/wiringPi) find_path(PIGPIO_INCLUDE_DIR NAMES pigpiod_if2.h HINTS ${CMAKE_SOURCE_DIR}/../pigpio)
if (NOT WIRINGPI_LIB OR NOT WIRINGPI_INCLUDE_DIR)
message(FATAL_ERROR "Could not find wiringPi: ${WIRINGPI_LIB} ${WIRINGPI_INCLUDE_DIR}")
else()
message(STATUS "Found wiringPi: ${WIRINGPI_LIB} ${WIRINGPI_INCLUDE_DIR}")
endif()
find_library(PIGPIO_LIB NAMES pigpiod_if HINTS ${CMAKE_SOURCE_DIR}/../pigpio)
find_path(PIGPIO_INCLUDE_DIR NAMES pigpiod_if.h HINTS ${CMAKE_SOURCE_DIR}/../pigpio)
if (NOT PIGPIO_LIB OR NOT PIGPIO_INCLUDE_DIR) if (NOT PIGPIO_LIB OR NOT PIGPIO_INCLUDE_DIR)
message(FATAL_ERROR "Could not find pigpio: ${PIGPIO_LIB} ${PIGPIO_INCLUDE_DIR}") message(FATAL_ERROR "Could not find pigpio: ${PIGPIO_LIB} ${PIGPIO_INCLUDE_DIR}")
else() else()

32
src/uaspire_controls/main.cc
View File

@ -51,11 +51,17 @@ public:
} }
void init_servos() { void init_servos() {
_servos = { Servo { 9 }, Servo { 25 } }; _servos = { Servo { 23 }, Servo { 24 } };
for (auto& servo : _servos) { for (auto& servo : _servos) {
#ifdef BUILD_RASPBERRY_PI #ifdef BUILD_RASPBERRY_PI
set_mode(_pi, servo.pin, PI_OUTPUT); int result = set_mode(_pi, servo.pin, PI_OUTPUT);
gpio_write(_pi, servo.pin, PI_LOW); if (result != 0) {
RCLCPP_ERROR(get_logger(), "Error setting pin mode: %d", result);
}
result = gpio_write(_pi, servo.pin, PI_LOW);
if (result != 0) {
RCLCPP_ERROR(get_logger(), "Error writing pin: %d", result);
}
#endif #endif
} }
} }
@ -70,9 +76,12 @@ public:
value = std::max(value, -90.f); value = std::max(value, -90.f);
// convert value from -90 to 90 to int from 1000 to 2000 (us) // convert value from -90 to 90 to int from 1000 to 2000 (us)
unsigned int decoded_value = static_cast<unsigned int>( unsigned int decoded_value = static_cast<unsigned int>(
((value / 90.f) * 500.f) + 1000); ((value / 90.f) * 500.f) + 1500);
#ifdef BUILD_RASPBERRY_PI #ifdef BUILD_RASPBERRY_PI
set_servo_pulsewidth(_pi, servo.pin, decoded_value); int result = set_servo_pulsewidth(_pi, servo.pin, decoded_value);
if (result != 0) {
RCLCPP_ERROR(get_logger(), "error setting servo pulse width: %d", result);
}
#endif #endif
} }
} }
@ -103,14 +112,21 @@ public:
std::bind(&ControlsNode::on_loop, this)); std::bind(&ControlsNode::on_loop, this));
_last_value = 0; _last_value = 0;
// start 100Hz servo timer
_servo_timer = create_wall_timer(std::chrono::milliseconds(10),
std::bind(&ControlsNode::on_servo_update, this));
} }
void on_message(const UaspireCommand::SharedPtr cmd) { void on_message(const UaspireCommand::SharedPtr cmd) {
RCLCPP_INFO(get_logger(), "Command received: %s", cmd->data.c_str()); RCLCPP_INFO(get_logger(), "Command received: %s", cmd->data.c_str());
if (cmd->data == "start") {
// start 25Hz servo timer
_servo_timer = create_wall_timer(std::chrono::milliseconds(40),
std::bind(&ControlsNode::on_servo_update, this));
} else if (cmd->data == "stop") {
if (_servo_timer) {
_servo_timer->cancel();
}
_servo_timer.reset();
}
} }
void on_loop() { void on_loop() {