Skip to content
Snippets Groups Projects
Commit 49f25fc1 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Migrated the head tracking motion module.

parent 90aaa517
No related branches found
No related tags found
No related merge requests found
Showing
with 538 additions and 437 deletions
#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
#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
#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
#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
#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
#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
#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
#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)
......
#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;
}
#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)
......
#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);
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment