Added the darwin_motion module.

Added the darwin_balance module.

include/darwin_balance.h

+#ifndef _DARWIN_BALANCE_H
+#define _DARWIN_BALANCE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "darwin_balance_registers.h"
+#include "motion_manager.h"
+#include "scheduler.h"
+#include "memory.h"
+
+typedef struct
+{
+  TMotionManager *mmanager;
+  // gains
+  unsigned short int knee_gain;
+  unsigned short int ankle_roll_gain;
+  unsigned short int ankle_pitch_gain;
+  unsigned short int hip_roll_gain;
+  // memory attributes
+  TMemModule mem_module;
+  TMemory *memory;
+  unsigned int ram_base_address;
+  unsigned int eeprom_base_address;
+}TBalance;
+
+typedef enum {FWD_FALL      = 0,
+              BWD_FALL      = 1,
+              STANDING      = 2} TFall;
+
+//public functions
+uint8_t balance_init(TScheduler *scheduler,TMemory *memory,TMotionManager *mmanager,unsigned int ram_base_address,unsigned short int eeprom_base_address);
+void balance_enable(void);
+void balance_disable(void);
+uint8_t balance_is_enabled(void);
+void balance_set_knee_gain(float gain);
+float balance_get_knee_gain(void);
+void balance_set_ankle_roll_gain(float gain);
+float balance_get_ankle_roll_gain(void);
+void balance_set_ankle_pitch_gain(float gain);
+float balance_get_ankle_pitch_gain(void);
+void balance_set_hip_roll_gain(float gain);
+float balance_get_hip_roll_gain(void);
+uint8_t balance_has_fallen(void);
+TFall balance_get_fallen_position(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

include/darwin_balance_registers.h

+#ifndef _DARWIN_BALANCE_REGISTERS_H
+#define _DARWIN_BALANCE_REGISTERS_H
+
+#define RAM_BALANCE_LENGTH                        1 
+
+#define BALANCE_CONTROL_OFFSET                    0//   bit 7  |   bit 6  |   bit 5  | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+                                                   // standing | bwd fall | fwd fall |       |       |       |       | enable
+  #define BALANCE_ENABLE                          0x01
+  #define BALANCE_STANDING                        0x80
+  #define BALANCE_BWD_FALL                        0x40
+  #define BALANCE_FWD_FALL                        0x20
+
+#define EEPROM_BALANCE_LENGTH                     8
+
+#define BALANCE_KNEE_GAIN_OFFSET                  0
+#define BALANCE_ANKLE_ROLL_GAIN_OFFSET            2
+#define BALANCE_ANKLE_PITCH_GAIN_OFFSET           4
+#define BALANCE_HIP_ROLL_GAIN_OFFSET              6
+
+#define balance_eeprom_data(name,section_name,base_address,DEFAULT_BALANCE_KNEE_GAIN,DEFAULT_BALANCE_ANKLE_ROLL_GAIN,DEFAULT_BALANCE_ANKLE_PITCH_GAIN,DEFAULT_BALANCE_HIP_ROLL_GAIN) \
+unsigned short int name##_eeprom_data[] __attribute__ ((section (section_name))) __attribute__ ((aligned (4)))= {\
+  DEFAULT_BALANCE_KNEE_GAIN&0x00FF,base_address+BALANCE_KNEE_GAIN_OFFSET, \
+  (DEFAULT_BALANCE_KNEE_GAIN>>8)&0x00FF,base_address+BALANCE_KNEE_GAIN_OFFSET+1, \
+  DEFAULT_BALANCE_ANKLE_ROLL_GAIN&0x00FF,base_address+BALANCE_ANKLE_ROLL_GAIN_OFFSET, \
+  (DEFAULT_BALANCE_ANKLE_ROLL_GAIN>>8)&0x00FF,base_address+BALANCE_ANKLE_ROLL_GAIN_OFFSET+1, \
+  DEFAULT_BALANCE_ANKLE_PITCH_GAIN&0x00FF,base_address+BALANCE_ANKLE_PITCH_GAIN_OFFSET, \
+  (DEFAULT_BALANCE_ANKLE_PITCH_GAIN>>8)&0x00FF,base_address+BALANCE_ANKLE_PITCH_GAIN_OFFSET+1, \
+  DEFAULT_BALANCE_HIP_ROLL_GAIN&0x00FF,base_address+BALANCE_HIP_ROLL_GAIN_OFFSET, \
+  (DEFAULT_BALANCE_HIP_ROLL_GAIN>>8)&0x00FF,base_address+BALANCE_HIP_ROLL_GAIN_OFFSET+1}
+
+
+#endif

include/darwin_motion.h

+#ifndef _DARWIN_MOTION_H
+#define _DARWIN_MOTION_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+
+typedef enum {
+  R_SHOULDER_PITCH     = 1,
+  L_SHOULDER_PITCH     = 2,
+  R_SHOULDER_ROLL      = 3,
+  L_SHOULDER_ROLL      = 4,
+  R_ELBOW              = 5,
+  L_ELBOW              = 6,
+  R_HIP_YAW            = 7,
+  L_HIP_YAW            = 8,
+  R_HIP_ROLL           = 9,
+  L_HIP_ROLL           = 10,
+  R_HIP_PITCH          = 11,
+  L_HIP_PITCH          = 12,
+  R_KNEE               = 13,
+  L_KNEE               = 14,
+  R_ANKLE_PITCH        = 15,
+  L_ANKLE_PITCH        = 16,
+  R_ANKLE_ROLL         = 17,
+  L_ANKLE_ROLL         = 18,
+  L_PAN                = 19,
+  L_TILT               = 20,
+  L_GRIPPER_TOP        = 21,
+  L_GRIPPER_BOT        = 22,
+  R_GRIPPER_TOP        = 23,
+  R_GRIPPER_BOT        = 24} servo_id_t;
+
+unsigned char darwin_mm_init(TScheduler *scheduler,TMemory *memory);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+

src/darwin_balance.c

