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

Moved the motion modules to the stm32_libraries repository.

parent 159bdefd
No related branches found
No related tags found
No related merge requests found
#ifndef _ACTION_H
#define _ACTION_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f1xx.h"
#include "motion_pages.h"
// public functions
void action_init(uint16_t period_us);
void action_set_period(uint16_t period_us);
uint16_t action_get_period(void);
uint8_t action_load_page(uint8_t page_id);
void action_start_page(void);
void action_stop_page(void);
uint8_t action_is_running(void);
uint8_t action_get_current_page(void);
uint8_t action_get_current_step(void);
void action_enable_int(void);
void action_disable_int(void);
uint8_t action_is_int_enabled(void);
// operation functions
uint8_t action_in_range(unsigned short int address, unsigned short int length);
void action_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
// motion manager interface
void action_process(void);
#ifdef __cplusplus
}
#endif
#endif
#ifndef _DARWIN_KINEMATICS_H
#define _DARWIN_KINEMATICS_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f1xx.h"
#define DARWIN_PELVIS_LENGTH 29.0 //mm
#define DARWIN_LEG_SIDE_OFFSET 37.0 //mm
#define DARWIN_THIGH_LENGTH 93.0 //mm
#define DARWIN_CALF_LENGTH 93.0 //mm
#define DARWIN_ANKLE_LENGTH 33.5 //mm
#define DARWIN_LEG_LENGTH (DARWIN_THIGH_LENGTH + DARWIN_CALF_LENGTH + DARWIN_ANKLE_LENGTH) //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
// public functions
uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c);
#ifdef __cplusplus
}
#endif
#endif
#ifndef _DARWIN_MATH_H
#define _DARWIN_MATH_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f1xx.h"
#define PI 3.14159
typedef struct{
float x;
float y;
float z;
}TPoint3D;
// point public functions
void point3d_init(TPoint3D *point);
void point3d_load(TPoint3D *point, float x, float y, float z);
void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src);
typedef struct {
float x;
float y;
float z;
}TVector3D;
// vector public functions
void vector3d_init(TVector3D *vector);
void vector3d_load(TVector3D *vector, float x, float y, float z);
void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src);
float vector3d_length(TVector3D *vector);
enum{
m00=0,
m01,
m02,
m03,
m10,
m11,
m12,
m13,
m20,
m21,
m22,
m23,
m30,
m31,
m32,
m33,
};
typedef struct{
float coef[16];
}TMatrix3D;
// matrix public functions
void matrix3d_init(TMatrix3D *matrix);
void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src);
void matrix3d_identity(TMatrix3D *matrix);
uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv);
void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector);
void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b);
#ifdef __cplusplus
}
#endif
#endif
#ifndef _GRIPPERS_H
#define _GRIPPERS_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f1xx.h"
#include "motion_manager.h"
typedef enum {GRIPPER_LEFT,GRIPPER_RIGHT} grippers_t;
// public functions
void grippers_init(uint16_t period_us);
void grippers_set_period(uint16_t period_us);
uint16_t grippers_get_period(void);
void grippers_open(grippers_t gripper);
void grippers_close(grippers_t gripper);
void grippers_stop(grippers_t gripper);
uint8_t gripper_is_open(grippers_t gripper);
uint8_t gripper_is_close(grippers_t gripper);
uint8_t gripper_is_moving(grippers_t gripper);
// operation functions
uint8_t grippers_in_range(unsigned short int address, unsigned short int length);
void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
void grippers_process(void);
#ifdef __cplusplus
}
#endif
#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
#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
#ifndef _STAIRS_H
#define _STAIRS_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f1xx.h"
#include "motion_manager.h"
// public functions
void stairs_init(uint16_t period_us);
void stairs_set_period(uint16_t period_us);
uint16_t stairs_get_period(void);
void stairs_start(uint8_t up);
void stairs_stop(void);
uint8_t is_climbing_stairs(void);
uint8_t stairs_get_phase(void);
// operation functions
uint8_t stairs_in_range(unsigned short int address, unsigned short int length);
void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
// motion manager interface functions
void stairs_process(void);
#ifdef __cplusplus
}
#endif
#endif
#ifndef _WALKING_H
#define _WALKING_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f1xx.h"
#include "motion_manager.h"
// public functions
void walking_init(uint16_t period_us);
void walking_set_period(uint16_t period_us);
uint16_t walking_get_period(void);
void walking_start(void);
void walking_stop(void);
uint8_t is_walking(void);
// operation functions
uint8_t walking_in_range(unsigned short int address, unsigned short int length);
void walking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
void walking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
// motion manager interface functions
void walking_process(void);
#ifdef __cplusplus
}
#endif
#endif
This diff is collapsed.
#include "darwin_kinematics.h"
#include "darwin_math.h"
#include <math.h>
uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c)
{
TMatrix3D Tad, Tda, Tcd, Tdc, Tac;
TVector3D vec,orientation;
TPoint3D position;
float _Rac, _Acos, _Atan, _k, _l, _m, _n, _s, _c, _theta;
vector3d_load(&orientation,a * 180.0 / PI, b * 180.0 / PI, c * 180.0 / PI);
point3d_load(&position,x, y, z - DARWIN_LEG_LENGTH);
matrix3d_set_transform(&Tad,&position,&orientation);
vec.x = x + Tad.coef[2] * DARWIN_ANKLE_LENGTH;
vec.y = y + Tad.coef[6] * DARWIN_ANKLE_LENGTH;
vec.z = (z - DARWIN_LEG_LENGTH) + Tad.coef[10] * DARWIN_ANKLE_LENGTH;
// Get Knee
_Rac = vector3d_length(&vec);
_Acos = acos((_Rac * _Rac - DARWIN_THIGH_LENGTH * DARWIN_THIGH_LENGTH - DARWIN_CALF_LENGTH * DARWIN_CALF_LENGTH) / (2 * DARWIN_THIGH_LENGTH * DARWIN_CALF_LENGTH));
if(isnan(_Acos) == 1)
return 0x00;;
*(out + 3) = _Acos;
// Get Ankle Roll
Tda = Tad;
if(matrix3d_inverse(&Tda,&Tda) == 0x00)
return 0x00;
_k = sqrt(Tda.coef[7] * Tda.coef[7] + Tda.coef[11] * Tda.coef[11]);
_l = sqrt(Tda.coef[7] * Tda.coef[7] + (Tda.coef[11] - DARWIN_ANKLE_LENGTH) * (Tda.coef[11] - DARWIN_ANKLE_LENGTH));
_m = (_k * _k - _l * _l - DARWIN_ANKLE_LENGTH * DARWIN_ANKLE_LENGTH) / (2 * _l * DARWIN_ANKLE_LENGTH);
if(_m > 1.0)
_m = 1.0;
else if(_m < -1.0)
_m = -1.0;
_Acos = acos(_m);
if(isnan(_Acos) == 1)
return 0x00;
if(Tda.coef[7] < 0.0)
*(out + 5) = -_Acos;
else
*(out + 5) = _Acos;
// Get Hip Yaw
vector3d_load(&orientation,*(out + 5) * 180.0 / PI, 0, 0);
point3d_load(&position,0, 0, -DARWIN_ANKLE_LENGTH);
matrix3d_set_transform(&Tcd,&position,&orientation);
Tdc = Tcd;
if(matrix3d_inverse(&Tdc,&Tdc) == 0x00)
return 0x00;
matrix3d_mult(&Tac,&Tad,&Tdc);
_Atan = atan2(-Tac.coef[1] , Tac.coef[5]);
if(isinf(_Atan) == 1)
return 0x00;
*(out) = _Atan;
// Get Hip Roll
_Atan = atan2(Tac.coef[9], -Tac.coef[1] * sin(*(out)) + Tac.coef[5] * cos(*(out)));
if(isinf(_Atan) == 1)
return 0x00;
*(out + 1) = _Atan;
// Get Hip Pitch and Ankle Pitch
_Atan = atan2(Tac.coef[2] * cos(*(out)) + Tac.coef[6] * sin(*(out)), Tac.coef[0] * cos(*(out)) + Tac.coef[4] * sin(*(out)));
if(isinf(_Atan) == 1)
return 0x00;
_theta = _Atan;
_k = sin(*(out + 3)) * DARWIN_CALF_LENGTH;
_l = -DARWIN_THIGH_LENGTH - cos(*(out + 3)) * DARWIN_CALF_LENGTH;
_m = cos(*(out)) * vec.x + sin(*(out)) * vec.y;
_n = cos(*(out + 1)) * vec.z + sin(*(out)) * sin(*(out + 1)) * vec.x - cos(*(out)) * sin(*(out + 1)) * vec.y;
_s = (_k * _n + _l * _m) / (_k * _k + _l * _l);
_c = (_n - _k * _s) / _l;
_Atan = atan2(_s, _c);
if(isinf(_Atan) == 1)
return 0x00;
*(out + 2) = _Atan;
*(out + 4) = _theta - *(out + 3) - *(out + 2);
return 0x01;
}
#include "darwin_math.h"
#include <math.h>
// point functions
void point3d_init(TPoint3D *point)
{
point->x=0.0;
point->y=0.0;
point->z=0.0;
}
void point3d_load(TPoint3D *point, float x, float y,float z)
{
point->x=x;
point->y=y;
point->z=z;
}
void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src)
{
point_dst->x=point_src->x;
point_dst->y=point_src->y;
point_dst->z=point_src->z;
}
// vector functions
void vector3d_init(TVector3D *vector)
{
vector->x=0.0;
vector->y=0.0;
vector->z=0.0;
}
void vector3d_load(TVector3D *vector, float x, float y, float z)
{
vector->x=x;
vector->y=y;
vector->z=z;
}
void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src)
{
vector_dst->x=vector_src->x;
vector_dst->y=vector_src->y;
vector_dst->z=vector_src->z;
}
float vector3d_length(TVector3D *vector)
{
return sqrt(vector->x*vector->x + vector->y*vector->y + vector->z*vector->z);
}
// matrix functions
void matrix3d_init(TMatrix3D *matrix)
{
matrix3d_identity(matrix);
}
void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src)
{
matrix_dst->coef[m00]=matrix_src->coef[m00];
matrix_dst->coef[m01]=matrix_src->coef[m01];
matrix_dst->coef[m02]=matrix_src->coef[m02];
matrix_dst->coef[m03]=matrix_src->coef[m03];
matrix_dst->coef[m10]=matrix_src->coef[m10];
matrix_dst->coef[m11]=matrix_src->coef[m11];
matrix_dst->coef[m12]=matrix_src->coef[m12];
matrix_dst->coef[m13]=matrix_src->coef[m13];
matrix_dst->coef[m20]=matrix_src->coef[m20];
matrix_dst->coef[m21]=matrix_src->coef[m21];
matrix_dst->coef[m22]=matrix_src->coef[m22];
matrix_dst->coef[m23]=matrix_src->coef[m23];
matrix_dst->coef[m30]=matrix_src->coef[m30];
matrix_dst->coef[m31]=matrix_src->coef[m31];
matrix_dst->coef[m32]=matrix_src->coef[m32];
matrix_dst->coef[m33]=matrix_src->coef[m33];
}
void matrix3d_zero(TMatrix3D *matrix)
{
matrix->coef[m00]=0.0;
matrix->coef[m01]=0.0;
matrix->coef[m02]=0.0;
matrix->coef[m03]=0.0;
matrix->coef[m10]=0.0;
matrix->coef[m11]=0.0;
matrix->coef[m12]=0.0;
matrix->coef[m13]=0.0;
matrix->coef[m20]=0.0;
matrix->coef[m21]=0.0;
matrix->coef[m22]=0.0;
matrix->coef[m23]=0.0;
matrix->coef[m30]=0.0;
matrix->coef[m31]=0.0;
matrix->coef[m32]=0.0;
matrix->coef[m33]=0.0;
}
void matrix3d_identity(TMatrix3D *matrix)
{
matrix->coef[m00]=1.0;
matrix->coef[m01]=0.0;
matrix->coef[m02]=0.0;
matrix->coef[m03]=0.0;
matrix->coef[m10]=0.0;
matrix->coef[m11]=1.0;
matrix->coef[m12]=0.0;
matrix->coef[m13]=0.0;
matrix->coef[m20]=0.0;
matrix->coef[m21]=0.0;
matrix->coef[m22]=1.0;
matrix->coef[m23]=0.0;
matrix->coef[m30]=0.0;
matrix->coef[m31]=0.0;
matrix->coef[m32]=0.0;
matrix->coef[m33]=1.0;
}
uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv)
{
TMatrix3D src, tmp;
float det;
uint8_t i;
/* transpose matrix */
for (i = 0; i < 4; i++)
{
src.coef[i] = org->coef[i*4];
src.coef[i + 4] = org->coef[i*4 + 1];
src.coef[i + 8] = org->coef[i*4 + 2];
src.coef[i + 12] = org->coef[i*4 + 3];
}
/* calculate pairs for first 8 elements (cofactors) */
tmp.coef[0] = src.coef[10] * src.coef[15];
tmp.coef[1] = src.coef[11] * src.coef[14];
tmp.coef[2] = src.coef[9] * src.coef[15];
tmp.coef[3] = src.coef[11] * src.coef[13];
tmp.coef[4] = src.coef[9] * src.coef[14];
tmp.coef[5] = src.coef[10] * src.coef[13];
tmp.coef[6] = src.coef[8] * src.coef[15];
tmp.coef[7] = src.coef[11] * src.coef[12];
tmp.coef[8] = src.coef[8] * src.coef[14];
tmp.coef[9] = src.coef[10] * src.coef[12];
tmp.coef[10] = src.coef[8] * src.coef[13];
tmp.coef[11] = src.coef[9] * src.coef[12];
/* calculate first 8 elements (cofactors) */
inv->coef[0] = (tmp.coef[0]*src.coef[5] + tmp.coef[3]*src.coef[6] + tmp.coef[4]*src.coef[7]) - (tmp.coef[1]*src.coef[5] + tmp.coef[2]*src.coef[6] + tmp.coef[5]*src.coef[7]);
inv->coef[1] = (tmp.coef[1]*src.coef[4] + tmp.coef[6]*src.coef[6] + tmp.coef[9]*src.coef[7]) - (tmp.coef[0]*src.coef[4] + tmp.coef[7]*src.coef[6] + tmp.coef[8]*src.coef[7]);
inv->coef[2] = (tmp.coef[2]*src.coef[4] + tmp.coef[7]*src.coef[5] + tmp.coef[10]*src.coef[7]) - (tmp.coef[3]*src.coef[4] + tmp.coef[6]*src.coef[5] + tmp.coef[11]*src.coef[7]);
inv->coef[3] = (tmp.coef[5]*src.coef[4] + tmp.coef[8]*src.coef[5] + tmp.coef[11]*src.coef[6]) - (tmp.coef[4]*src.coef[4] + tmp.coef[9]*src.coef[5] + tmp.coef[10]*src.coef[6]);
inv->coef[4] = (tmp.coef[1]*src.coef[1] + tmp.coef[2]*src.coef[2] + tmp.coef[5]*src.coef[3]) - (tmp.coef[0]*src.coef[1] + tmp.coef[3]*src.coef[2] + tmp.coef[4]*src.coef[3]);
inv->coef[5] = (tmp.coef[0]*src.coef[0] + tmp.coef[7]*src.coef[2] + tmp.coef[8]*src.coef[3]) - (tmp.coef[1]*src.coef[0] + tmp.coef[6]*src.coef[2] + tmp.coef[9]*src.coef[3]);
inv->coef[6] = (tmp.coef[3]*src.coef[0] + tmp.coef[6]*src.coef[1] + tmp.coef[11]*src.coef[3]) - (tmp.coef[2]*src.coef[0] + tmp.coef[7]*src.coef[1] + tmp.coef[10]*src.coef[3]);
inv->coef[7] = (tmp.coef[4]*src.coef[0] + tmp.coef[9]*src.coef[1] + tmp.coef[10]*src.coef[2]) - (tmp.coef[5]*src.coef[0] + tmp.coef[8]*src.coef[1] + tmp.coef[11]*src.coef[2]);
/* calculate pairs for second 8 elements (cofactors) */
tmp.coef[0] = src.coef[2]*src.coef[7];
tmp.coef[1] = src.coef[3]*src.coef[6];
tmp.coef[2] = src.coef[1]*src.coef[7];
tmp.coef[3] = src.coef[3]*src.coef[5];
tmp.coef[4] = src.coef[1]*src.coef[6];
tmp.coef[5] = src.coef[2]*src.coef[5];
//Streaming SIMD Extensions - Inverse of 4x4 Matrix 8
tmp.coef[6] = src.coef[0]*src.coef[7];
tmp.coef[7] = src.coef[3]*src.coef[4];
tmp.coef[8] = src.coef[0]*src.coef[6];
tmp.coef[9] = src.coef[2]*src.coef[4];
tmp.coef[10] = src.coef[0]*src.coef[5];
tmp.coef[11] = src.coef[1]*src.coef[4];
/* calculate second 8 elements (cofactors) */
inv->coef[8] = (tmp.coef[0]*src.coef[13] + tmp.coef[3]*src.coef[14] + tmp.coef[4]*src.coef[15]) - (tmp.coef[1]*src.coef[13] + tmp.coef[2]*src.coef[14] + tmp.coef[5]*src.coef[15]);
inv->coef[9] = (tmp.coef[1]*src.coef[12] + tmp.coef[6]*src.coef[14] + tmp.coef[9]*src.coef[15]) - (tmp.coef[0]*src.coef[12] + tmp.coef[7]*src.coef[14] + tmp.coef[8]*src.coef[15]);
inv->coef[10] = (tmp.coef[2]*src.coef[12] + tmp.coef[7]*src.coef[13] + tmp.coef[10]*src.coef[15]) - (tmp.coef[3]*src.coef[12] + tmp.coef[6]*src.coef[13] + tmp.coef[11]*src.coef[15]);
inv->coef[11] = (tmp.coef[5]*src.coef[12] + tmp.coef[8]*src.coef[13] + tmp.coef[11]*src.coef[14]) - (tmp.coef[4]*src.coef[12] + tmp.coef[9]*src.coef[13] + tmp.coef[10]*src.coef[14]);
inv->coef[12] = (tmp.coef[2]*src.coef[10] + tmp.coef[5]*src.coef[11] + tmp.coef[1]*src.coef[9]) - (tmp.coef[4]*src.coef[11] + tmp.coef[0]*src.coef[9] + tmp.coef[3]*src.coef[10]);
inv->coef[13] = (tmp.coef[8]*src.coef[11] + tmp.coef[0]*src.coef[8] + tmp.coef[7]*src.coef[10]) - (tmp.coef[6]*src.coef[10] + tmp.coef[9]*src.coef[11] + tmp.coef[1]*src.coef[8]);
inv->coef[14] = (tmp.coef[6]*src.coef[9] + tmp.coef[11]*src.coef[11] + tmp.coef[3]*src.coef[8]) - (tmp.coef[10]*src.coef[11] + tmp.coef[2]*src.coef[8] + tmp.coef[7]*src.coef[9]);
inv->coef[15] = (tmp.coef[10]*src.coef[10] + tmp.coef[4]*src.coef[8] + tmp.coef[9]*src.coef[9]) - (tmp.coef[8]*src.coef[9] + tmp.coef[11]*src.coef[10] + tmp.coef[5]*src.coef[8]);
/* calculate determinant */
det = src.coef[0]*inv->coef[0] + src.coef[1]*inv->coef[1] + src.coef[2]*inv->coef[2] + src.coef[3]*inv->coef[3];
/* calculate matrix inverse */
if (det == 0)
{
det = 0;
return 0x00;
}
else
{
det = 1 / det;
}
for (i = 0; i < 16; i++)
inv->coef[i]*=det;
return 0x01;
}
void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector)
{
float Cx = cos(vector->x * 3.141592 / 180.0);
float Cy = cos(vector->y * 3.141592 / 180.0);
float Cz = cos(vector->z * 3.141592 / 180.0);
float Sx = sin(vector->x * 3.141592 / 180.0);
float Sy = sin(vector->y * 3.141592 / 180.0);
float Sz = sin(vector->z * 3.141592 / 180.0);
matrix3d_identity(matrix);
matrix->coef[0] = Cz * Cy;
matrix->coef[1] = Cz * Sy * Sx - Sz * Cx;
matrix->coef[2] = Cz * Sy * Cx + Sz * Sx;
matrix->coef[3] = point->x;
matrix->coef[4] = Sz * Cy;
matrix->coef[5] = Sz * Sy * Sx + Cz * Cx;
matrix->coef[6] = Sz * Sy * Cx - Cz * Sx;
matrix->coef[7] = point->y;
matrix->coef[8] = -Sy;
matrix->coef[9] = Cy * Sx;
matrix->coef[10] = Cy * Cx;
matrix->coef[11] = point->z;
}
void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b)
{
uint8_t i,j,c;
matrix3d_zero(dst);
for(j = 0; j < 4; j++)
{
for(i = 0; i < 4; i++)
{
for(c = 0; c < 4; c++)
{
dst->coef[j*4+i] += a->coef[j*4+c] * b->coef[c*4+i];
}
}
}
}
#include "darwin_dyn_master_v2.h"
#include "dyn_servos.h"
#include "grippers.h"
#include "ram.h"
#include "joint_motion.h"
// private variables
int64_t grippers_period;
int16_t grippers_period_us;
uint8_t gripper_left_target;
uint8_t gripper_right_target;
// public functions
void grippers_init(uint16_t period_us)
{
uint16_t force;
uint32_t servo_mask=0x00000000;
/* initialize private variables */
gripper_left_target=0x00;//close;
gripper_right_target=0x00;//close;
grippers_period=(period_us<<16)/1000000;
grippers_period_us=period_us;
/* configure the maximum force of the servos */
servo_mask=0x00000000;
if(ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]!=0xFF)
{
force=ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_H]<<8);
dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_TOP_ID],XL_TORQUE_LIMIT_L,force);
dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_BOT_ID],XL_TORQUE_LIMIT_L,force);
servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_TOP_ID];
servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_BOT_ID];
joint_motion_set_group_servos(1,servo_mask);
}
servo_mask=0x00000000;
if(ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]!=0xFF)
{
force=ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_H]<<8);
dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID],XL_TORQUE_LIMIT_L,force);
dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID],XL_TORQUE_LIMIT_L,force);
servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID];
servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID];
joint_motion_set_group_servos(2,servo_mask);
}
grippers_close(GRIPPER_LEFT);
grippers_close(GRIPPER_RIGHT);
}
void grippers_set_period(uint16_t period_us)
{
grippers_period=(period_us<<16)/1000000;
grippers_period_us=period_us;
}
uint16_t grippers_get_period(void)
{
return grippers_period_us;
}
void grippers_open(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
{
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
joint_motion_start(1);
}
else if(gripper==GRIPPER_RIGHT)
{
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
joint_motion_start(2);
}
}
void grippers_close(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
{
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
joint_motion_start(1);
}
else if(gripper==GRIPPER_RIGHT)
{
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
joint_motion_start(2);
}
}
void grippers_stop(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
joint_motion_stop(1);
else
joint_motion_stop(2);
}
uint8_t gripper_is_open(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
{
if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
return 0x01;
else
return 0x00;
}
else
{
if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
return 0x01;
else
return 0x00;
}
}
uint8_t gripper_is_close(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
{
if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
return 0x00;
else
return 0x01;
}
else
{
if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
return 0x00;
else
return 0x01;
}
}
uint8_t gripper_is_moving(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
return are_joints_moving(1);
else
return are_joints_moving(2);
}
// operation functions
uint8_t grippers_in_range(unsigned short int address, unsigned short int length)
{
if(ram_in_window(GRIPPER_BASE_ADDRESS,GRIPPER_MEM_LENGTH,address,length) ||
ram_in_window(GRIPPER_EEPROM_ADDRESS,GRIPPER_EEPROM_LENGTH,address,length))
return 0x01;
else
return 0x00;
}
void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
}
void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
if(ram_in_range(DARWIN_GRIPPER_CNTRL,address,length))
{
if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_OPEN_RIGHT)
grippers_open(GRIPPER_RIGHT);
else if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_CLOSE_RIGHT)
grippers_close(GRIPPER_RIGHT);
if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_OPEN_LEFT)
grippers_open(GRIPPER_LEFT);
else if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_CLOSE_LEFT)
grippers_close(GRIPPER_LEFT);
}
}
void grippers_process(void)
{
if(!are_joints_moving(1))
{
ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_LEFT;
if(gripper_left_target==0x01)
ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_LEFT;
else
ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
}
else
{
ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_LEFT;
}
if(!are_joints_moving(2))
{
ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_RIGHT;
if(gripper_right_target==0x01)
ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_RIGHT;
else
ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
}
else
{
ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_RIGHT;
}
}
#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);
}
#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);
}
}
}
}
This diff is collapsed.
This diff is collapsed.
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