diff --git a/dynamixel_manager/include/modules/action_mm_registers.h b/dynamixel_manager/include/modules/action_mm_registers.h
deleted file mode 100644
index 847f0534efaf38bafcf6ac2c3b8e00ee1fec57e9..0000000000000000000000000000000000000000
--- a/dynamixel_manager/include/modules/action_mm_registers.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#ifndef _ACTION_MM_REGISTERS_H
-#define _ACTION_MM_REGISTERS_H
-
-#define RAM_ACTION_MM_LENGTH 2
-
-#define ACTION_MM_CONTROL_OFFSET 0
-#define ACTION_MM_PAGE_OFFSET 1
- #define ACTION_MM_START 0x01
- #define ACTION_MM_STOP 0x02
- #define ACTION_MM_RUNNING 0x80
-
-#endif
-
diff --git a/dynamixel_manager/include/modules/action_registers.h b/dynamixel_manager/include/modules/action_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..f1c156338974c3b206e8b81493fc8f0a11b1c35a
--- /dev/null
+++ b/dynamixel_manager/include/modules/action_registers.h
@@ -0,0 +1,13 @@
+#ifndef _ACTION_REGISTERS_H
+#define _ACTION_REGISTERS_H
+
+#define RAM_ACTION_LENGTH 2
+
+#define ACTION_CONTROL_OFFSET 0
+#define ACTION_PAGE_OFFSET 1
+ #define ACTION_START 0x01
+ #define ACTION_STOP 0x02
+ #define ACTION_RUNNING 0x80
+
+#endif
+
diff --git a/dynamixel_manager/include/modules/head_tracking.h b/dynamixel_manager/include/modules/head_tracking.h
new file mode 100644
index 0000000000000000000000000000000000000000..4b552b796b89217cb78c9866e1fb876721b385c4
--- /dev/null
+++ b/dynamixel_manager/include/modules/head_tracking.h
@@ -0,0 +1,60 @@
+#ifndef _HEAD_TRACKING_H
+#define _HEAD_TRACKINH_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "motion_module.h"
+#include "memory.h"
+
+typedef struct{
+ unsigned short int kp;
+ unsigned short int ki;
+ unsigned short int kd;
+ long int prev_error;// 48|16
+ long int integral_part;
+ long int integral_clamp;
+ long int max_angle;
+ long int min_angle;
+ long int target_angle;
+}TJointControl;
+
+typedef struct{
+ TMotionModule mmodule;
+ TMemory *memory;
+ TMemModule mem_module;
+ unsigned short int ram_base_address;
+ unsigned short int eeprom_base_address;
+ TJointControl tracking_pan;
+ unsigned char pan_servo_id;
+ TJointControl tracking_tilt;
+ unsigned char tilt_servo_id;
+ long int tracking_period;
+ short int tracking_period_ms;
+ unsigned char tracking_enabled;
+}THeadMModule;
+
+// public functions
+unsigned char head_tracking_init(THeadMModule *joint,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address);
+void head_tracking_start(THeadMModule *joint);
+void head_tracking_stop(THeadMModule *joint);
+unsigned char head_is_tracking(THeadMModule *joint);
+void head_tracking_set_pan_range(THeadMModule *joint,short int max_angle,short int min_angle);
+void head_tracking_get_pan_range(THeadMModule *joint,short int *max_angle,short int *min_angle);
+void head_tracking_set_pan_pid(THeadMModule *joint,unsigned short int kp,unsigned short int ki,unsigned short int kd, unsigned short int i_clamp);
+void head_tracking_get_pan_pid(THeadMModule *joint,unsigned short int *kp,unsigned short int *ki,unsigned short int *kd, unsigned short int *i_clamp);
+void head_tracking_set_tilt_range(THeadMModule *joint,short int max_angle,short int min_angle);
+void head_tracking_get_tilt_range(THeadMModule *joint,short int *max_angle,short int *min_angle);
+void head_tracking_set_tilt_pid(THeadMModule *joint,unsigned short int kp,unsigned short int ki,unsigned short int kd, unsigned short int i_clamp);
+void head_tracking_get_tilt_pid(THeadMModule *joint,unsigned short int *kp,unsigned short int *ki,unsigned short int *kd, unsigned short int *i_clamp);
+void head_tracking_set_pan_pan(THeadMModule *joint,short int target);
+void head_tracking_set_tilt_target(THeadMModule *joint,short int target);
+void head_tracking_set_pan_servo_id(THeadMModule *joint,unsigned char servo_id);
+void head_tracking_set_tilt_servo_id(THeadMModule *joint,unsigned char servo_id);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/dynamixel_manager/include/modules/head_tracking_registers.h b/dynamixel_manager/include/modules/head_tracking_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..761a2c45348383e981d7c54a7f7287b6de0b1d7e
--- /dev/null
+++ b/dynamixel_manager/include/modules/head_tracking_registers.h
@@ -0,0 +1,54 @@
+#ifndef _HEAD_TRACKING_REGISTERS_H
+#define _HEAD_TRACKING_REGISTERS_H
+
+#define RAM_HEAD_TRACKING_LENGTH 13
+
+#define HEAD_TRACKING_CONTROL_OFFSET 0 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | tracking | | | stop tracking | start tracking
+ #define HEAD_TRACKING_START 0x01
+ #define HEAD_TRACKING_STOP 0x02
+ #define HEAD_TRACKING_RUNNING 0x10
+#define HEAD_TRACKING_MAX_PAN_OFFSET 1
+#define HEAD_TRACKING_MIN_PAN_OFFSET 3
+#define HEAD_TRACKING_PAN_TARGET_OFFSET 5
+#define HEAD_TRACKING_MAX_TILT_OFFSET 7
+#define HEAD_TRACKING_MIN_TILT_OFFSET 9
+#define HEAD_TRACKING_TILT_TARGET_OFFSET 11
+
+#define EEPROM_HEAD_TRACKING_LENGTH 18
+
+#define HEAD_TRACKING_PAN_P_GAIN_OFFSET 0
+#define HEAD_TRACKING_PAN_I_GAIN_OFFSET 2
+#define HEAD_TRACKING_PAN_D_GAIN_OFFSET 4
+#define HEAD_TRACKING_PAN_I_CLAMP_OFFSET 6
+#define HEAD_TRACKING_TILT_P_GAIN_OFFSET 8
+#define HEAD_TRACKING_TILT_I_GAIN_OFFSET 10
+#define HEAD_TRACKING_TILT_D_GAIN_OFFSET 12
+#define HEAD_TRACKING_TILT_I_CLAMP_OFFSET 14
+#define HEAD_TRACKING_PAN_SERVO_ID_OFFSET 16
+#define HEAD_TRACKING_TILT_SERVO_ID_OFFSET 17
+
+#define head_tracking_eeprom_data(name,section_name,base_address,DEFAULT_PAN_P_GAIN,DEFAULT_PAN_I_GAIN,DEFAULT_PAN_D_GAIN,DEFAULT_PAN_I_CLAMP,DEFAULT_TILT_P_GAIN,DEFAULT_TILT_I_GAIN,DEFAULT_TILT_D_GAIN,DEFAULT_TILT_I_CLAMP,DEFAULT_PAN_SERVO_ID,DEFAULT_TILT_SERVO_ID) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))= {\
+ DEFAULT_PAN_P_GAIN&0x00FF,base_address+HEAD_TRACKING_PAN_P_GAIN_OFFSET, \
+ (DEFAULT_PAN_P_GAIN>>8)&0x00FF,base_address+HEAD_TRACKING_PAN_P_GAIN_OFFSET+1, \
+ DEFAULT_PAN_I_GAIN&0x00FF,base_address+HEAD_TRACKING_PAN_I_GAIN_OFFSET, \
+ (DEFAULT_PAN_I_GAIN>>8)&0x00FF,base_address+HEAD_TRACKING_PAN_I_GAIN_OFFSET+1, \
+ DEFAULT_PAN_D_GAIN&0x00FF,base_address+HEAD_TRACKING_PAN_D_GAIN_OFFSET, \
+ (DEFAULT_PAN_D_GAIN>>8)&0x00FF,base_address+HEAD_TRACKING_PAN_D_GAIN_OFFSET+1, \
+ DEFAULT_PAN_I_CLAMP&0x00FF,base_address+HEAD_TRACKING_PAN_I_CLAMP_OFFSET, \
+ (DEFAULT_PAN_I_CLAMP>>8)&0x00FF,base_address+HEAD_TRACKING_PAN_I_CLAMP_OFFSET+1, \
+ DEFAULT_TILT_P_GAIN&0x00FF,base_address+HEAD_TRACKING_TILT_P_GAIN_OFFSET, \
+ (DEFAULT_TILT_P_GAIN>>8)&0x00FF,base_address+HEAD_TRACKING_TILT_P_GAIN_OFFSET+1, \
+ DEFAULT_TILT_I_GAIN&0x00FF,base_address+HEAD_TRACKING_TILT_I_GAIN_OFFSET, \
+ (DEFAULT_TILT_I_GAIN>>8)&0x00FF,base_address+HEAD_TRACKING_TILT_I_GAIN_OFFSET+1, \
+ DEFAULT_TILT_D_GAIN&0x00FF,base_address+HEAD_TRACKING_TILT_D_GAIN_OFFSET, \
+ (DEFAULT_TILT_D_GAIN>>8)&0x00FF,base_address+HEAD_TRACKING_TILT_D_GAIN_OFFSET+1, \
+ DEFAULT_TILT_I_CLAMP&0x00FF,base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET, \
+ (DEFAULT_TILT_I_CLAMP>>8)&0x00FF,base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET+1 \
+ DEFAULT_PAN_SERVO_ID&0x00FF,base_address+HEAD_TRACKING_PAN_SERVO_ID_OFFSET, \
+ DEFAULT_TILT_SERVO_ID&0x00FF,base_address+HEAD_TRACKING_TILT_SERVO_ID_OFFSET};
+
+
+#endif
+
diff --git a/dynamixel_manager/include/modules/joint_motion_mm_registers.h b/dynamixel_manager/include/modules/joint_motion_mm_registers.h
deleted file mode 100644
index a9e8e90a970620044f7f7c82d3712cb703d7ce14..0000000000000000000000000000000000000000
--- a/dynamixel_manager/include/modules/joint_motion_mm_registers.h
+++ /dev/null
@@ -1,16 +0,0 @@
-#ifndef _JOINT_MOTION_MM_REGISTERS_H
-#define _JOINT_MOTION_MM_REGISTERS_H
-
-#define RAM_JOINT_MOTION_MM_LENGTH (5*NUM_JOINT_GROUPS+DYN_MANAGER_MAX_NUM_DEVICES*4)
-
-#define JOINT_MOTION_MM_GROUP_CONTROL_OFFSET 0
- #define JOINT_MOTION_MM_START 0x01
- #define JOINT_MOTION_MM_STOP 0x02
- #define JOINT_MOTION_MM_RUNNING 0x80
-#define JOINT_MOTION_MM_GROUP_SERVOS_OFFSET NUM_JOINT_GROUPS
-#define JOINT_MOTION_MM_SERVO_ANGLES_OFFSET (5*NUM_JOINT_GROUPS)
-#define JOINT_MOTION_MM_SERVO_SPEEDS_OFFSET (5*NUM_JOINT_GROUPS)+DYN_MANAGER_MAX_NUM_DEVICES*2
-#define JOINT_MOTION_MM_SERVO_ACCELS_OFFSET (5*NUM_JOINT_GROUPS)+DYN_MANAGER_MAX_NUM_DEVICES*3
-
-#endif
-
diff --git a/dynamixel_manager/include/modules/joint_motion_registers.h b/dynamixel_manager/include/modules/joint_motion_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd47f967fb60c01535ed52d5b33c13f60a78cdfe
--- /dev/null
+++ b/dynamixel_manager/include/modules/joint_motion_registers.h
@@ -0,0 +1,16 @@
+#ifndef _JOINT_MOTION_REGISTERS_H
+#define _JOINT_MOTION_REGISTERS_H
+
+#define RAM_JOINT_MOTION_LENGTH (5*NUM_JOINT_GROUPS+DYN_MANAGER_MAX_NUM_DEVICES*4)
+
+#define JOINT_MOTION_GROUP_CONTROL_OFFSET 0
+ #define JOINT_MOTION_START 0x01
+ #define JOINT_MOTION_STOP 0x02
+ #define JOINT_MOTION_RUNNING 0x80
+#define JOINT_MOTION_GROUP_SERVOS_OFFSET NUM_JOINT_GROUPS
+#define JOINT_MOTION_SERVO_ANGLES_OFFSET (5*NUM_JOINT_GROUPS)
+#define JOINT_MOTION_SERVO_SPEEDS_OFFSET (5*NUM_JOINT_GROUPS)+DYN_MANAGER_MAX_NUM_DEVICES*2
+#define JOINT_MOTION_SERVO_ACCELS_OFFSET (5*NUM_JOINT_GROUPS)+DYN_MANAGER_MAX_NUM_DEVICES*3
+
+#endif
+
diff --git a/dynamixel_manager/include/modules/old/head_tracking.h b/dynamixel_manager/include/modules/old/head_tracking.h
deleted file mode 100644
index f5562649aa1bc791cf19d7e402df00d8e5f18c62..0000000000000000000000000000000000000000
--- a/dynamixel_manager/include/modules/old/head_tracking.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#ifndef _HEAD_TRACKING_H
-#define _HEAD_TRACKINH_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-#include "motion_manager.h"
-
-// public functions
-void head_tracking_init(uint16_t period_us);
-void head_tracking_set_period(uint16_t period_us);
-uint16_t head_tracking_get_period(void);
-void head_tracking_start(void);
-void head_tracking_stop(void);
-uint8_t head_is_tracking(void);
-void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle);
-void head_tracking_get_pan_range(int16_t *max_angle,int16_t *min_angle);
-void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp);
-void head_tracking_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp);
-void head_tracking_set_tilt_range(int16_t max_angle,int16_t min_angle);
-void head_tracking_get_tilt_range(int16_t *max_angle,int16_t *min_angle);
-void head_tracking_set_tilt_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp);
-void head_tracking_get_tilt_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp);
-void head_tracking_set_target_pan(int16_t target);
-void head_tracking_set_target_tilt(int16_t target);
-
-// operation functions
-uint8_t head_tracking_in_range(unsigned short int address, unsigned short int length);
-void head_tracking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-void head_tracking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-
-// motion manager process function
-void head_tracking_process(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/dynamixel_manager/src/modules/action.c b/dynamixel_manager/src/modules/action.c
index e901e01264cd3153a919c00899412317c73b4373..47203899fd870bb6c03732e5ff30afdcda2a2eb2 100755
--- a/dynamixel_manager/src/modules/action.c
+++ b/dynamixel_manager/src/modules/action.c
@@ -1,5 +1,5 @@
#include "action.h"
-#include "action_mm_registers.h"
+#include "action_registers.h"
#include "ram.h"
#define SPEED_BASE_SCHEDULE 0x00
@@ -11,15 +11,15 @@ void action_write_cmd(TActionMModule *module,unsigned short int address,unsigned
unsigned int ram_offset;
ram_offset=address-module->ram_base_address;
- if(ram_in_range(module->ram_base_address+ACTION_MM_CONTROL_OFFSET,address,length))
+ if(ram_in_range(module->ram_base_address+ACTION_CONTROL_OFFSET,address,length))
{
- if(data[ACTION_MM_CONTROL_OFFSET-ram_offset]&ACTION_MM_START)
+ if(data[ACTION_CONTROL_OFFSET-ram_offset]&ACTION_START)
action_start_page(module);
- if(data[ACTION_MM_CONTROL_OFFSET-ram_offset]&ACTION_MM_STOP)
+ if(data[ACTION_CONTROL_OFFSET-ram_offset]&ACTION_STOP)
action_stop_page(module);
}
- if(ram_in_range(module->ram_base_address+ACTION_MM_PAGE_OFFSET,address,length))// load the page identifier
- action_load_page(module,data[ACTION_MM_PAGE_OFFSET-ram_offset]);
+ if(ram_in_range(module->ram_base_address+ACTION_PAGE_OFFSET,address,length))// load the page identifier
+ action_load_page(module,data[ACTION_PAGE_OFFSET-ram_offset]);
}
void action_read_cmd(TActionMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
@@ -213,7 +213,7 @@ void action_finish(TActionMModule *action)
action->end=0x00;
// change the internal state
action->running=0x00;
- action->memory->data[action->ram_base_address+ACTION_MM_CONTROL_OFFSET]&=(~ACTION_MM_RUNNING);
+ action->memory->data[action->ram_base_address+ACTION_CONTROL_OFFSET]&=(~ACTION_RUNNING);
}
void action_set_period(void *module,unsigned short int period_ms)
@@ -496,7 +496,7 @@ unsigned char action_init(TActionMModule *action,TMemory *memory,unsigned short
action->mem_module.data=action;
action->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))action_write_cmd;
action->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))action_read_cmd;
- if(!mem_module_add_ram_segment(&action->mem_module,ram_base_address,RAM_ACTION_MM_LENGTH))
+ if(!mem_module_add_ram_segment(&action->mem_module,ram_base_address,RAM_ACTION_LENGTH))
return 0x00;
if(!mem_add_module(memory,&action->mem_module))
return 0x00;
@@ -520,7 +520,7 @@ unsigned char action_load_page(TActionMModule *action,unsigned char page_id)
return 0x00;
if(!pages_check_checksum(&action->next_page))
return 0x00;
- action->memory->data[action->ram_base_address+ACTION_MM_PAGE_OFFSET]=page_id;
+ action->memory->data[action->ram_base_address+ACTION_PAGE_OFFSET]=page_id;
return 0x01;
}
@@ -537,7 +537,7 @@ void action_start_page(TActionMModule *action)
action->current_step_index=-1;
/* clear the interrupt flag */
action->running=0x01;
- action->memory->data[action->ram_base_address+ACTION_MM_CONTROL_OFFSET]|=ACTION_MM_RUNNING;
+ action->memory->data[action->ram_base_address+ACTION_CONTROL_OFFSET]|=ACTION_RUNNING;
}
void action_stop_page(TActionMModule *action)
diff --git a/dynamixel_manager/src/modules/head_tracking.c b/dynamixel_manager/src/modules/head_tracking.c
new file mode 100644
index 0000000000000000000000000000000000000000..1487fa08bad16e8c6dab3fd2226339527bf3d34f
--- /dev/null
+++ b/dynamixel_manager/src/modules/head_tracking.c
@@ -0,0 +1,361 @@
+#include "head_tracking.h"
+#include "head_tracking_registers.h"
+#include "ram.h"
+
+// provate functions
+void head_tracking_read_cmd(THeadMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(module->memory,address,length,data);
+}
+
+void head_tracking_write_cmd(THeadMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ unsigned short int kp,kd,ki,i_clamp,ram_offset,eeprom_offset;
+ short int max,min,target;
+
+ eeprom_offset=address-module->eeprom_base_address;
+ if(ram_in_window(module->eeprom_base_address+HEAD_TRACKING_PAN_P_GAIN_OFFSET,8,address,length))
+ {
+ head_tracking_get_pan_pid(module,&kp,&kd,&ki,&i_clamp);
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_P_GAIN_OFFSET,address,length))
+ kp=(kp&0xFF00)|data[HEAD_TRACKING_PAN_P_GAIN_OFFSET-eeprom_offset];
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_P_GAIN_OFFSET+1,address,length))
+ kp=(kp&0x00FF)|(data[HEAD_TRACKING_PAN_P_GAIN_OFFSET+1-eeprom_offset]<<8);
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_I_GAIN_OFFSET,address,length))
+ ki=(kp&0xFF00)|data[HEAD_TRACKING_PAN_I_GAIN_OFFSET-eeprom_offset];
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_I_GAIN_OFFSET+1,address,length))
+ ki=(kp&0x00FF)|(data[HEAD_TRACKING_PAN_I_GAIN_OFFSET+1-eeprom_offset]<<8);
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_D_GAIN_OFFSET,address,length))
+ kd=(kp&0xFF00)|data[HEAD_TRACKING_PAN_D_GAIN_OFFSET-eeprom_offset];
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_D_GAIN_OFFSET+1,address,length))
+ kd=(kp&0x00FF)|(data[HEAD_TRACKING_PAN_D_GAIN_OFFSET+1-eeprom_offset]<<8);
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_I_CLAMP_OFFSET,address,length))
+ i_clamp=(kp&0xFF00)|data[HEAD_TRACKING_PAN_I_CLAMP_OFFSET-eeprom_offset];
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_I_CLAMP_OFFSET+1,address,length))
+ i_clamp=(kp&0x00FF)|(data[HEAD_TRACKING_PAN_I_CLAMP_OFFSET+1-eeprom_offset]<<8);
+ head_tracking_set_pan_pid(module,kp,kd,ki,i_clamp);
+ }
+ if(ram_in_window(module->eeprom_base_address+HEAD_TRACKING_TILT_P_GAIN_OFFSET,8,address,length))
+ {
+ head_tracking_get_tilt_pid(module,&kp,&kd,&ki,&i_clamp);
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_P_GAIN_OFFSET,address,length))
+ kp=(kp&0xFF00)|data[HEAD_TRACKING_TILT_P_GAIN_OFFSET-eeprom_offset];
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_P_GAIN_OFFSET+1,address,length))
+ kp=(kp&0x00FF)|(data[HEAD_TRACKING_TILT_P_GAIN_OFFSET+1-eeprom_offset]<<8);
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_I_GAIN_OFFSET,address,length))
+ ki=(kp&0xFF00)|data[HEAD_TRACKING_TILT_I_GAIN_OFFSET-eeprom_offset];
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_I_GAIN_OFFSET+1,address,length))
+ ki=(kp&0x00FF)|(data[HEAD_TRACKING_TILT_I_GAIN_OFFSET+1-eeprom_offset]<<8);
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_D_GAIN_OFFSET,address,length))
+ kd=(kp&0xFF00)|data[HEAD_TRACKING_TILT_D_GAIN_OFFSET-eeprom_offset];
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_D_GAIN_OFFSET+1,address,length))
+ kd=(kp&0x00FF)|(data[HEAD_TRACKING_TILT_D_GAIN_OFFSET+1-eeprom_offset]<<8);
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET,address,length))
+ i_clamp=(kp&0xFF00)|data[HEAD_TRACKING_TILT_I_CLAMP_OFFSET-eeprom_offset];
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET+1,address,length))
+ i_clamp=(kp&0x00FF)|(data[HEAD_TRACKING_TILT_I_CLAMP_OFFSET+1-eeprom_offset]<<8);
+ head_tracking_set_tilt_pid(module,kp,kd,ki,i_clamp);
+ }
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_PAN_SERVO_ID_OFFSET,address,length))
+ head_tracking_set_pan_servo_id(module,data[HEAD_TRACKING_PAN_SERVO_ID_OFFSET-eeprom_offset]);
+ if(ram_in_range(module->eeprom_base_address+HEAD_TRACKING_TILT_SERVO_ID_OFFSET,address,length))
+ head_tracking_set_tilt_servo_id(module,data[HEAD_TRACKING_TILT_SERVO_ID_OFFSET-eeprom_offset]);
+
+ // head tracking control
+ ram_offset=address-module->ram_base_address;
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_CONTROL_OFFSET,address,length))
+ {
+ if(data[HEAD_TRACKING_CONTROL_OFFSET-ram_offset]&HEAD_TRACKING_START)
+ head_tracking_start(module);
+ if(data[HEAD_TRACKING_CONTROL_OFFSET-ram_offset]&HEAD_TRACKING_STOP)
+ head_tracking_stop(module);
+ }
+ if(ram_in_window(module->ram_base_address+HEAD_TRACKING_MAX_PAN_OFFSET,4,address,length))
+ {
+ head_tracking_get_pan_range(module,&max,&min);
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MAX_PAN_OFFSET,address,length))
+ max=(max&0xFF00)|data[HEAD_TRACKING_MAX_PAN_OFFSET-ram_offset];
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MAX_PAN_OFFSET+1,address,length))
+ max=(max&0x00FF)|(data[HEAD_TRACKING_MAX_PAN_OFFSET+1-ram_offset]<<8);
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MIN_PAN_OFFSET,address,length))
+ max=(max&0xFF00)|data[HEAD_TRACKING_MIN_PAN_OFFSET-ram_offset];
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MIN_PAN_OFFSET+1,address,length))
+ max=(max&0x00FF)|(data[HEAD_TRACKING_MIN_PAN_OFFSET+1-ram_offset]<<8);
+ head_tracking_set_pan_range(module,max,min);
+ }
+ if(ram_in_window(module->ram_base_address+HEAD_TRACKING_MAX_TILT_OFFSET,4,address,length))
+ {
+ head_tracking_get_tilt_range(module,&max,&min);
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MAX_TILT_OFFSET,address,length))
+ max=(max&0xFF00)|data[HEAD_TRACKING_MAX_TILT_OFFSET-ram_offset];
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MAX_TILT_OFFSET+1,address,length))
+ max=(max&0x00FF)|(data[HEAD_TRACKING_MAX_TILT_OFFSET+1-ram_offset]<<8);
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MIN_TILT_OFFSET,address,length))
+ max=(max&0xFF00)|data[HEAD_TRACKING_MIN_TILT_OFFSET-ram_offset];
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_MIN_TILT_OFFSET+1,address,length))
+ max=(max&0x00FF)|(data[HEAD_TRACKING_MIN_TILT_OFFSET+1-ram_offset]<<8);
+ head_tracking_set_tilt_range(module,max,min);
+ }
+ if(ram_in_window(module->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET,2,address,length))
+ {
+ target=(module->tracking_pan.target_angle>>9);
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET,address,length))
+ {
+ target=(target&0xFF00)|data[HEAD_TRACKING_PAN_TARGET_OFFSET-ram_offset];
+ module->memory->data[module->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET]=data[HEAD_TRACKING_PAN_TARGET_OFFSET-ram_offset];
+ }
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET+1,address,length))
+ {
+ target=(target&0x00FF)|(data[HEAD_TRACKING_PAN_TARGET_OFFSET+1-ram_offset]<<8);
+ module->memory->data[module->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET]=data[HEAD_TRACKING_PAN_TARGET_OFFSET-ram_offset];
+ }
+ module->tracking_pan.target_angle=(target<<9);
+ }
+ if(ram_in_window(module->ram_base_address+HEAD_TRACKING_TILT_TARGET_OFFSET,2,address,length))
+ {
+ target=(module->tracking_tilt.target_angle>>9);
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_TILT_TARGET_OFFSET,address,length))
+ {
+ target=(target&0xFF00)|data[HEAD_TRACKING_TILT_TARGET_OFFSET-ram_offset];
+ module->memory->data[module->ram_base_address+HEAD_TRACKING_TILT_TARGET_OFFSET]=data[HEAD_TRACKING_TILT_TARGET_OFFSET-ram_offset];
+ }
+ if(ram_in_range(module->ram_base_address+HEAD_TRACKING_TILT_TARGET_OFFSET+1,address,length))
+ {
+ target=(target&0x00FF)|(data[HEAD_TRACKING_TILT_TARGET_OFFSET+1-ram_offset]<<8);
+ module->memory->data[module->ram_base_address+HEAD_TRACKING_TILT_TARGET_OFFSET]=data[HEAD_TRACKING_TILT_TARGET_OFFSET-ram_offset];
+ }
+ module->tracking_tilt.target_angle=(target<<9);
+ }
+}
+
+TMotionModule *head_tracking_get_module(THeadMModule *head)
+{
+ return &head->mmodule;
+}
+
+void head_tracking_set_period(void *module,unsigned short int period_ms)
+{
+ THeadMModule *head=(THeadMModule *)module;
+
+ // load the current period
+ head->tracking_period=(period_ms<<16)/1000;
+ head->tracking_period_ms=period_ms;
+}
+
+void head_tracking_set_module(void *module,unsigned char servo_id)
+{
+
+}
+
+// motion manager process function
+void head_tracking_pre_process(void *module)
+{
+ THeadMModule *head=(THeadMModule *)module;
+ long int pan_error, tilt_error;
+ long int derivative_pan=0, derivative_tilt=0;
+
+ if(head->tracking_enabled)
+ {
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_CONTROL_OFFSET]|=HEAD_TRACKING_RUNNING;
+ if(mmanager_get_module(head->mmodule.manager,head->pan_servo_id)==MM_HEAD)
+ {
+ pan_error=head->tracking_pan.target_angle-head->mmodule.manager->servo_values[head->pan_servo_id].target_angle;
+ head->tracking_pan.integral_part+=(pan_error*head->tracking_period)>>16;
+ if(head->tracking_pan.integral_part>head->tracking_pan.integral_clamp)
+ head->tracking_pan.integral_part=head->tracking_pan.integral_clamp;
+ else if(head->tracking_pan.integral_part<-head->tracking_pan.integral_clamp)
+ head->tracking_pan.integral_part=-head->tracking_pan.integral_clamp;
+ derivative_pan=((pan_error-head->tracking_pan.prev_error)<<16)/head->tracking_period;
+ head->mmodule.manager->servo_values[head->pan_servo_id].target_angle+=((head->tracking_pan.kp*pan_error)>>16);
+ head->mmodule.manager->servo_values[head->pan_servo_id].target_angle+=((head->tracking_pan.ki*head->tracking_pan.integral_part)>>16);
+ head->mmodule.manager->servo_values[head->pan_servo_id].target_angle+=((head->tracking_pan.kd*derivative_pan)>>16);
+ if(head->mmodule.manager->servo_values[head->pan_servo_id].target_angle>head->tracking_pan.max_angle)
+ head->mmodule.manager->servo_values[head->pan_servo_id].target_angle=head->tracking_pan.max_angle;
+ else if(head->mmodule.manager->servo_values[head->pan_servo_id].target_angle<head->tracking_pan.min_angle)
+ head->mmodule.manager->servo_values[head->pan_servo_id].target_angle=head->tracking_pan.min_angle;
+ head->tracking_pan.prev_error=pan_error;
+ }
+ if(mmanager_get_module(head->mmodule.manager,head->tilt_servo_id)==MM_HEAD)
+ {
+ tilt_error=head->tracking_tilt.target_angle-head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle;
+ head->tracking_tilt.integral_part+=(tilt_error*head->tracking_period)>>16;
+ if(head->tracking_tilt.integral_part>head->tracking_tilt.integral_clamp)
+ head->tracking_tilt.integral_part=head->tracking_tilt.integral_clamp;
+ else if(head->tracking_tilt.integral_part<-head->tracking_tilt.integral_clamp)
+ head->tracking_tilt.integral_part=-head->tracking_tilt.integral_clamp;
+ derivative_tilt=((tilt_error-head->tracking_tilt.prev_error)<<16)/head->tracking_period;
+ head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle+=((head->tracking_tilt.kp*tilt_error)>>16);
+ head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle+=((head->tracking_tilt.ki*head->tracking_tilt.integral_part)>>16);
+ head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle+=((head->tracking_tilt.kd*derivative_tilt)>>16);
+ if(head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle>head->tracking_tilt.max_angle)
+ head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle=head->tracking_tilt.max_angle;
+ else if(head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle<head->tracking_tilt.min_angle)
+ head->mmodule.manager->servo_values[head->tilt_servo_id].target_angle=head->tracking_tilt.min_angle;
+ head->tracking_tilt.prev_error=tilt_error;
+ }
+ }
+ else
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_CONTROL_OFFSET]&=(~HEAD_TRACKING_RUNNING);
+}
+
+// public functions
+unsigned char head_tracking_init(THeadMModule *head,TMemory *memory,unsigned short int ram_base_address,unsigned short int eeprom_base_address)
+{
+ /* initialize private variables */
+ head->tracking_pan.prev_error=0;
+ head->tracking_pan.integral_part=0;
+ head->tracking_pan.target_angle=0;
+ head->tracking_tilt.prev_error=0;
+ head->tracking_tilt.integral_part=0;
+ head->tracking_tilt.target_angle=0;
+ head->tracking_enabled=0x00;
+
+ /* initialize the motion module */
+ mmodule_init(&head->mmodule);
+ head->mmodule.id=MM_JOINTS;
+ head->mmodule.set_period=head_tracking_set_period;
+ head->mmodule.set_module=head_tracking_set_module;
+ head->mmodule.pre_process=head_tracking_pre_process;
+ head->mmodule.data=head;
+
+ mem_module_init(&head->mem_module);
+ head->mem_module.data=head;
+ head->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))head_tracking_write_cmd;
+ head->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))head_tracking_read_cmd;
+ if(!mem_module_add_ram_segment(&head->mem_module,ram_base_address,RAM_HEAD_TRACKING_LENGTH))
+ return 0x00;
+ if(!mem_module_add_eeprom_segment(&head->mem_module,eeprom_base_address,EEPROM_HEAD_TRACKING_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&head->mem_module))
+ return 0x00;
+ head->ram_base_address=ram_base_address;
+ head->eeprom_base_address=eeprom_base_address;
+ head->memory=memory;
+
+ return 0x01;
+}
+
+void head_tracking_start(THeadMModule *head)
+{
+ head->tracking_enabled=0x01;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_CONTROL_OFFSET]|=HEAD_TRACKING_RUNNING;
+ head_tracking_set_pan_range(head,head->mmodule.manager->servo_configs[head->pan_servo_id]->ccw_angle_limit<<9,head->mmodule.manager->servo_configs[head->pan_servo_id]->cw_angle_limit<<9);
+ head_tracking_set_pan_range(head,head->mmodule.manager->servo_configs[head->tilt_servo_id]->ccw_angle_limit<<9,head->mmodule.manager->servo_configs[head->tilt_servo_id]->cw_angle_limit<<9);
+}
+
+void head_tracking_stop(THeadMModule *head)
+{
+ head->tracking_enabled=0x00;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_CONTROL_OFFSET]&=(~HEAD_TRACKING_RUNNING);
+}
+
+unsigned char head_is_tracking(THeadMModule *head)
+{
+ return head->tracking_enabled;
+}
+
+void head_tracking_set_pan_range(THeadMModule *head,short int max_angle,short int min_angle)
+{
+ head->tracking_pan.max_angle=max_angle<<9;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_MAX_PAN_OFFSET]=max_angle&0x00FF;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_MAX_PAN_OFFSET+1]=(max_angle>>8)&0x00FF;
+ head->tracking_pan.min_angle=min_angle<<9;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_MIN_PAN_OFFSET]=min_angle&0x00FF;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_MIN_PAN_OFFSET+1]=(min_angle>>8)&0x00FF;
+}
+
+void head_tracking_get_pan_range(THeadMModule *head,short int *max_angle,short int *min_angle)
+{
+ *max_angle=head->tracking_pan.max_angle>>9;
+ *min_angle=head->tracking_pan.min_angle>>9;
+}
+
+void head_tracking_set_pan_pid(THeadMModule *head,unsigned short int kp,unsigned short int ki,unsigned short int kd, unsigned short int i_clamp)
+{
+ head->tracking_pan.kp=kp;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_PAN_P_GAIN_OFFSET]=kp&0x00FF;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_PAN_P_GAIN_OFFSET+1]=(kp>>8)&0x00FF;
+ head->tracking_pan.ki=ki;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_PAN_I_GAIN_OFFSET]=ki&0x00FF;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_PAN_I_GAIN_OFFSET+1]=(ki>>8)&0x00FF;
+ head->tracking_pan.kd=kd;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_PAN_D_GAIN_OFFSET]=kd&0x00FF;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_PAN_D_GAIN_OFFSET+1]=(kd>>8)&0x00FF;
+ head->tracking_pan.integral_clamp=i_clamp<<9;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_PAN_I_CLAMP_OFFSET]=i_clamp&0x00FF;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_PAN_I_CLAMP_OFFSET+1]=(i_clamp>>8)&0x00FF;
+}
+
+void head_tracking_get_pan_pid(THeadMModule *head,unsigned short int *kp,unsigned short int *ki,unsigned short int *kd, unsigned short int *i_clamp)
+{
+ *kp=head->tracking_pan.kp;
+ *ki=head->tracking_pan.ki;
+ *kd=head->tracking_pan.kd;
+ *i_clamp=head->tracking_pan.integral_clamp>>9;
+}
+
+void head_tracking_set_tilt_range(THeadMModule *head,short int max_angle,short int min_angle)
+{
+ head->tracking_tilt.max_angle=max_angle<<9;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_MAX_TILT_OFFSET]=max_angle&0x00FF;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_MAX_TILT_OFFSET+1]=(max_angle>>8)&0x00FF;
+ head->tracking_tilt.min_angle=min_angle<<9;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_MAX_TILT_OFFSET]=min_angle&0x00FF;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_MAX_TILT_OFFSET+1]=(min_angle>>8)&0x00FF;
+}
+
+void head_tracking_get_tilt_range(THeadMModule *head,short int *max_angle,short int *min_angle)
+{
+ *max_angle=head->tracking_tilt.max_angle>>9;
+ *min_angle=head->tracking_tilt.min_angle>>9;
+}
+
+void head_tracking_set_tilt_pid(THeadMModule *head,unsigned short int kp,unsigned short int ki,unsigned short int kd, unsigned short int i_clamp)
+{
+ head->tracking_tilt.kp=kp;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_TILT_P_GAIN_OFFSET]=kp&0x00FF;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_TILT_P_GAIN_OFFSET+1]=(kp>>8)&0x00FF;
+ head->tracking_tilt.ki=ki;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_TILT_I_GAIN_OFFSET]=ki&0x00FF;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_TILT_I_GAIN_OFFSET+1]=(ki>>8)&0x00FF;
+ head->tracking_tilt.kd=kd;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_TILT_D_GAIN_OFFSET]=kd&0x00FF;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_TILT_D_GAIN_OFFSET+1]=(kd>>8)&0x00FF;
+ head->tracking_tilt.integral_clamp=i_clamp<<9;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET]=i_clamp&0x00FF;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_TILT_I_CLAMP_OFFSET+1]=(i_clamp>>8)&0x00FF;
+}
+
+void head_tracking_get_tilt_pid(THeadMModule *head,unsigned short int *kp,unsigned short int *ki,unsigned short int *kd, unsigned short int *i_clamp)
+{
+ *kp=head->tracking_tilt.kp;
+ *ki=head->tracking_tilt.ki;
+ *kd=head->tracking_tilt.kd;
+ *i_clamp=head->tracking_tilt.integral_clamp>>9;
+}
+
+void head_tracking_set_pan_target(THeadMModule *head,short int target)
+{
+ head->tracking_pan.target_angle=target<<9;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET]=target&0x00FF;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_PAN_TARGET_OFFSET+1]=(target>>8)&0x00FF;
+}
+
+void head_tracking_set_tilt_target(THeadMModule *head,short int target)
+{
+ head->tracking_tilt.target_angle=target<<9;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_TILT_TARGET_OFFSET]=target&0x00FF;
+ head->memory->data[head->ram_base_address+HEAD_TRACKING_TILT_TARGET_OFFSET+1]=(target>>8)&0x00FF;
+}
+
+void head_tracking_set_pan_servo_id(THeadMModule *head,unsigned char servo_id)
+{
+ head->pan_servo_id=servo_id;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_PAN_SERVO_ID_OFFSET]=servo_id;
+}
+
+void head_tracking_set_tilt_servo_id(THeadMModule *head,unsigned char servo_id)
+{
+ head->tilt_servo_id=servo_id;
+ head->memory->data[head->eeprom_base_address+HEAD_TRACKING_TILT_SERVO_ID_OFFSET]=servo_id;
+}
diff --git a/dynamixel_manager/src/modules/joint_motion.c b/dynamixel_manager/src/modules/joint_motion.c
index 332b080cfcfbaf55a4f61e24f3aecf9e7503cfb2..ebbe9eb9a13e5a697ea551ebd4e4dd3fb0eb4fed 100644
--- a/dynamixel_manager/src/modules/joint_motion.c
+++ b/dynamixel_manager/src/modules/joint_motion.c
@@ -1,5 +1,5 @@
#include "joint_motion.h"
-#include "joint_motion_mm_registers.h"
+#include "joint_motion_registers.h"
#include "ram.h"
#include <stdlib.h>
@@ -18,24 +18,24 @@ void joint_motion_write_cmd(TJointMModule *module,unsigned short int address,uns
// load motion information
ram_offset=address-module->ram_base_address;
for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES*4;i++)
- if(ram_in_range(module->ram_base_address+JOINT_MOTION_MM_SERVO_ANGLES_OFFSET+i,address,length))
- module->memory->data[i]=data[JOINT_MOTION_MM_SERVO_ANGLES_OFFSET+i-ram_offset];
+ if(ram_in_range(module->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i,address,length))
+ module->memory->data[i]=data[JOINT_MOTION_SERVO_ANGLES_OFFSET+i-ram_offset];
// group servos
for(j=0;j<NUM_JOINT_GROUPS;j++)
{
- if(ram_in_window(module->ram_base_address+JOINT_MOTION_MM_GROUP_SERVOS_OFFSET+j*4,4,address,length))
+ if(ram_in_window(module->ram_base_address+JOINT_MOTION_GROUP_SERVOS_OFFSET+j*4,4,address,length))
{
servos=joint_motion_get_group_servos(module,j);
for(i=0;i<4;i++)
- if(ram_in_range(module->ram_base_address+JOINT_MOTION_MM_GROUP_SERVOS_OFFSET+j*4+i,address,length))
- servos|=(data[JOINT_MOTION_MM_GROUP_SERVOS_OFFSET+j*4+i-ram_offset]<<(i*8));
+ if(ram_in_range(module->ram_base_address+JOINT_MOTION_GROUP_SERVOS_OFFSET+j*4+i,address,length))
+ servos|=(data[JOINT_MOTION_GROUP_SERVOS_OFFSET+j*4+i-ram_offset]<<(i*8));
joint_motion_set_group_servos(module,j,servos);
}
- if(ram_in_range(module->ram_base_address+JOINT_MOTION_MM_GROUP_CONTROL_OFFSET+j,address,length))
+ if(ram_in_range(module->ram_base_address+JOINT_MOTION_GROUP_CONTROL_OFFSET+j,address,length))
{
- if(data[JOINT_MOTION_MM_GROUP_CONTROL_OFFSET+j-ram_offset]&JOINT_MOTION_MM_START)
+ if(data[JOINT_MOTION_GROUP_CONTROL_OFFSET+j-ram_offset]&JOINT_MOTION_START)
joint_motion_start(module,j);
- if(data[JOINT_MOTION_MM_GROUP_CONTROL_OFFSET+j-ram_offset]&JOINT_MOTION_MM_STOP)
+ if(data[JOINT_MOTION_GROUP_CONTROL_OFFSET+j-ram_offset]&JOINT_MOTION_STOP)
joint_motion_stop(module,j);
}
}
@@ -154,7 +154,7 @@ void joint_motion_pre_process(void *module)
{
joint->moving[j]=0x00;
joint->stop[j]=0x00;
- joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_GROUP_CONTROL_OFFSET+j]&=(~JOINT_MOTION_MM_RUNNING);
+ joint->memory->data[joint->ram_base_address+JOINT_MOTION_GROUP_CONTROL_OFFSET+j]&=(~JOINT_MOTION_RUNNING);
}
}
}
@@ -197,7 +197,7 @@ unsigned char joint_motion_init(TJointMModule *joint,TMemory *memory,unsigned sh
joint->mem_module.data=joint;
joint->mem_module.write_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))joint_motion_write_cmd;
joint->mem_module.read_cmd=(void(*)(void *,unsigned short int, unsigned short int,unsigned char *))joint_motion_read_cmd;
- if(!mem_module_add_ram_segment(&joint->mem_module,ram_base_address,RAM_JOINT_MOTION_MM_LENGTH))
+ if(!mem_module_add_ram_segment(&joint->mem_module,ram_base_address,RAM_JOINT_MOTION_LENGTH))
return 0x00;
if(!mem_add_module(memory,&joint->mem_module))
return 0x00;
@@ -210,10 +210,10 @@ unsigned char joint_motion_init(TJointMModule *joint,TMemory *memory,unsigned sh
void joint_motion_set_group_servos(TJointMModule *joint,unsigned char group_id,unsigned int servos)
{
joint->group_servos[group_id]=servos;
- joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_GROUP_SERVOS_OFFSET+group_id*4]=servos&0x000000FF;
- joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_GROUP_SERVOS_OFFSET+group_id*4+1]=(servos>>8)&0x000000FF;
- joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_GROUP_SERVOS_OFFSET+group_id*4+2]=(servos>>16)&0x000000FF;
- joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_GROUP_SERVOS_OFFSET+group_id*4+3]=(servos>>24)&0x000000FF;
+ joint->memory->data[joint->ram_base_address+JOINT_MOTION_GROUP_SERVOS_OFFSET+group_id*4]=servos&0x000000FF;
+ joint->memory->data[joint->ram_base_address+JOINT_MOTION_GROUP_SERVOS_OFFSET+group_id*4+1]=(servos>>8)&0x000000FF;
+ joint->memory->data[joint->ram_base_address+JOINT_MOTION_GROUP_SERVOS_OFFSET+group_id*4+2]=(servos>>16)&0x000000FF;
+ joint->memory->data[joint->ram_base_address+JOINT_MOTION_GROUP_SERVOS_OFFSET+group_id*4+3]=(servos>>24)&0x000000FF;
}
unsigned int joint_motion_get_group_servos(TJointMModule *joint,unsigned char group_id)
@@ -232,33 +232,33 @@ void joint_motion_start(TJointMModule *joint,unsigned char group_id)
{
if(joint->group_servos[group_id]&servo_index)
{
- if(joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_SERVO_SPEEDS_OFFSET+i]==0)
+ if(joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_SPEEDS_OFFSET+i]==0)
{
joint->target_angles[i]=joint->mmodule.manager->servo_values[i].target_angle;
joint->current_angles[i]=joint->mmodule.manager->servo_values[i].target_angle;
angle=joint->mmodule.manager->servo_values[i].target_angle>>9;
- joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_SERVO_ANGLES_OFFSET+i*2]=angle&0x00FF;
- joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_SERVO_ANGLES_OFFSET+i*2+1]=(angle>>8)&0x00FF;
+ joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i*2]=angle&0x00FF;
+ joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i*2+1]=(angle>>8)&0x00FF;
}
else
{
// current values
joint->current_angles[i]=joint->mmodule.manager->servo_values[i].target_angle;
// target angle
- angle=joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_SERVO_ANGLES_OFFSET+i*2]+(joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_SERVO_ANGLES_OFFSET+i*2+1]<<8);
+ angle=joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i*2]+(joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ANGLES_OFFSET+i*2+1]<<8);
if(angle>joint->mmodule.manager->servo_configs[i]->ccw_angle_limit)
angle=joint->mmodule.manager->servo_configs[i]->ccw_angle_limit;
else if(angle<joint->mmodule.manager->servo_configs[i]->cw_angle_limit)
angle=joint->mmodule.manager->servo_configs[i]->ccw_angle_limit;
joint->target_angles[i]=angle<<9;
// target speed
- joint->target_speeds[i]=joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_SERVO_SPEEDS_OFFSET+i]<<16;
+ joint->target_speeds[i]=joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_SPEEDS_OFFSET+i]<<16;
// target accel
- joint->target_accels[i]=joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_SERVO_ACCELS_OFFSET+i]<<16;
+ joint->target_accels[i]=joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ACCELS_OFFSET+i]<<16;
if(joint->target_accels[i]==0)
{
joint->target_accels[i]=1<<16;
- joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_SERVO_ACCELS_OFFSET+i]=1;
+ joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ACCELS_OFFSET+i]=1;
}
// check the parameters
if(abs(joint->target_angles[i]-joint->current_angles[i])>=65)
@@ -266,7 +266,7 @@ void joint_motion_start(TJointMModule *joint,unsigned char group_id)
if((joint->target_speeds[i]*joint->target_speeds[i])/joint->target_accels[i]>abs(joint->target_angles[i]-joint->current_angles[i]))
{
joint->target_accels[i]=(joint->target_speeds[i]*joint->target_speeds[i])/abs(joint->target_angles[i]-joint->current_angles[i]);
- joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_SERVO_ACCELS_OFFSET+i]=joint->target_accels[i]>>16;
+ joint->memory->data[joint->ram_base_address+JOINT_MOTION_SERVO_ACCELS_OFFSET+i]=joint->target_accels[i]>>16;
}
}
// stop angles
@@ -281,7 +281,7 @@ void joint_motion_start(TJointMModule *joint,unsigned char group_id)
}
joint->moving[group_id]=0x01;
joint->stop[group_id]=0x00;
- joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_GROUP_CONTROL_OFFSET+group_id]|=JOINT_MOTION_MM_RUNNING;
+ joint->memory->data[joint->ram_base_address+JOINT_MOTION_GROUP_CONTROL_OFFSET+group_id]|=JOINT_MOTION_RUNNING;
}
void joint_motion_stop(TJointMModule *joint,unsigned char group_id)
diff --git a/dynamixel_manager/src/modules/old/head_tracking.c b/dynamixel_manager/src/modules/old/head_tracking.c
deleted file mode 100644
index 12ee27c4d4485534af863f89d724864df4104f16..0000000000000000000000000000000000000000
--- a/dynamixel_manager/src/modules/old/head_tracking.c
+++ /dev/null
@@ -1,333 +0,0 @@
-#include "head_tracking.h"
-#include "ram.h"
-
-// private variables
-typedef struct{
- uint16_t kp;
- uint16_t ki;
- uint16_t kd;
- int64_t prev_error;// 48|16
- int64_t integral_part;
- int64_t integral_clamp;
- int64_t max_angle;
- int64_t min_angle;
- int64_t target_angle;
-}TJointControl;
-
-TJointControl head_tracking_pan;
-TJointControl head_tracking_tilt;
-int64_t head_tracking_period;
-int16_t head_tracking_period_us;
-uint8_t head_tracking_enabled;
-
-// public functions
-void head_tracking_init(uint16_t period_us)
-{
- /* initialize private variables */
- head_tracking_pan.kp=ram_data[DARWIN_HEAD_PAN_P_L]+(ram_data[DARWIN_HEAD_PAN_P_H]<<8);
- head_tracking_pan.ki=ram_data[DARWIN_HEAD_PAN_I_L]+(ram_data[DARWIN_HEAD_PAN_I_H]<<8);
- head_tracking_pan.kd=ram_data[DARWIN_HEAD_PAN_D_L]+(ram_data[DARWIN_HEAD_PAN_D_H]<<8);
- head_tracking_pan.prev_error=0;
- head_tracking_pan.integral_part=0;
- head_tracking_pan.integral_clamp=(ram_data[DARWIN_HEAD_PAN_I_CLAMP_L]+(ram_data[DARWIN_HEAD_PAN_I_CLAMP_H]<<8))<<9;
- head_tracking_pan.min_angle=manager_get_cw_angle_limit(L_PAN)<<9;
- ram_data[DARWIN_HEAD_MIN_PAN_L]=manager_get_cw_angle_limit(L_PAN)%256;
- ram_data[DARWIN_HEAD_MIN_PAN_H]=manager_get_cw_angle_limit(L_PAN)/256;
- head_tracking_pan.max_angle=manager_get_ccw_angle_limit(L_PAN)<<9;
- ram_data[DARWIN_HEAD_MAX_PAN_L]=manager_get_ccw_angle_limit(L_PAN)%256;
- ram_data[DARWIN_HEAD_MAX_PAN_H]=manager_get_ccw_angle_limit(L_PAN)/256;
- head_tracking_pan.target_angle=0;
- head_tracking_tilt.kp=ram_data[DARWIN_HEAD_TILT_P_L]+(ram_data[DARWIN_HEAD_TILT_P_H]<<8);
- head_tracking_tilt.ki=ram_data[DARWIN_HEAD_TILT_I_L]+(ram_data[DARWIN_HEAD_TILT_I_H]<<8);
- head_tracking_tilt.kd=ram_data[DARWIN_HEAD_TILT_D_L]+(ram_data[DARWIN_HEAD_TILT_D_H]<<8);
- head_tracking_tilt.prev_error=0;
- head_tracking_tilt.integral_part=0;
- head_tracking_tilt.integral_clamp=(ram_data[DARWIN_HEAD_TILT_I_CLAMP_L]+(ram_data[DARWIN_HEAD_TILT_I_CLAMP_H]<<8))<<9;
- head_tracking_tilt.min_angle=manager_get_cw_angle_limit(L_TILT)<<9;
- ram_data[DARWIN_HEAD_MIN_TILT_L]=manager_get_cw_angle_limit(L_TILT)%256;
- ram_data[DARWIN_HEAD_MIN_TILT_H]=manager_get_cw_angle_limit(L_TILT)/256;
- head_tracking_tilt.max_angle=manager_get_ccw_angle_limit(L_TILT)<<9;
- ram_data[DARWIN_HEAD_MAX_TILT_L]=manager_get_ccw_angle_limit(L_TILT)%256;
- ram_data[DARWIN_HEAD_MAX_TILT_H]=manager_get_ccw_angle_limit(L_TILT)/256;
- head_tracking_tilt.target_angle=0;
- head_tracking_period=(period_us<<16)/1000000;
- head_tracking_period_us=period_us;
- head_tracking_enabled=0x00;
-}
-
-void head_tracking_set_period(uint16_t period_us)
-{
- head_tracking_period=(period_us<<16)/1000000;
- head_tracking_period_us=period_us;
-}
-
-uint16_t head_tracking_get_period(void)
-{
- return head_tracking_period_us;
-}
-
-void head_tracking_start(void)
-{
- head_tracking_enabled=0x01;
- ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS;
-}
-
-void head_tracking_stop(void)
-{
- head_tracking_enabled=0x00;
- ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS);
-}
-
-uint8_t head_is_tracking(void)
-{
- return head_tracking_enabled;
-}
-
-void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle)
-{
- head_tracking_pan.max_angle=max_angle<<9;
- ram_data[DARWIN_HEAD_MAX_PAN_L]=max_angle%256;
- ram_data[DARWIN_HEAD_MAX_PAN_H]=max_angle/256;
- head_tracking_pan.min_angle=min_angle<<9;
- ram_data[DARWIN_HEAD_MIN_PAN_L]=min_angle%256;
- ram_data[DARWIN_HEAD_MIN_PAN_H]=min_angle/256;
-}
-
-void head_tracking_get_pan_range(int16_t *max_angle,int16_t *min_angle)
-{
- *max_angle=head_tracking_pan.max_angle>>9;
- *min_angle=head_tracking_pan.min_angle>>9;
-}
-
-void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp)
-{
- head_tracking_pan.kp=kp;
- ram_data[DARWIN_HEAD_PAN_P_L]=kp%256;
- ram_data[DARWIN_HEAD_PAN_P_H]=kp/256;
- head_tracking_pan.ki=ki;
- ram_data[DARWIN_HEAD_PAN_I_L]=ki%256;
- ram_data[DARWIN_HEAD_PAN_I_H]=ki/256;
- head_tracking_pan.kd=kd;
- ram_data[DARWIN_HEAD_PAN_D_L]=kd%256;
- ram_data[DARWIN_HEAD_PAN_D_H]=kd/256;
- head_tracking_pan.integral_clamp=i_clamp<<9;
- ram_data[DARWIN_HEAD_PAN_I_CLAMP_L]=i_clamp%256;
- ram_data[DARWIN_HEAD_PAN_I_CLAMP_H]=i_clamp/256;
-}
-
-void head_tracking_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp)
-{
- *kp=head_tracking_pan.kp;
- *ki=head_tracking_pan.ki;
- *kd=head_tracking_pan.kd;
- *i_clamp=head_tracking_pan.integral_clamp>>9;
-}
-
-void head_tracking_set_tilt_range(int16_t max_angle,int16_t min_angle)
-{
- head_tracking_tilt.max_angle=max_angle<<9;
- ram_data[DARWIN_HEAD_MAX_TILT_L]=max_angle%256;
- ram_data[DARWIN_HEAD_MAX_TILT_H]=max_angle/256;
- head_tracking_tilt.min_angle=min_angle<<9;
- ram_data[DARWIN_HEAD_MIN_TILT_L]=min_angle%256;
- ram_data[DARWIN_HEAD_MIN_TILT_H]=min_angle/256;
-}
-
-void head_tracking_get_tilt_range(int16_t *max_angle,int16_t *min_angle)
-{
- *max_angle=head_tracking_tilt.max_angle>>9;
- *min_angle=head_tracking_tilt.min_angle>>9;
-}
-
-void head_tracking_set_tilt_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp)
-{
- head_tracking_tilt.kp=kp;
- ram_data[DARWIN_HEAD_TILT_P_L]=kp%256;
- ram_data[DARWIN_HEAD_TILT_P_H]=kp/256;
- head_tracking_tilt.ki=ki;
- ram_data[DARWIN_HEAD_TILT_I_L]=ki%256;
- ram_data[DARWIN_HEAD_TILT_I_H]=ki/256;
- head_tracking_tilt.kd=kd;
- ram_data[DARWIN_HEAD_TILT_D_L]=kd%256;
- ram_data[DARWIN_HEAD_TILT_D_H]=kd/256;
- head_tracking_tilt.integral_clamp=i_clamp<<9;
- ram_data[DARWIN_HEAD_TILT_I_CLAMP_L]=i_clamp%256;
- ram_data[DARWIN_HEAD_TILT_I_CLAMP_H]=i_clamp/256;
-}
-
-void head_tracking_get_tilt_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp)
-{
- *kp=head_tracking_tilt.kp;
- *ki=head_tracking_tilt.ki;
- *kd=head_tracking_tilt.kd;
- *i_clamp=head_tracking_tilt.integral_clamp>>9;
-}
-
-void head_tracking_set_target_pan(int16_t target)
-{
- head_tracking_pan.target_angle=target<<9;
- ram_data[DARWIN_HEAD_PAN_TARGET_L]=target%256;
- ram_data[DARWIN_HEAD_PAN_TARGET_H]=target/256;
-}
-
-void head_tracking_set_target_tilt(int16_t target)
-{
- head_tracking_tilt.target_angle=target<<9;
- ram_data[DARWIN_HEAD_TILT_TARGET_L]=target%256;
- ram_data[DARWIN_HEAD_TILT_TARGET_H]=target/256;
-}
-
-// operation functions
-uint8_t head_tracking_in_range(unsigned short int address, unsigned short int length)
-{
- if(ram_in_window(HEAD_BASE_ADDRESS,HEAD_MEM_LENGTH,address,length) ||
- ram_in_window(HEAD_EEPROM_ADDRESS,HEAD_EEPROM_LENGTH,address,length))
- return 0x01;
- else
- return 0x00;
-}
-
-void head_tracking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-
-}
-
-void head_tracking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
- uint16_t kp,kd,ki,i_clamp;
- int16_t max,min,target;
-
- // head tracking control
- if(ram_in_range(DARWIN_HEAD_CNTRL,address,length))
- {
- if(data[DARWIN_HEAD_CNTRL-address]&HEAD_START)
- head_tracking_start();
- if(data[DARWIN_HEAD_CNTRL-address]&HEAD_STOP)
- head_tracking_stop();
- }
- if(ram_in_window(DARWIN_HEAD_PAN_P_L,8,address,length))
- {
- head_tracking_get_pan_pid(&kp,&kd,&ki,&i_clamp);
- if(ram_in_range(DARWIN_HEAD_PAN_P_L,address,length)) kp=(kp&0xFF00)|data[DARWIN_HEAD_PAN_P_L-address];
- if(ram_in_range(DARWIN_HEAD_PAN_P_H,address,length)) kp=(kp&0x00FF)|(data[DARWIN_HEAD_PAN_P_H-address]<<8);
- if(ram_in_range(DARWIN_HEAD_PAN_I_L,address,length)) ki=(ki&0xFF00)|data[DARWIN_HEAD_PAN_I_L-address];
- if(ram_in_range(DARWIN_HEAD_PAN_I_H,address,length)) ki=(ki&0x00FF)|(data[DARWIN_HEAD_PAN_I_H-address]<<8);
- if(ram_in_range(DARWIN_HEAD_PAN_D_L,address,length)) kd=(kd&0xFF00)|data[DARWIN_HEAD_PAN_D_L-address];
- if(ram_in_range(DARWIN_HEAD_PAN_D_H,address,length)) kd=(kd&0x00FF)|(data[DARWIN_HEAD_PAN_D_H-address]<<8);
- if(ram_in_range(DARWIN_HEAD_PAN_I_CLAMP_L,address,length)) i_clamp=(i_clamp&0xFF00)|data[DARWIN_HEAD_PAN_I_CLAMP_L-address];
- if(ram_in_range(DARWIN_HEAD_PAN_I_CLAMP_H,address,length)) i_clamp=(i_clamp&0x00FF)|(data[DARWIN_HEAD_PAN_I_CLAMP_H-address]<<8);
- head_tracking_set_pan_pid(kp,kd,ki,i_clamp);
- }
- if(ram_in_window(DARWIN_HEAD_TILT_P_L,8,address,length))
- {
- head_tracking_get_tilt_pid(&kp,&kd,&ki,&i_clamp);
- if(ram_in_range(DARWIN_HEAD_TILT_P_L,address,length)) kp=(kp&0xFF00)|data[DARWIN_HEAD_TILT_P_L-address];
- if(ram_in_range(DARWIN_HEAD_TILT_P_H,address,length)) kp=(kp&0x00FF)|(data[DARWIN_HEAD_TILT_P_H-address]<<8);
- if(ram_in_range(DARWIN_HEAD_TILT_I_L,address,length)) ki=(ki&0xFF00)|data[DARWIN_HEAD_TILT_I_L-address];
- if(ram_in_range(DARWIN_HEAD_TILT_I_H,address,length)) ki=(ki&0x00FF)|(data[DARWIN_HEAD_TILT_I_H-address]<<8);
- if(ram_in_range(DARWIN_HEAD_TILT_D_L,address,length)) kd=(kd&0xFF00)|data[DARWIN_HEAD_TILT_D_L-address];
- if(ram_in_range(DARWIN_HEAD_TILT_D_H,address,length)) kd=(kd&0x00FF)|(data[DARWIN_HEAD_TILT_D_H-address]<<8);
- if(ram_in_range(DARWIN_HEAD_TILT_I_CLAMP_L,address,length)) i_clamp=(i_clamp&0xFF00)|data[DARWIN_HEAD_TILT_I_CLAMP_L-address];
- if(ram_in_range(DARWIN_HEAD_TILT_I_CLAMP_H,address,length)) i_clamp=(i_clamp&0x00FF)|(data[DARWIN_HEAD_TILT_I_CLAMP_H-address]<<8);
- head_tracking_set_tilt_pid(kp,kd,ki,i_clamp);
- }
- if(ram_in_window(DARWIN_HEAD_MAX_PAN_L,4,address,length))
- {
- head_tracking_get_pan_range(&max,&min);
- if(ram_in_range(DARWIN_HEAD_MAX_PAN_L,address,length)) max=(max&0xFF00)|data[DARWIN_HEAD_MAX_PAN_L-address];
- if(ram_in_range(DARWIN_HEAD_MAX_PAN_H,address,length)) max=(max&0x00FF)|(data[DARWIN_HEAD_MAX_PAN_H-address]<<8);
- if(ram_in_range(DARWIN_HEAD_MIN_PAN_L,address,length)) min=(min&0xFF00)|data[DARWIN_HEAD_MIN_PAN_L-address];
- if(ram_in_range(DARWIN_HEAD_MIN_PAN_H,address,length)) min=(min&0x00FF)|(data[DARWIN_HEAD_MIN_PAN_H-address]<<8);
- head_tracking_set_pan_range(max,min);
- }
- if(ram_in_window(DARWIN_HEAD_MAX_TILT_L,4,address,length))
- {
- head_tracking_get_tilt_range(&max,&min);
- if(ram_in_range(DARWIN_HEAD_MAX_TILT_L,address,length)) max=(max&0xFF00)|data[DARWIN_HEAD_MAX_TILT_L-address];
- if(ram_in_range(DARWIN_HEAD_MAX_TILT_H,address,length)) max=(max&0x00FF)|(data[DARWIN_HEAD_MAX_TILT_H-address]<<8);
- if(ram_in_range(DARWIN_HEAD_MIN_TILT_L,address,length)) min=(min&0xFF00)|data[DARWIN_HEAD_MIN_TILT_L-address];
- if(ram_in_range(DARWIN_HEAD_MIN_TILT_H,address,length)) min=(min&0x00FF)|(data[DARWIN_HEAD_MIN_TILT_H-address]<<8);
- head_tracking_set_tilt_range(max,min);
- }
- if(ram_in_window(DARWIN_HEAD_PAN_TARGET_L,2,address,length))
- {
- target=(head_tracking_pan.target_angle>>9);
- if(ram_in_range(DARWIN_HEAD_PAN_TARGET_L,address,length))
- {
- target=(target&0xFF00)|data[DARWIN_HEAD_PAN_TARGET_L-address];
- ram_data[DARWIN_HEAD_PAN_TARGET_L]=data[DARWIN_HEAD_PAN_TARGET_L-address];
- }
- if(ram_in_range(DARWIN_HEAD_PAN_TARGET_H,address,length))
- {
- target=(target&0x00FF)|(data[DARWIN_HEAD_PAN_TARGET_H-address]<<8);
- ram_data[DARWIN_HEAD_PAN_TARGET_H]=data[DARWIN_HEAD_PAN_TARGET_H-address];
- }
- head_tracking_pan.target_angle=(target<<9);
- }
- if(ram_in_window(DARWIN_HEAD_TILT_TARGET_L,2,address,length))
- {
- target=(head_tracking_tilt.target_angle>>9);
- if(ram_in_range(DARWIN_HEAD_TILT_TARGET_L,address,length))
- {
- target=(target&0xFF00)|data[DARWIN_HEAD_TILT_TARGET_L-address];
- ram_data[DARWIN_HEAD_TILT_TARGET_L]=data[DARWIN_HEAD_TILT_TARGET_L-address];
- }
- if(ram_in_range(DARWIN_HEAD_TILT_TARGET_H,address,length))
- {
- target=(target&0x00FF)|(data[DARWIN_HEAD_TILT_TARGET_H-address]<<8);
- ram_data[DARWIN_HEAD_TILT_TARGET_H]=data[DARWIN_HEAD_TILT_TARGET_H-address];
- }
- head_tracking_tilt.target_angle=(target<<9);
- }
-}
-
-// motion manager process function
-void head_tracking_process(void)
-{
- int64_t pan_error, tilt_error;
- int64_t derivative_pan=0, derivative_tilt=0;
-
- if(head_tracking_enabled)
- {
- ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS;
- if(manager_get_module(L_PAN)==MM_HEAD)
- {
- pan_error = head_tracking_pan.target_angle-manager_current_angles[L_PAN];
- head_tracking_pan.integral_part+=(pan_error*head_tracking_period)>>16;
- if(head_tracking_pan.integral_part>head_tracking_pan.integral_clamp)
- head_tracking_pan.integral_part=head_tracking_pan.integral_clamp;
- else if(head_tracking_pan.integral_part<-head_tracking_pan.integral_clamp)
- head_tracking_pan.integral_part=-head_tracking_pan.integral_clamp;
- derivative_pan = ((pan_error - head_tracking_pan.prev_error)<<16)/head_tracking_period;
- manager_current_angles[L_PAN]+=((head_tracking_pan.kp*pan_error)>>16);
- manager_current_angles[L_PAN]+=((head_tracking_pan.ki*head_tracking_pan.integral_part)>>16);
- manager_current_angles[L_PAN]+=((head_tracking_pan.kd*derivative_pan)>>16);
- if(manager_current_angles[L_PAN]>head_tracking_pan.max_angle)
- manager_current_angles[L_PAN]=head_tracking_pan.max_angle;
- else if(manager_current_angles[L_PAN]<head_tracking_pan.min_angle)
- manager_current_angles[L_PAN]=head_tracking_pan.min_angle;
- head_tracking_pan.prev_error = pan_error;
- }
- if(manager_get_module(L_TILT)==MM_HEAD)
- {
- tilt_error = head_tracking_tilt.target_angle-manager_current_angles[L_TILT];
- head_tracking_tilt.integral_part+=(tilt_error*head_tracking_period)>>16;
- if(head_tracking_tilt.integral_part>head_tracking_tilt.integral_clamp)
- head_tracking_tilt.integral_part=head_tracking_tilt.integral_clamp;
- else if(head_tracking_tilt.integral_part<-head_tracking_tilt.integral_clamp)
- head_tracking_tilt.integral_part=-head_tracking_tilt.integral_clamp;
- derivative_tilt = ((tilt_error - head_tracking_tilt.prev_error)<<16)/head_tracking_period;
- manager_current_angles[L_TILT]+=((head_tracking_tilt.kp*tilt_error)>>16);
- manager_current_angles[L_TILT]+=((head_tracking_tilt.ki*head_tracking_tilt.integral_part)>>16);
- manager_current_angles[L_TILT]+=((head_tracking_tilt.kd*derivative_tilt)>>16);
- if(manager_current_angles[L_TILT]>head_tracking_tilt.max_angle)
- manager_current_angles[L_TILT]=head_tracking_tilt.max_angle;
- else if(manager_current_angles[L_TILT]<head_tracking_tilt.min_angle)
- manager_current_angles[L_TILT]=head_tracking_tilt.min_angle;
- head_tracking_tilt.prev_error = tilt_error;
- }
- }
- else
- ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS);
-}
-