+#include "darwin_balance.h"
+#include "darwin_motion.h"
+#include "darwin_imu.h"
+#include "eeprom.h"
+#include "ram.h"
+#include <stdlib.h>
+
+// private variables
+TBalance darwin_balance;
+
+void balance(short int offsets[DYN_MANAGER_MAX_NUM_DEVICES])
+{
+  int32_t gyro_x,gyro_y,gyro_z;
+
+  // get the values of the gyroscope
+  imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
+  // compensate the servo angle values
+  offsets[R_KNEE]=-((((int64_t)gyro_y*(int64_t)darwin_balance.knee_gain)/6000)>>9);
+  offsets[R_ANKLE_PITCH]=((((int64_t)gyro_y*(int64_t)darwin_balance.ankle_pitch_gain)/6000)>>9);
+  offsets[L_KNEE]=((((int64_t)gyro_y*(int64_t)darwin_balance.knee_gain)/6000)>>9);
+  offsets[L_ANKLE_PITCH]=-((((int64_t)gyro_y*(int64_t)darwin_balance.ankle_pitch_gain)/6000)>>9);
+  offsets[R_HIP_ROLL]=((((int64_t)gyro_x*(int64_t)darwin_balance.hip_roll_gain)/6000)>>9);
+  offsets[L_HIP_ROLL]=((((int64_t)gyro_x*(int64_t)darwin_balance.hip_roll_gain)/6000)>>9);
+  offsets[R_ANKLE_ROLL]=-((((int64_t)gyro_x*(int64_t)darwin_balance.ankle_roll_gain)/6000)>>9);
+  offsets[L_ANKLE_ROLL]=-((((int64_t)gyro_x*(int64_t)darwin_balance.ankle_roll_gain)/6000)>>9);
+}
+
+void balance_fallen(void *data)
+{
+  TBalance *balance=(TBalance *)data;
+
+  int32_t accel_x,accel_y,accel_z;
+
+  imu_get_accel_data(&accel_x,&accel_y,&accel_z);
+  if(abs(accel_y)>abs(accel_z))
+  {
+    if(accel_y>0)
+    {
+      balance->memory->data[balance->ram_base_address+BALANCE_CONTROL_OFFSET]&=~(BALANCE_FWD_FALL);
+      balance->memory->data[balance->ram_base_address+BALANCE_CONTROL_OFFSET]|=BALANCE_BWD_FALL;
+      balance->memory->data[balance->ram_base_address+BALANCE_CONTROL_OFFSET]&=~(BALANCE_STANDING);
+    }
+    else
+    {
+      balance->memory->data[balance->ram_base_address+BALANCE_CONTROL_OFFSET]&=~(BALANCE_BWD_FALL);
+      balance->memory->data[balance->ram_base_address+BALANCE_CONTROL_OFFSET]|=BALANCE_FWD_FALL;
+      balance->memory->data[balance->ram_base_address+BALANCE_CONTROL_OFFSET]&=~(BALANCE_STANDING);
+    }
+  }
+  else
+  {
+    balance->memory->data[balance->ram_base_address+BALANCE_CONTROL_OFFSET]&=~(BALANCE_FWD_FALL);
+    balance->memory->data[balance->ram_base_address+BALANCE_CONTROL_OFFSET]&=~(BALANCE_BWD_FALL);
+    balance->memory->data[balance->ram_base_address+BALANCE_CONTROL_OFFSET]|=(BALANCE_STANDING);
+  }
+}
+
+/* eeprom data */
+balance_eeprom_data(darwin_balance,".eeprom",EEPROM_BALANCE_BASE_ADDRESS,DEFAULT_BALANCE_KNEE_GAIN,DEFAULT_BALANCE_ANKLE_ROLL_GAIN,DEFAULT_BALANCE_ANKLE_PITCH_GAIN,DEFAULT_BALANCE_HIP_ROLL_GAIN);
+
+void balance_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  TBalance *balance=(TBalance *)module;
+  unsigned short int ram_offset,eeprom_offset;
+  uint8_t *data_ptr;
+
+  ram_offset=address-balance->ram_base_address;
+  if(ram_in_range(balance->ram_base_address+BALANCE_CONTROL_OFFSET,address,length))
+  {
+    if(data[BALANCE_CONTROL_OFFSET-ram_offset]&BALANCE_ENABLE)
+      balance_enable();
+    else 
+      balance_disable();
+  }
+  eeprom_offset=address-balance->eeprom_base_address;
+  if(ram_in_window(balance->eeprom_base_address+BALANCE_KNEE_GAIN_OFFSET,2,address,length))
+  {
+    data_ptr=(unsigned char *)&balance->knee_gain;
+    if(ram_in_range(balance->eeprom_base_address+BALANCE_KNEE_GAIN_OFFSET,address,length))
+    {
+      data_ptr[0]=data[BALANCE_KNEE_GAIN_OFFSET-eeprom_offset];
+      balance->memory->data[balance->eeprom_base_address+BALANCE_KNEE_GAIN_OFFSET]=data_ptr[0];
+    }
+    if(ram_in_range(balance->eeprom_base_address+BALANCE_KNEE_GAIN_OFFSET+1,address,length))
+    {
+      data_ptr[1]=data[BALANCE_KNEE_GAIN_OFFSET+1-eeprom_offset];
+      balance->memory->data[balance->eeprom_base_address+BALANCE_KNEE_GAIN_OFFSET+1]=data_ptr[1];
+    }
+  }
+  if(ram_in_window(balance->eeprom_base_address+BALANCE_ANKLE_ROLL_GAIN_OFFSET,2,address,length))
+  { 
+    data_ptr=(unsigned char *)&balance->ankle_roll_gain;
+    if(ram_in_range(balance->eeprom_base_address+BALANCE_ANKLE_ROLL_GAIN_OFFSET,address,length))
+    {
+      data_ptr[0]=data[BALANCE_ANKLE_ROLL_GAIN_OFFSET-eeprom_offset];
+      balance->memory->data[balance->eeprom_base_address+BALANCE_ANKLE_ROLL_GAIN_OFFSET]=data_ptr[0];
+    }
+    if(ram_in_range(balance->eeprom_base_address+BALANCE_ANKLE_ROLL_GAIN_OFFSET+1,address,length))
+    {
+      data_ptr[1]=data[BALANCE_ANKLE_ROLL_GAIN_OFFSET+1-eeprom_offset];
+      balance->memory->data[balance->eeprom_base_address+BALANCE_ANKLE_ROLL_GAIN_OFFSET+1]=data_ptr[1];
+    }
+  }
+  if(ram_in_window(balance->eeprom_base_address+BALANCE_ANKLE_PITCH_GAIN_OFFSET,2,address,length))
+  { 
+    data_ptr=(unsigned char *)&balance->ankle_pitch_gain;
+    if(ram_in_range(balance->eeprom_base_address+BALANCE_ANKLE_PITCH_GAIN_OFFSET,address,length))
+    {
+      data_ptr[0]=data[BALANCE_ANKLE_PITCH_GAIN_OFFSET-eeprom_offset];
+      balance->memory->data[balance->eeprom_base_address+BALANCE_ANKLE_PITCH_GAIN_OFFSET]=data_ptr[0];
+    }
+    if(ram_in_range(balance->eeprom_base_address+BALANCE_ANKLE_PITCH_GAIN_OFFSET+1,address,length))
+    {
+      data_ptr[1]=data[BALANCE_ANKLE_PITCH_GAIN_OFFSET+1-eeprom_offset];
+      balance->memory->data[balance->eeprom_base_address+BALANCE_ANKLE_PITCH_GAIN_OFFSET+1]=data_ptr[1];
+    }
+  }
+  if(ram_in_window(balance->eeprom_base_address+BALANCE_HIP_ROLL_GAIN_OFFSET,2,address,length))
+  { 
+    data_ptr=(unsigned char *)&balance->hip_roll_gain;
+    if(ram_in_range(balance->eeprom_base_address+BALANCE_HIP_ROLL_GAIN_OFFSET,address,length))
+    {
+      data_ptr[0]=data[BALANCE_HIP_ROLL_GAIN_OFFSET-eeprom_offset];
+      balance->memory->data[balance->eeprom_base_address+BALANCE_HIP_ROLL_GAIN_OFFSET]=data_ptr[0];
+    }
+    if(ram_in_range(balance->eeprom_base_address+BALANCE_HIP_ROLL_GAIN_OFFSET+1,address,length))
+    {
+      data_ptr[1]=data[BALANCE_HIP_ROLL_GAIN_OFFSET+1-eeprom_offset];
+      balance->memory->data[balance->eeprom_base_address+BALANCE_HIP_ROLL_GAIN_OFFSET+1]=data_ptr[1];
+    }
+  }
+}
+
+void balance_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+  TBalance *balance=(TBalance *)module;
+
+  ram_read_table(balance->memory,address,length,data);
+}
+
+// public functions
+uint8_t balance_init(TScheduler *scheduler,TMemory *memory,TMotionManager *mmanager,unsigned int ram_base_address,unsigned short int eeprom_base_address)
+{
+  /* initialize attributes */
+  darwin_balance.mmanager=mmanager; 
+  mmanager->balance=balance;
+  /* initialize memory module */
+  mem_module_init(&darwin_balance.mem_module);
+  darwin_balance.mem_module.write_cmd=balance_write_cmd;
+  darwin_balance.mem_module.read_cmd=balance_read_cmd;
+  if(!mem_module_add_ram_segment(&darwin_balance.mem_module,ram_base_address,RAM_BALANCE_LENGTH))
+    return 0x00;
+  darwin_balance.ram_base_address=ram_base_address;
+  if(!mem_module_add_eeprom_segment(&darwin_balance.mem_module,eeprom_base_address,EEPROM_BALANCE_LENGTH))
+    return 0x00;
+  darwin_balance.eeprom_base_address=eeprom_base_address;
+  if(!mem_add_module(memory,&darwin_balance.mem_module))
+    return 0x00;
+  darwin_balance.memory=memory;
+  darwin_balance.mem_module.data=&darwin_balance;
+  EE_update_num_variables(EEPROM_BALANCE_LENGTH);
+
+  scheduler_set_channel(scheduler,SCHED_CH4,(void(*)(void *))balance_fallen,100,&darwin_balance); 
+  scheduler_enable_channel(scheduler,SCHED_CH4);
+
+  return 0x01;
+}
+
+void balance_enable(void)
+{
+  if(darwin_balance.mmanager!=0x000000)
+    mmanager_enable_balance(darwin_balance.mmanager);
+}
+
+void balance_disable(void)
+{
+  if(darwin_balance.mmanager!=0x000000)
+    mmanager_disable_balance(darwin_balance.mmanager);
+}
+
+uint8_t balance_is_enabled(void)
+{
+  return mmanager_is_balance_enabled(darwin_balance.mmanager);
+}
+
+float balance_get_knee_gain(void)
+{
+  return darwin_balance.knee_gain;
+}
+
+float balance_get_ankle_roll_gain(void)
+{
+  return darwin_balance.ankle_roll_gain;
+}
+
+float balance_get_ankle_pitch_gain(void)
+{
+  return darwin_balance.ankle_pitch_gain;
+}
+
+float balance_get_hip_roll_gain(void)
+{
+  return darwin_balance.hip_roll_gain;
+}
+
+uint8_t balance_has_fallen(void)
+{
+  if(darwin_balance.memory->data[darwin_balance.ram_base_address+BALANCE_CONTROL_OFFSET]&(BALANCE_FWD_FALL | BALANCE_BWD_FALL))
+    return 0x01;
+  else
+    return 0x00;
+}
+
+TFall balance_get_fallen_position(void)
+{
+  if(darwin_balance.memory->data[darwin_balance.ram_base_address+BALANCE_CONTROL_OFFSET]&BALANCE_FWD_FALL)
+    return FWD_FALL;
+  else if(darwin_balance.memory->data[darwin_balance.ram_base_address+BALANCE_CONTROL_OFFSET]&BALANCE_BWD_FALL)
+    return BWD_FALL;
+  else
+    return STANDING;
+}
+

