From 65a55dacebf7a733c8c55929d3486eba3a44daf6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sun, 7 Aug 2016 12:49:44 +0200
Subject: [PATCH] Solved a problem when the emmulated servos were sent to their
current position. The module got stucked. Changed the emmulated servos motion
range to 180 degrees instead of 300. Changed the limits accordingly.
---
motion/src/motion_manager.c | 2 +-
motion/src/pan_tilt.c | 24 ++++++++++++++----------
2 files changed, 15 insertions(+), 11 deletions(-)
diff --git a/motion/src/motion_manager.c b/motion/src/motion_manager.c
index 53084cb..7a116dc 100644
--- a/motion/src/motion_manager.c
+++ b/motion/src/motion_manager.c
@@ -214,7 +214,7 @@ uint8_t manager_init(uint8_t num_servos)
else if(model==0x0C00)// Emmulated servos from the expansion board pan&tilt servos
{
enable_servo(id);
- manager_servos[id].angle=300;
+ manager_servos[id].angle=180;
manager_servos[id].max_value=1023;
manager_servos[id].min_value=0;
manager_servos[id].center_value=512;
diff --git a/motion/src/pan_tilt.c b/motion/src/pan_tilt.c
index 66f1ab5..768cef4 100644
--- a/motion/src/pan_tilt.c
+++ b/motion/src/pan_tilt.c
@@ -60,14 +60,16 @@ void pan_move_angle(int8_t pan_angle)
}
else
{
- pan_current_angle=manager_servos[pan_servo_id].current_value<<7;
- pan_target_angle=((uint32_t)(((int32_t)pan_angle*(int32_t)manager_servos[pan_servo_id].max_value)/((int32_t)manager_servos[pan_servo_id].angle))+(uint32_t)manager_servos[pan_servo_id].center_value);
+ pan_target_angle=(uint32_t)((((int32_t)pan_angle*(int32_t)manager_servos[pan_servo_id].max_value)/((int32_t)manager_servos[pan_servo_id].angle)+(int32_t)manager_servos[pan_servo_id].center_value));
if(pan_target_angle>manager_servos[pan_servo_id].ccw_limit)
pan_target_angle=manager_servos[pan_servo_id].ccw_limit;
else if(pan_target_angle<manager_servos[pan_servo_id].cw_limit)
pan_target_angle=manager_servos[pan_servo_id].cw_limit;
- pan_target_angle=pan_target_angle<<7;
- pan_moving=0x01;
+ if(manager_servos[pan_servo_id].current_value==pan_target_angle)
+ pan_moving=0x00;
+ else
+ pan_moving=0x01;
+ pan_target_angle=(pan_target_angle<<7);
}
}
}
@@ -99,14 +101,16 @@ void tilt_move_angle(int8_t tilt_angle)
}
else
{
- tilt_current_angle=manager_servos[tilt_servo_id].current_value<<7;
- tilt_target_angle=(((int32_t)tilt_angle*(int32_t)manager_servos[tilt_servo_id].max_value)/((int32_t)manager_servos[tilt_servo_id].angle))+manager_servos[tilt_servo_id].center_value;
+ tilt_target_angle=(int32_t)((((int32_t)tilt_angle*(int32_t)manager_servos[tilt_servo_id].max_value)/((int32_t)manager_servos[tilt_servo_id].angle)+(int32_t)manager_servos[tilt_servo_id].center_value));
if(tilt_target_angle>manager_servos[tilt_servo_id].ccw_limit)
tilt_target_angle=manager_servos[tilt_servo_id].ccw_limit;
else if(tilt_target_angle<manager_servos[tilt_servo_id].cw_limit)
tilt_target_angle=manager_servos[tilt_servo_id].cw_limit;
- tilt_target_angle=tilt_target_angle<<7;
- tilt_moving=0x01;
+ if(manager_servos[tilt_servo_id].current_value==tilt_target_angle)
+ tilt_moving=0x00;
+ else
+ tilt_moving=0x01;
+ tilt_target_angle=(tilt_target_angle<<7);
}
}
}
@@ -161,7 +165,7 @@ void pan_tilt_process(void)
if(pan_current_angle<=pan_target_angle)
{
pan_current_angle=pan_target_angle;
- pan_moving=0x00;
+ pan_moving=0x00;
}
}
}
@@ -203,7 +207,7 @@ void pan_tilt_process(void)
tilt_moving=0x00;
}
}
- }
+ }
}
manager_servos[tilt_servo_id].current_value=(tilt_current_angle>>7);
}
--
GitLab