From 4ff0e7fae0f7b56249613fa387d7b4b42cc7a3ad Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Mon, 22 Feb 2016 22:58:32 +0000
Subject: [PATCH] Added the slope control to the action execution.
---
include/motion_manager.h | 3 +++
src/action.c | 3 +--
src/motion_manager.c | 7 +++++--
3 files changed, 9 insertions(+), 4 deletions(-)
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 2885584..c74a16e 100644
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -52,6 +52,8 @@ typedef struct{
uint16_t center_angle;
uint16_t max_speed;
int16_t current_angle;
+ uint8_t cw_comp;
+ uint8_t ccw_comp;
uint16_t current_value;
TModules module;
uint8_t enabled;
@@ -64,6 +66,7 @@ typedef struct{
// public variables
extern int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
+extern int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
// public functions
void manager_init(uint16_t period_us);
diff --git a/src/action.c b/src/action.c
index bccc1fe..62ccc53 100644
--- a/src/action.c
+++ b/src/action.c
@@ -124,10 +124,8 @@ void action_load_next_step(void)
}
}
// compute trajectory
-// action_pause_time=((action_current_page.steps[action_current_step_index].pause<<4)*action_current_page.header.speed);
action_pause_time=(action_current_page.steps[action_current_step_index].pause*action_current_page.header.speed)>>5;
action_pause_time=action_pause_time<<9;
-// action_step_time=((action_current_page.steps[action_current_step_index].time<<4)*action_current_page.header.speed);
action_step_time=(action_current_page.steps[action_current_step_index].time*action_current_page.header.speed)>>5;
action_step_time=action_step_time<<9;
if(action_step_time<action_period)
@@ -178,6 +176,7 @@ void action_load_next_step(void)
if(tmp_angle>max_angle)
max_angle=tmp_angle;
}
+ manager_current_slopes[i]=action_current_page.header.slope[i];
}
}
if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)
diff --git a/src/motion_manager.c b/src/motion_manager.c
index d67dbd8..824bb2f 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -20,6 +20,7 @@ uint8_t manager_num_servos;
TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
// current angles used for all motion modules
int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
+int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
// balance values and configuration
int64_t manager_balance_offset[MANAGER_MAX_NUM_SERVOS];
uint8_t manager_balance_enabled;
@@ -36,12 +37,14 @@ void manager_send_motion_command(void)
if(manager_servos[i].enabled && manager_servos[i].model!=0x0000)
{
servo_ids[num]=manager_servos[i].id;
- write_data[num].data_addr=(uint8_t *)&(manager_servos[i].current_value);
+ manager_servos[i].cw_comp=(1<<manager_current_slopes[i]);
+ manager_servos[i].ccw_comp=(1<<manager_current_slopes[i]);
+ write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
num++;
}
}
if(num>0)
- dyn_master_sync_write(&bioloid_dyn_master_servos,num,servo_ids,P_GOAL_POSITION_L,2,write_data);
+ dyn_master_sync_write(&bioloid_dyn_master_servos,num,servo_ids,P_GOAL_POSITION_L,4,write_data);
}
inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
--
GitLab