src/darwin_motion.c

+#include "darwin_motion.h"
+#include "darwin_balance.h"
+#include "darwin_dyn_master.h"
+#include "motion_manager.h"
+#include "action.h"
+#include "eeprom.h"
+
+TDynManager darwin_dyn_manager;
+TDynamixelMaster *darwin_master;
+TMotionManager darwin_mmanager;
+TActionMModule action_mm;
+
+dyn_manager_eeprom_data(darwin_dyn_manager,".eeprom",EEPROM_DYN_MANAGER_BASE_ADDRESS,DYN_MANAGER_PERIOD);
+dyn_mm_eeprom_data(darwin_mmanager,".eeprom",EEPROM_MMANAGER_BASE_ADDRESS,MMANAGER_PERIOD,0,EEPROM_MMANAGER_BASE_ADDRESS+1,0,EEPROM_MMANAGER_BASE_ADDRESS+2,0,EEPROM_MMANAGER_BASE_ADDRESS+3,0,EEPROM_MMANAGER_BASE_ADDRESS+4,0,EEPROM_MMANAGER_BASE_ADDRESS+5,0,EEPROM_MMANAGER_BASE_ADDRESS+6,0,EEPROM_MMANAGER_BASE_ADDRESS+7,0,EEPROM_MMANAGER_BASE_ADDRESS+8,0,EEPROM_MMANAGER_BASE_ADDRESS+9,0,EEPROM_MMANAGER_BASE_ADDRESS+10,0,EEPROM_MMANAGER_BASE_ADDRESS+11,0,EEPROM_MMANAGER_BASE_ADDRESS+12,0,EEPROM_MMANAGER_BASE_ADDRESS+13,0,EEPROM_MMANAGER_BASE_ADDRESS+14,0,EEPROM_MMANAGER_BASE_ADDRESS+15,0,EEPROM_MMANAGER_BASE_ADDRESS+16,0,EEPROM_MMANAGER_BASE_ADDRESS+17,0,EEPROM_MMANAGER_BASE_ADDRESS+18,0,EEPROM_MMANAGER_BASE_ADDRESS+19,0,EEPROM_MMANAGER_BASE_ADDRESS+20,0,EEPROM_MMANAGER_BASE_ADDRESS+21,0,EEPROM_MMANAGER_BASE_ADDRESS+22,0,EEPROM_MMANAGER_BASE_ADDRESS+23,0,EEPROM_MMANAGER_BASE_ADDRESS+24,0,EEPROM_MMANAGER_BASE_ADDRESS+25,0,EEPROM_MMANAGER_BASE_ADDRESS+26,0,EEPROM_MMANAGER_BASE_ADDRESS+27,0,EEPROM_MMANAGER_BASE_ADDRESS+28,0,EEPROM_MMANAGER_BASE_ADDRESS+29,0,EEPROM_MMANAGER_BASE_ADDRESS+30,0,EEPROM_MMANAGER_BASE_ADDRESS+31,0,EEPROM_MMANAGER_BASE_ADDRESS+32);
+
+unsigned char darwin_mm_init(TScheduler *scheduler,TMemory *memory)
+{
+  dyn_manager_init(&darwin_dyn_manager,memory,scheduler,SCHED_CH3,SCHED_CH2,RAM_DYN_MANAGER_BASE_ADDRESS,EEPROM_DYN_MANAGER_BASE_ADDRESS);
+  darwin_master=darwin_dyn_master_init();
+  dyn_manager_add_master(&darwin_dyn_manager,darwin_master);
+
+  mmanager_init(&darwin_mmanager,memory,RAM_MMANAGER_BASE_ADDRESS,EEPROM_MMANAGER_BASE_ADDRESS);
+  action_init(&action_mm,memory,RAM_ACTION_MM_BASE_ADDRESS);
+  mmanager_add_module(&darwin_mmanager,action_get_module(&action_mm));
+  dyn_manager_add_module(&darwin_dyn_manager,mmanager_get_dyn_module(&darwin_mmanager));
+  EE_update_num_variables(EEPROM_DYN_MANAGER_LENGTH+EEPROM_DYN_MODULE_LENGTH+EEPROM_MM_LENGTH);
+
+  // initialize balance module
+  balance_init(scheduler,memory,&darwin_mmanager,RAM_BALANCE_BASE_ADDRESS,EEPROM_BALANCE_BASE_ADDRESS);
+
+  return 0x01;
+}