SHERPA/microros_turntable_controller
4
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixed everything

Browse Source
This commit is contained in:
Timo Schneider
2026-01-10 19:58:33 +01:00
parent 579fca0b40
commit 55d35c41af
6 changed files with 42 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
src/stepper_controller/stepper_controller.cpp
View File

@@ -104,27 +104,36 @@ void StepperController::init() {
void StepperController::calibrate() {
gpio_pin_configure_dt(&status_led, GPIO_OUTPUT_INACTIVE);
gpio_pin_configure_dt(&calibration_led, GPIO_OUTPUT_ACTIVE);
set_motor_velocity(0.1);
k_sleep(K_MSEC(250));
set_motor_velocity(0.02);
k_sleep(K_MSEC(1000));
sample_angle();
float left_angle = current_angle;
set_motor_velocity(-0.1);
k_sleep(K_MSEC(500));
set_motor_velocity(-0.02);
k_sleep(K_MSEC(2000));
sample_angle();
float right_angle = current_angle;
set_motor_velocity(0.1);
k_sleep(K_MSEC(250));
if (right_angle >= left_angle) {
set_motor_velocity(0.02);
k_sleep(K_MSEC(1000));
if (right_angle < left_angle) {
direction_factor = -1.0;
}
printk("Servo direction calibration:\n");
printk("Left = %d.%02d\n", (int)left_angle, abs((int)(left_angle * 100) % 100));
printk("Right = %d.%02d\n", (int)right_angle, abs((int)(right_angle * 100) % 100));
printk("-> %c\n" , direction_factor > 0 ? '+' : '-');
printk("----------------------------\n");
stepper_stop(stepper0);
k_sleep(K_MSEC(100));
gpio_pin_configure_dt(&status_led, GPIO_OUTPUT_ACTIVE);
gpio_pin_configure_dt(&calibration_led, GPIO_OUTPUT_INACTIVE);
}
rcl_ret_t StepperController::setup(rclc_support_t* support, rclc_executor_t* executor)
@@ -190,9 +199,9 @@ void StepperController::sample_angle() {
float angle = direction_factor * ((float)val.val1 - 180.0f + (float)val.val2 / 1000000.0f);
float full_angle = angle + 360.0f * (float)angle_rotation_count;
if (full_angle - last_angle > 270)
if (full_angle - last_angle > 330)
angle_rotation_count--;
if (full_angle - last_angle < -270)
if (full_angle - last_angle < -330)
angle_rotation_count++;
last_angle = current_angle;
@@ -264,6 +273,8 @@ void StepperController::pid() {
}
void StepperController::set_motor_velocity(float rps) {
gpio_pin_configure_dt(&work_led, fabs(rps) > 0.05 ? GPIO_OUTPUT_ACTIVE : GPIO_OUTPUT_INACTIVE);
uint32_t motor_steps_hz = abs((int32_t)(rps * (float)SENSOR_STEPS_PER_REVOLUTION));
if (motor_steps_hz == 0) {