diff --git a/dynamixel_manager/include/modules/action.h b/dynamixel_manager/include/modules/action.h
index 309c50ccca35e54327abf3c241ff714a3874ff73..da727468f95e769a37f04fa51b23077ac22fa1a8 100755
--- a/dynamixel_manager/include/modules/action.h
+++ b/dynamixel_manager/include/modules/action.h
@@ -7,7 +7,6 @@ extern "C" {
#include "motion_pages.h"
#include "motion_module.h"
-#include "motion_pages.h"
#include "memory.h"
typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states;
@@ -17,7 +16,6 @@ typedef struct{
TMemory *memory;
TMemModule mem_module;
unsigned short int ram_base_address;
- unsigned short int eeprom_base_address;
TPage next_page;
TPage current_page;
unsigned char current_step_index;
diff --git a/dynamixel_manager/include/modules/joint_motion.h b/dynamixel_manager/include/modules/joint_motion.h
new file mode 100644
index 0000000000000000000000000000000000000000..dce4992ad5f158bc9f9033a2db0a8c744aa5f13b
--- /dev/null
+++ b/dynamixel_manager/include/modules/joint_motion.h
@@ -0,0 +1,44 @@
+#ifndef _JOINT_MOTION_H
+#define _JOINT_MOTION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "motion_module.h"
+#include "memory.h"
+
+#define NUM_JOINT_GROUPS 4
+
+typedef struct{
+ TMotionModule mmodule;
+ TMemory *memory;
+ TMemModule mem_module;
+ unsigned short int ram_base_address;
+ long int target_angles[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
+ long int target_speeds[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
+ long int target_accels[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
+ long int target_stop_angles[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
+ long int current_angles[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
+ long int current_speeds[DYN_MANAGER_MAX_NUM_DEVICES];//48|16
+ char dir[DYN_MANAGER_MAX_NUM_DEVICES];// joint_direction
+ long int motion_period;
+ short int motion_period_ms;
+ unsigned char stop[NUM_JOINT_GROUPS];
+ unsigned char moving[NUM_JOINT_GROUPS];
+ unsigned int group_servos[NUM_JOINT_GROUPS];
+}TJointMModule;
+
+// public functions
+unsigned char joint_motion_init(TJointMModule *joint,TMemory *memory,unsigned short int ram_base_address);
+void joint_motion_set_group_servos(TJointMModule *joint,unsigned char group_id,unsigned int servos);
+unsigned int joint_motion_get_group_servos(TJointMModule *joint,unsigned char group_id);
+void joint_motion_start(TJointMModule *joint,unsigned char group_id);
+void joint_motion_stop(TJointMModule *joint,unsigned char group_id);
+unsigned char are_joints_moving(TJointMModule *joint,unsigned char group_id);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/dynamixel_manager/include/modules/joint_motion_mm_registers.h b/dynamixel_manager/include/modules/joint_motion_mm_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9e8e90a970620044f7f7c82d3712cb703d7ce14
--- /dev/null
+++ b/dynamixel_manager/include/modules/joint_motion_mm_registers.h
@@ -0,0 +1,16 @@
+#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/old/joint_motion.h b/dynamixel_manager/include/modules/old/joint_motion.h
deleted file mode 100644
index f2f0e1621409487cc494c876ab7d90b55a343028..0000000000000000000000000000000000000000
--- a/dynamixel_manager/include/modules/old/joint_motion.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#ifndef _JOINT_MOTION_H
-#define _JOINT_MOTION_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-#include "motion_manager.h"
-
-#define NUM_JOINT_GROUPS 4
-
-// public functions
-void joint_motion_init(uint16_t period_us);
-void joint_motion_set_period(uint16_t period_us);
-uint16_t joint_motion_get_period(void);
-void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos);
-uint32_t joint_motion_get_group_servos(uint8_t group_id);
-void joint_motion_start(uint8_t group_id);
-void joint_motion_stop(uint8_t group_id);
-uint8_t are_joints_moving(uint8_t group_id);
-
-// operation functions
-uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length);
-void joint_motion_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-void joint_motion_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-// motion manager interface functions
-void joint_motion_process(void);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/dynamixel_manager/src/modules/joint_motion.c b/dynamixel_manager/src/modules/joint_motion.c
new file mode 100644
index 0000000000000000000000000000000000000000..332b080cfcfbaf55a4f61e24f3aecf9e7503cfb2
--- /dev/null
+++ b/dynamixel_manager/src/modules/joint_motion.c
@@ -0,0 +1,295 @@
+#include "joint_motion.h"
+#include "joint_motion_mm_registers.h"
+#include "ram.h"
+#include <stdlib.h>
+
+// private functions
+void joint_motion_read_cmd(TJointMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(module->memory,address,length,data);
+}
+
+void joint_motion_write_cmd(TJointMModule *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ unsigned char j;
+ unsigned short int i;
+ unsigned int servos,ram_offset;
+
+ // 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];
+ // 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))
+ {
+ 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));
+ 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(data[JOINT_MOTION_MM_GROUP_CONTROL_OFFSET+j-ram_offset]&JOINT_MOTION_MM_START)
+ joint_motion_start(module,j);
+ if(data[JOINT_MOTION_MM_GROUP_CONTROL_OFFSET+j-ram_offset]&JOINT_MOTION_MM_STOP)
+ joint_motion_stop(module,j);
+ }
+ }
+}
+
+TMotionModule *joint_motion_get_module(TJointMModule *joint)
+{
+ return &joint->mmodule;
+}
+
+void joint_motion_set_period(void *module,unsigned short int period_ms)
+{
+ TJointMModule *joint=(TJointMModule *)module;
+
+ // load the current period
+ joint->motion_period=(period_ms<<16)/1000;
+ joint->motion_period_ms=period_ms;
+}
+
+void joint_motion_set_module(void *module,unsigned char servo_id)
+{
+
+}
+
+// motion manager interface functions
+void joint_motion_pre_process(void *module)
+{
+ TJointMModule *joint=(TJointMModule *)module;
+ unsigned char moving,i,j;
+ unsigned int servo_index;
+
+ for(j=0;j<NUM_JOINT_GROUPS;j++)
+ {
+ moving=0x00;
+ if(joint->moving[j]==0x01)
+ {
+ for(i=0,servo_index=0x00000001;i<DYN_MANAGER_MAX_NUM_DEVICES;i++,servo_index=servo_index<<1)
+ {
+ if(mmanager_get_module(joint->mmodule.manager,i)==MM_JOINTS && joint->group_servos[j]&servo_index)
+ {
+ if(joint->stop[j]==0x01)
+ {
+ joint->target_angles[i]=joint->mmodule.manager->servo_values[i].target_angle;
+ joint->target_speeds[i]=0;
+ joint->target_accels[i]=0;
+ joint->current_speeds[i]=0;
+ }
+ else
+ {
+ if(abs(joint->target_angles[i]-joint->current_angles[i])>=65)
+ {
+ moving=0x01;
+ if(joint->current_speeds[i]!=joint->dir[i]*joint->target_speeds[i])// it is necessary to change the current speed
+ {
+ joint->current_angles[i]+=((joint->current_speeds[i]*joint->motion_period)>>17);
+ if(joint->current_speeds[i]>joint->dir[i]*joint->target_speeds[i])
+ {
+ joint->current_speeds[i]-=((joint->target_accels[i]*joint->motion_period)>>16);// reduce speed
+ if(joint->current_speeds[i]<joint->dir[i]*joint->target_speeds[i])
+ joint->current_speeds[i]=joint->dir[i]*joint->target_speeds[i];
+ }
+ else
+ {
+ joint->current_speeds[i]+=((joint->target_accels[i]*joint->motion_period)>>16);// increase speed
+ if(joint->current_speeds[i]>joint->dir[i]*joint->target_speeds[i])
+ joint->current_speeds[i]=joint->dir[i]*joint->target_speeds[i];
+ }
+ joint->current_angles[i]+=((joint->current_speeds[i]*joint->motion_period)>>17);
+ if(joint->dir[i]==1 && joint->current_angles[i]>joint->target_angles[i])
+ joint->current_angles[i]=joint->target_angles[i];
+ if(joint->dir[i]==-1 && joint->current_angles[i]<joint->target_angles[i])
+ joint->current_angles[i]=joint->target_angles[i];
+ }
+ else
+ {
+ if(abs(joint->target_angles[i]-joint->current_angles[i])>joint->target_stop_angles[i])// constant speed phase
+ {
+ joint->current_angles[i]+=((joint->current_speeds[i]*joint->motion_period)>>16);
+ if(joint->dir[i]==1 && joint->current_angles[i]>joint->target_angles[i])
+ joint->current_angles[i]=joint->target_angles[i];
+ if(joint->dir[i]==-1 && joint->current_angles[i]<joint->target_angles[i])
+ joint->current_angles[i]=joint->target_angles[i];
+ }
+ else// deceleration phase
+ {
+ joint->current_angles[i]=joint->current_angles[i]+((joint->current_speeds[i]*joint->motion_period)>>17);
+ joint->target_speeds[i]=joint->target_speeds[i]-((joint->target_accels[i]*joint->motion_period)>>16);
+ if(joint->target_speeds[i]<0)
+ joint->target_speeds[i]=0;
+ joint->current_speeds[i]=joint->dir[i]*joint->target_speeds[i];
+ joint->current_angles[i]=joint->current_angles[i]+((joint->current_speeds[i]*joint->motion_period)>>17);
+ if(joint->dir[i]==1 && joint->current_angles[i]>joint->target_angles[i])
+ joint->current_angles[i]=joint->target_angles[i];
+ if(joint->dir[i]==-1 && joint->current_angles[i]<joint->target_angles[i])
+ joint->current_angles[i]=joint->target_angles[i];
+ }
+ }
+ }
+ else
+ {
+ joint->current_speeds[i]=0;
+ }
+ joint->mmodule.manager->servo_values[i].target_angle=joint->current_angles[i];
+ if(joint->mmodule.manager->servo_configs[i]->pid)
+ {
+ }
+ else
+ {
+ joint->mmodule.manager->servo_values[i].cw_compliance=5;
+ joint->mmodule.manager->servo_values[i].ccw_compliance=5;
+ }
+ }
+ }
+ }
+ if(moving==0)
+ {
+ 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);
+ }
+ }
+ }
+}
+
+// public functions
+unsigned char joint_motion_init(TJointMModule *joint,TMemory *memory,unsigned short int ram_base_address)
+{
+ unsigned char i;
+
+ /* initialize the internal variables */
+ for(i=0;i<DYN_MANAGER_MAX_NUM_DEVICES;i++)
+ {
+ // target values
+ joint->target_angles[i]=0;
+ joint->target_speeds[i]=0;
+ joint->target_accels[i]=0;
+ // current values
+ joint->current_angles[i]=0;
+ joint->current_speeds[i]=0;
+ // the current angles, speeds and accelerations are in RAM
+ joint->dir[i]=0;
+ }
+ for(i=0;i<NUM_JOINT_GROUPS;i++)
+ {
+ joint->stop[i]=0x00;
+ joint->moving[i]=0x00;
+ joint->group_servos[i]=0x00000000;
+ }
+
+ /* initialize the motion module */
+ mmodule_init(&joint->mmodule);
+ joint->mmodule.id=MM_JOINTS;
+ joint->mmodule.set_period=joint_motion_set_period;
+ joint->mmodule.set_module=joint_motion_set_module;
+ joint->mmodule.pre_process=joint_motion_pre_process;
+ joint->mmodule.data=joint;
+
+ mem_module_init(&joint->mem_module);
+ 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))
+ return 0x00;
+ if(!mem_add_module(memory,&joint->mem_module))
+ return 0x00;
+ joint->ram_base_address=ram_base_address;
+ joint->memory=memory;
+
+ return 0x01;
+}
+
+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;
+}
+
+unsigned int joint_motion_get_group_servos(TJointMModule *joint,unsigned char group_id)
+{
+ return joint->group_servos[group_id];
+}
+
+void joint_motion_start(TJointMModule *joint,unsigned char group_id)
+{
+ unsigned char i;
+ short int angle;
+ unsigned int servo_index;
+
+ /* initialize the internal variables */
+ for(i=0,servo_index=0x00000001;i<DYN_MANAGER_MAX_NUM_DEVICES;i++,servo_index=servo_index<<1)
+ {
+ if(joint->group_servos[group_id]&servo_index)
+ {
+ if(joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_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;
+ }
+ 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);
+ 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;
+ // target accel
+ joint->target_accels[i]=joint->memory->data[joint->ram_base_address+JOINT_MOTION_MM_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;
+ }
+ // check the parameters
+ if(abs(joint->target_angles[i]-joint->current_angles[i])>=65)
+ {
+ 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;
+ }
+ }
+ // stop angles
+ joint->target_stop_angles[i]=joint->target_speeds[i]*joint->target_speeds[i]/(2*joint->target_accels[i]);
+ // the current angles, speeds and accelerations are in RAM
+ if(joint->target_angles[i]>=joint->current_angles[i])
+ joint->dir[i]=1;
+ else
+ joint->dir[i]=-1;
+ }
+ }
+ }
+ 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;
+}
+
+void joint_motion_stop(TJointMModule *joint,unsigned char group_id)
+{
+ joint->stop[group_id]=0x01;
+}
+
+unsigned char are_joints_moving(TJointMModule *joint,unsigned char group_id)
+{
+ return joint->moving[group_id];
+}
diff --git a/dynamixel_manager/src/modules/old/joint_motion.c b/dynamixel_manager/src/modules/old/joint_motion.c
deleted file mode 100644
index bd537d08b82d5557a02bbcbcc8535aae354557ba..0000000000000000000000000000000000000000
--- a/dynamixel_manager/src/modules/old/joint_motion.c
+++ /dev/null
@@ -1,285 +0,0 @@
-#include "joint_motion.h"
-#include "ram.h"
-#include <stdlib.h>
-
-// private variables
-int64_t joint_target_angles[MANAGER_MAX_NUM_SERVOS];//48|16
-int64_t joint_target_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
-int64_t joint_target_accels[MANAGER_MAX_NUM_SERVOS];//48|16
-int64_t joint_target_stop_angles[MANAGER_MAX_NUM_SERVOS];//48|16
-
-int64_t joint_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16
-int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
-
-int8_t joint_dir[MANAGER_MAX_NUM_SERVOS];// joint_direction
-
-int64_t joint_motion_period;
-int16_t joint_motion_period_us;
-
-uint8_t joint_stop[NUM_JOINT_GROUPS];
-uint8_t joint_moving[NUM_JOINT_GROUPS];
-uint32_t joint_group_servos[NUM_JOINT_GROUPS];
-
-// public functions
-void joint_motion_init(uint16_t period_us)
-{
- uint8_t i;
-
- /* initialize the internal variables */
- for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
- {
- // target values
- joint_target_angles[i]=manager_current_angles[i];
- joint_target_speeds[i]=0;
- joint_target_accels[i]=0;
- // current values
- joint_current_angles[i]=manager_current_angles[i];
- joint_current_speeds[i]=0;
- // the current angles, speeds and accelerations are in RAM
- joint_dir[i]=0;
- }
- for(i=0;i<NUM_JOINT_GROUPS;i++)
- {
- joint_stop[i]=0x00;
- joint_moving[i]=0x00;
- joint_group_servos[i]=0x00000000;
- }
- joint_motion_period=(period_us<<16)/1000000;
- joint_motion_period_us=period_us;
-}
-
-void joint_motion_set_period(uint16_t period_us)
-{
- joint_motion_period=(period_us<<16)/1000000;
- joint_motion_period_us=period_us;
-}
-
-uint16_t joint_motion_get_period(void)
-{
- return joint_motion_period_us;
-}
-
-void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos)
-{
- joint_group_servos[group_id]=servos;
- ram_data[DARWIN_JOINT_GRP0_SERVOS0+group_id*5]=servos&0x000000FF;
- ram_data[DARWIN_JOINT_GRP0_SERVOS1+group_id*5]=(servos>>8)&0x000000FF;
- ram_data[DARWIN_JOINT_GRP0_SERVOS2+group_id*5]=(servos>>16)&0x000000FF;
- ram_data[DARWIN_JOINT_GRP0_SERVOS3+group_id*5]=(servos>>24)&0x000000FF;
-}
-
-uint32_t joint_motion_get_group_servos(uint8_t group_id)
-{
- return joint_group_servos[group_id];
-}
-
-void joint_motion_start(uint8_t group_id)
-{
- uint8_t i;
- int16_t angle;
- uint32_t servo_index;
-
- /* initialize the internal variables */
- for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1)
- {
- if(joint_group_servos[group_id]&servo_index)
- {
- if(ram_data[DARWIN_JOINT_SERVO0_SPEED+i]==0)
- {
- joint_target_angles[i]=manager_current_angles[i];
- joint_current_angles[i]=manager_current_angles[i];
- angle=manager_current_angles[i]>>9;
- ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+i*2]=angle%256;
- ram_data[DARWIN_JOINT_SERVO0_ANGLE_H+i*2]=angle/256;
- }
- else
- {
- // current values
- joint_current_angles[i]=manager_current_angles[i];
- // target angle
- angle=ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+i*2]+(ram_data[DARWIN_JOINT_SERVO0_ANGLE_H+i*2]<<8);
- if(angle>manager_get_ccw_angle_limit(i))
- angle=manager_get_ccw_angle_limit(i);
- else if(angle<manager_get_cw_angle_limit(i))
- angle=manager_get_cw_angle_limit(i);
- joint_target_angles[i]=angle<<9;
- // target speed
- joint_target_speeds[i]=ram_data[DARWIN_JOINT_SERVO0_SPEED+i]<<16;
- // target speed
- joint_target_accels[i]=ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]<<16;
- if(joint_target_accels[i]==0)
- {
- joint_target_accels[i]=1<<16;
- ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=1;
- }
- // check the parameters
- if(abs(joint_target_angles[i]-joint_current_angles[i])>=65)
- {
- 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]);
- ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16;
- }
- }
- // stop angles
- joint_target_stop_angles[i]=joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i]);
- // the current angles, speeds and accelerations are in RAM
- if(joint_target_angles[i]>=joint_current_angles[i])
- joint_dir[i]=1;
- else
- joint_dir[i]=-1;
- }
- }
- }
- joint_moving[group_id]=0x01;
- joint_stop[group_id]=0x00;
- ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS;
-}
-
-void joint_motion_stop(uint8_t group_id)
-{
- joint_stop[group_id]=0x01;
-}
-
-uint8_t are_joints_moving(uint8_t group_id)
-{
- return joint_moving[group_id];
-}
-
-// operation functions
-uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length)
-{
- if(ram_in_window(JOINT_BASE_ADDRESS,JOINT_MEM_LENGTH,address,length))
- return 0x01;
- else
- return 0x00;
-}
-
-void joint_motion_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
-
-}
-
-void joint_motion_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
- uint8_t j,k;
- uint16_t i;
- uint32_t servos;
-
- // load motion information
- for(i=DARWIN_JOINT_SERVO0_ANGLE_L;i<=DARWIN_JOINT_SERVO31_ACCEL;i++)
- if(ram_in_range(i,address,length))
- ram_data[i]=data[i-address];
- // group servos
- for(j=0;j<4;j++)
- {
- if(ram_in_window(DARWIN_JOINT_GRP0_SERVOS0+j*5,4,address,length))
- {
- servos=joint_motion_get_group_servos(j);
- for(i=DARWIN_JOINT_GRP0_SERVOS0+j*5,k=0;i<=DARWIN_JOINT_GRP0_SERVOS3+j*5;i++,k++)
- if(ram_in_range(i,address,length))
- servos|=(data[i-address]<<(k*8));
- joint_motion_set_group_servos(j,servos);
- }
- if(ram_in_range(DARWIN_JOINT_GRP0_CNTRL+j*5,address,length))
- {
- if(data[DARWIN_JOINT_GRP0_CNTRL+j*5-address]&JOINT_START)
- joint_motion_start(j);
- if(data[DARWIN_JOINT_GRP0_CNTRL+j*5-address]&JOINT_STOP)
- joint_motion_stop(j);
- }
- }
-}
-
-// motion manager interface functions
-void joint_motion_process(void)
-{
- uint8_t moving,i,j;
- uint32_t servo_index;
-
- for(j=0;j<4;j++)
- {
- moving=0x00;
- if(joint_moving[j]==0x01)
- {
- for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1)
- {
- if(manager_get_module(i)==MM_JOINTS && joint_group_servos[j]&servo_index)
- {
- if(joint_stop[j]==0x01)
- {
- joint_target_angles[i]=manager_current_angles[i];
- joint_target_speeds[i]=0;
- joint_target_accels[i]=0;
- joint_current_speeds[i]=0;
- }
- else
- {
- if(abs(joint_target_angles[i]-joint_current_angles[i])>=65)
- {
- moving=0x01;
- if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])// it is necessary to change the current speed
- {
- joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17);
- if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
- {
- joint_current_speeds[i]-=((joint_target_accels[i]*joint_motion_period)>>16);// reduce speed
- if(joint_current_speeds[i]<joint_dir[i]*joint_target_speeds[i])
- joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
- }
- else
- {
- joint_current_speeds[i]+=((joint_target_accels[i]*joint_motion_period)>>16);// increase speed
- if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
- joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
- }
- joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17);
- if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
- joint_current_angles[i]=joint_target_angles[i];
- if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
- joint_current_angles[i]=joint_target_angles[i];
- }
- else
- {
- if(abs(joint_target_angles[i]-joint_current_angles[i])>joint_target_stop_angles[i])// constant speed phase
- {
- joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
- if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
- joint_current_angles[i]=joint_target_angles[i];
- if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
- joint_current_angles[i]=joint_target_angles[i];
- }
- else// deceleration phase
- {
- joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17);
- joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
- if(joint_target_speeds[i]<0)
- joint_target_speeds[i]=0;
- joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
- joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17);
- if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
- joint_current_angles[i]=joint_target_angles[i];
- if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
- joint_current_angles[i]=joint_target_angles[i];
- }
- }
- }
- else
- {
- joint_current_speeds[i]=0;
- }
- manager_current_angles[i]=joint_current_angles[i];
- manager_current_slopes[i]=5;
- }
- }
- }
- if(moving==0)
- {
- joint_moving[j]=0x00;
- joint_stop[j]=0x00;
- ram_data[DARWIN_JOINT_GRP0_CNTRL+j*5]&=(~JOINT_STATUS);
- }
- }
- }
-}
-