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

Added a simple gripper module.

parent ef1d1bc6
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Makefile
+ 1
− 0
View file @ f74fe90b
...@@ -26,6 +26,7 @@ TARGET_FILES+=src/darwin_math.c ...@@ -26,6 +26,7 @@ TARGET_FILES+=src/darwin_math.c
TARGET_FILES+=src/darwin_kinematics.c TARGET_FILES+=src/darwin_kinematics.c
TARGET_FILES+=src/joint_motion.c TARGET_FILES+=src/joint_motion.c
TARGET_FILES+=src/head_tracking.c TARGET_FILES+=src/head_tracking.c
TARGET_FILES+=src/grippers.c
TARGET_PROCESSOR=STM32F103RE TARGET_PROCESSOR=STM32F103RE
......
include/darwin_registers.h
+ 34
− 1
View file @ f74fe90b
...@@ -73,6 +73,14 @@ extern "C" { ...@@ -73,6 +73,14 @@ extern "C" {
#define HEAD_TILT_I ((unsigned short int)0x004E) #define HEAD_TILT_I ((unsigned short int)0x004E)
#define HEAD_TILT_D ((unsigned short int)0x0050) #define HEAD_TILT_D ((unsigned short int)0x0050)
#define HEAD_TILT_I_CLAMP ((unsigned short int)0x0052) #define HEAD_TILT_I_CLAMP ((unsigned short int)0x0052)
#define GRIPPER_LEFT_ID ((unsigned short int)0x0054)
#define GRIPPER_LEFT_MAX_ANGLE ((unsigned short int)0x0055)
#define GRIPPER_LEFT_MIN_ANGLE ((unsigned short int)0x0057)
#define GRIPPER_LEFT_MAX_FORCE ((unsigned short int)0x0058)
#define GRIPPER_RIGHT_ID ((unsigned short int)0x0059)
#define GRIPPER_RIGHT_MAX_ANGLE ((unsigned short int)0x005A)
#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x005C)
#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x005E)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF) #define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
...@@ -161,6 +169,20 @@ typedef enum { ...@@ -161,6 +169,20 @@ typedef enum {
DARWIN_HEAD_TILT_D_H = HEAD_TILT_D+1, DARWIN_HEAD_TILT_D_H = HEAD_TILT_D+1,
DARWIN_HEAD_TILT_I_CLAMP_L = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7 DARWIN_HEAD_TILT_I_CLAMP_L = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7
DARWIN_HEAD_TILT_I_CLAMP_H = HEAD_TILT_I_CLAMP+1, DARWIN_HEAD_TILT_I_CLAMP_H = HEAD_TILT_I_CLAMP+1,
DARWIN_GRIPPER_LEFT_ID = GRIPPER_LEFT_ID,
DARWIN_GRIPPER_LEFT_MAX_ANGLE_L = GRIPPER_LEFT_MAX_ANGLE,
DARWIN_GRIPPER_LEFT_MAX_ANGLE_H = GRIPPER_LEFT_MAX_ANGLE+1,
DARWIN_GRIPPER_LEFT_MIN_ANGLE_L = GRIPPER_LEFT_MIN_ANGLE,
DARWIN_GRIPPER_LEFT_MIN_ANGLE_H = GRIPPER_LEFT_MIN_ANGLE+1,
DARWIN_GRIPPER_LEFT_MAX_FORCE_L = GRIPPER_LEFT_MAX_FORCE,
DARWIN_GRIPPER_LEFT_MAX_FORCE_H = GRIPPER_LEFT_MAX_FORCE+1,
DARWIN_GRIPPER_RIGHT_ID = GRIPPER_RIGHT_ID,
DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L = GRIPPER_RIGHT_MAX_ANGLE,
DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H = GRIPPER_RIGHT_MAX_ANGLE+1,
DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L = GRIPPER_RIGHT_MIN_ANGLE,
DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H = GRIPPER_RIGHT_MIN_ANGLE+1,
DARWIN_GRIPPER_RIGHT_MAX_FORCE_L = GRIPPER_RIGHT_MAX_FORCE,
DARWIN_GRIPPER_RIGHT_MAX_FORCE_H = GRIPPER_RIGHT_MAX_FORCE+1,
DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | blink | toggle | value | internally used // | | | | blink | toggle | value | internally used
DARWIN_TX_LED_PERIOD_L = 0x0101, DARWIN_TX_LED_PERIOD_L = 0x0101,
...@@ -582,7 +604,9 @@ typedef enum { ...@@ -582,7 +604,9 @@ typedef enum {
DARWIN_BATT_CELL3_VOLTAGE_L = 0x026E, DARWIN_BATT_CELL3_VOLTAGE_L = 0x026E,
DARWIN_BATT_CELL3_VOLTAGE_H = 0x026F, DARWIN_BATT_CELL3_VOLTAGE_H = 0x026F,
DARWIN_BATT_CELL4_VOLTAGE_L = 0x0270, DARWIN_BATT_CELL4_VOLTAGE_L = 0x0270,
DARWIN_BATT_CELL4_VOLTAGE_H = 0x0271 DARWIN_BATT_CELL4_VOLTAGE_H = 0x0271,
DARWIN_GRIPPER_CNTRL = 0x0272, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// left opened | right opened | | | close left | open left | close right | open right
} darwin_registers; } darwin_registers;
#define GPIO_BASE_ADDRESS 0x0100 #define GPIO_BASE_ADDRESS 0x0100
...@@ -664,6 +688,15 @@ typedef enum { ...@@ -664,6 +688,15 @@ typedef enum {
#define HEAD_STOP 0x02 #define HEAD_STOP 0x02
#define HEAD_STATUS 0x10 #define HEAD_STATUS 0x10
#define GRIPPER_BASE_ADDRESS 0x0272
#define GRIPPER_MEM_LENGTH 1
#define GRIPPER_EEPROM_ADDRESS 0x0054
#define GRIPPER_EEPROM_LENGTH 14
#define GRIPPER_OPEN_RIGHT 0x01
#define GRIPPER_CLOSE_RIGHT 0x02
#define GRIPPER_OPEN_LEFT 0x04
#define GRIPPER_CLOSE_LEFT 0x08
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif
......
include/eeprom_init.h
+ 8
− 1
View file @ f74fe90b
...@@ -73,7 +73,14 @@ extern "C" { ...@@ -73,7 +73,14 @@ extern "C" {
#define DEFAULT_HEAD_TILT_I 0x0000 // 0.0 in fixed point format 0|16 #define DEFAULT_HEAD_TILT_I 0x0000 // 0.0 in fixed point format 0|16
#define DEFAULT_HEAD_TILT_D 0x0000 // 0.0 in fixed point format 0|16 #define DEFAULT_HEAD_TILT_D 0x0000 // 0.0 in fixed point format 0|16
#define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7 #define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
#define DEFAULT_GRIPPER_LEFT_ID 0x0017 // ID 23 for the left gripper
#define DEFAULT_GRIPPER_LEFT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
#define DEFAULT_GRIPPER_LEFT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
#define DEFAULT_GRIPPER_LEFT_MAX_FORCE 0x03FF // 1023 max force in binary format
#define DEFAULT_GRIPPER_RIGHT_ID 0x0018 // ID 24 for the left gripper
#define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
#define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x03FF // 1023 max force in binary format
#ifdef __cplusplus #ifdef __cplusplus
} }
......
include/grippers.h 0 → 100644
+ 32
− 0
View file @ f74fe90b
#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);
inline void grippers_set_period(uint16_t period_us);
inline uint16_t grippers_get_period(void);
void grippers_open(grippers_t gripper);
void grippers_close(grippers_t gripper);
uint8_t gripper_is_open(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);
// motion manager interface functions
void grippers_process(void);
#ifdef __cplusplus
}
#endif
#endif
include/motion_manager.h
+ 2
− 1
View file @ f74fe90b
...@@ -6,6 +6,7 @@ extern "C" { ...@@ -6,6 +6,7 @@ extern "C" {
#endif #endif
#include "stm32f1xx.h" #include "stm32f1xx.h"
#include "dynamixel_common.h"
typedef enum { typedef enum {
R_SHOULDER_PITCH = 1, R_SHOULDER_PITCH = 1,
...@@ -62,7 +63,7 @@ typedef struct{ ...@@ -62,7 +63,7 @@ typedef struct{
uint8_t enabled; uint8_t enabled;
int16_t cw_angle_limit; int16_t cw_angle_limit;
int16_t ccw_angle_limit; int16_t ccw_angle_limit;
uint8_t dyn_version; TDynVersion dyn_version;
}TServoInfo; }TServoInfo;
#define MANAGER_MAX_NUM_SERVOS 31 #define MANAGER_MAX_NUM_SERVOS 31
......
src/darwin_dyn_slave.c
+ 4
− 0
View file @ f74fe90b
...@@ -11,6 +11,7 @@ ...@@ -11,6 +11,7 @@
#include "walking.h" #include "walking.h"
#include "joint_motion.h" #include "joint_motion.h"
#include "head_tracking.h" #include "head_tracking.h"
#include "grippers.h"
/* timer for the execution of the dynamixel slave loop */ /* timer for the execution of the dynamixel slave loop */
#define DYN_SLAVE_TIMER TIM7 #define DYN_SLAVE_TIMER TIM7
...@@ -88,6 +89,9 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng ...@@ -88,6 +89,9 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng
// head_tracking commands // head_tracking commands
if(head_tracking_in_range(address,length)) if(head_tracking_in_range(address,length))
head_tracking_process_write_cmd(address,length,data); head_tracking_process_write_cmd(address,length,data);
// gripper commands
if(grippers_in_range(address,length))
grippers_process_write_cmd(address,length,data);
// write eeprom // write eeprom
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++) for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]); EE_WriteVariable(i,data[j]);
......
src/eeprom.c
+ 100
− 86
View file @ f74fe90b
...@@ -53,92 +53,106 @@ ...@@ -53,92 +53,106 @@
uint16_t DataVar = 0; uint16_t DataVar = 0;
/* Virtual address defined by the user: 0xFFFF value is prohibited */ /* Virtual address defined by the user: 0xFFFF value is prohibited */
uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF, uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF,
DEFAULT_DEVICE_MODEL&0xFF, DEVICE_MODEL_OFFSET,// model number LSB DEFAULT_DEVICE_MODEL&0xFF, DEVICE_MODEL_OFFSET,// model number LSB
DEFAULT_DEVICE_MODEL>>8, DEVICE_MODEL_OFFSET+1,// model number MSB DEFAULT_DEVICE_MODEL>>8, DEVICE_MODEL_OFFSET+1,// model number MSB
DEFAULT_FIRMWARE_VERSION, FIRMWARE_VERSION_OFFSET,// firmware version DEFAULT_FIRMWARE_VERSION, FIRMWARE_VERSION_OFFSET,// firmware version
DEFAULT_DEVICE_ID, DEVICE_ID_OFFSET,// default device id DEFAULT_DEVICE_ID, DEVICE_ID_OFFSET,// default device id
DEFAULT_BAUDRATE, BAUDRATE_OFFSET,// default baudrate DEFAULT_BAUDRATE, BAUDRATE_OFFSET,// default baudrate
DEFAULT_RETURN_DELAY, RETURN_DELAY_OFFSET,// return delay time DEFAULT_RETURN_DELAY, RETURN_DELAY_OFFSET,// return delay time
DEFAULT_MM_PERIOD&0xFF, MM_PERIOD_OFFSET, DEFAULT_MM_PERIOD&0xFF, MM_PERIOD_OFFSET,
DEFAULT_MM_PERIOD>>8, MM_PERIOD_OFFSET+1, DEFAULT_MM_PERIOD>>8, MM_PERIOD_OFFSET+1,
DEFAULT_BAL_KNEE_GAIN&0xFF, MM_BAL_KNEE_GAIN_OFFSET, DEFAULT_BAL_KNEE_GAIN&0xFF, MM_BAL_KNEE_GAIN_OFFSET,
DEFAULT_BAL_KNEE_GAIN>>8, MM_BAL_KNEE_GAIN_OFFSET+1, DEFAULT_BAL_KNEE_GAIN>>8, MM_BAL_KNEE_GAIN_OFFSET+1,
DEFAULT_BAL_ANKLE_ROLL_GAIN&0xFF, MM_BAL_ANKLE_ROLL_GAIN_OFFSET, DEFAULT_BAL_ANKLE_ROLL_GAIN&0xFF, MM_BAL_ANKLE_ROLL_GAIN_OFFSET,
DEFAULT_BAL_ANKLE_ROLL_GAIN>>8, MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1, DEFAULT_BAL_ANKLE_ROLL_GAIN>>8, MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
DEFAULT_BAL_ANKLE_PITCH_GAIN&0xFF, MM_BAL_ANKLE_PITCH_GAIN_OFFSET, DEFAULT_BAL_ANKLE_PITCH_GAIN&0xFF, MM_BAL_ANKLE_PITCH_GAIN_OFFSET,
DEFAULT_BAL_ANKLE_PITCH_GAIN>>8, MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1, DEFAULT_BAL_ANKLE_PITCH_GAIN>>8, MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
DEFAULT_BAL_HIP_ROLL_GAIN&0xFF, MM_BAL_HIP_ROLL_GAIN_OFFSET, DEFAULT_BAL_HIP_ROLL_GAIN&0xFF, MM_BAL_HIP_ROLL_GAIN_OFFSET,
DEFAULT_BAL_HIP_ROLL_GAIN>>8, MM_BAL_HIP_ROLL_GAIN_OFFSET+1, DEFAULT_BAL_HIP_ROLL_GAIN>>8, MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
DEFAULT_RETURN_LEVEL, RETURN_LEVEL_OFFSET,// return level DEFAULT_RETURN_LEVEL, RETURN_LEVEL_OFFSET,// return level
DEFAULT_SERVO0_OFFSET, MM_SERVO0_OFFSET, DEFAULT_SERVO0_OFFSET, MM_SERVO0_OFFSET,
DEFAULT_SERVO1_OFFSET, MM_SERVO1_OFFSET, DEFAULT_SERVO1_OFFSET, MM_SERVO1_OFFSET,
DEFAULT_SERVO2_OFFSET, MM_SERVO2_OFFSET, DEFAULT_SERVO2_OFFSET, MM_SERVO2_OFFSET,
DEFAULT_SERVO3_OFFSET, MM_SERVO3_OFFSET, DEFAULT_SERVO3_OFFSET, MM_SERVO3_OFFSET,
DEFAULT_SERVO4_OFFSET, MM_SERVO4_OFFSET, DEFAULT_SERVO4_OFFSET, MM_SERVO4_OFFSET,
DEFAULT_SERVO5_OFFSET, MM_SERVO5_OFFSET, DEFAULT_SERVO5_OFFSET, MM_SERVO5_OFFSET,
DEFAULT_SERVO6_OFFSET, MM_SERVO6_OFFSET, DEFAULT_SERVO6_OFFSET, MM_SERVO6_OFFSET,
DEFAULT_SERVO7_OFFSET, MM_SERVO7_OFFSET, DEFAULT_SERVO7_OFFSET, MM_SERVO7_OFFSET,
DEFAULT_SERVO8_OFFSET, MM_SERVO8_OFFSET, DEFAULT_SERVO8_OFFSET, MM_SERVO8_OFFSET,
DEFAULT_SERVO9_OFFSET, MM_SERVO9_OFFSET, DEFAULT_SERVO9_OFFSET, MM_SERVO9_OFFSET,
DEFAULT_SERVO10_OFFSET, MM_SERVO10_OFFSET, DEFAULT_SERVO10_OFFSET, MM_SERVO10_OFFSET,
DEFAULT_SERVO11_OFFSET, MM_SERVO11_OFFSET, DEFAULT_SERVO11_OFFSET, MM_SERVO11_OFFSET,
DEFAULT_SERVO12_OFFSET, MM_SERVO12_OFFSET, DEFAULT_SERVO12_OFFSET, MM_SERVO12_OFFSET,
DEFAULT_SERVO13_OFFSET, MM_SERVO13_OFFSET, DEFAULT_SERVO13_OFFSET, MM_SERVO13_OFFSET,
DEFAULT_SERVO14_OFFSET, MM_SERVO14_OFFSET, DEFAULT_SERVO14_OFFSET, MM_SERVO14_OFFSET,
DEFAULT_SERVO15_OFFSET, MM_SERVO15_OFFSET, DEFAULT_SERVO15_OFFSET, MM_SERVO15_OFFSET,
DEFAULT_SERVO16_OFFSET, MM_SERVO16_OFFSET, DEFAULT_SERVO16_OFFSET, MM_SERVO16_OFFSET,
DEFAULT_SERVO17_OFFSET, MM_SERVO17_OFFSET, DEFAULT_SERVO17_OFFSET, MM_SERVO17_OFFSET,
DEFAULT_SERVO18_OFFSET, MM_SERVO18_OFFSET, DEFAULT_SERVO18_OFFSET, MM_SERVO18_OFFSET,
DEFAULT_SERVO19_OFFSET, MM_SERVO19_OFFSET, DEFAULT_SERVO19_OFFSET, MM_SERVO19_OFFSET,
DEFAULT_SERVO20_OFFSET, MM_SERVO20_OFFSET, DEFAULT_SERVO20_OFFSET, MM_SERVO20_OFFSET,
DEFAULT_SERVO21_OFFSET, MM_SERVO21_OFFSET, DEFAULT_SERVO21_OFFSET, MM_SERVO21_OFFSET,
DEFAULT_SERVO22_OFFSET, MM_SERVO22_OFFSET, DEFAULT_SERVO22_OFFSET, MM_SERVO22_OFFSET,
DEFAULT_SERVO23_OFFSET, MM_SERVO23_OFFSET, DEFAULT_SERVO23_OFFSET, MM_SERVO23_OFFSET,
DEFAULT_SERVO24_OFFSET, MM_SERVO24_OFFSET, DEFAULT_SERVO24_OFFSET, MM_SERVO24_OFFSET,
DEFAULT_SERVO25_OFFSET, MM_SERVO25_OFFSET, DEFAULT_SERVO25_OFFSET, MM_SERVO25_OFFSET,
DEFAULT_SERVO26_OFFSET, MM_SERVO26_OFFSET, DEFAULT_SERVO26_OFFSET, MM_SERVO26_OFFSET,
DEFAULT_SERVO27_OFFSET, MM_SERVO27_OFFSET, DEFAULT_SERVO27_OFFSET, MM_SERVO27_OFFSET,
DEFAULT_SERVO28_OFFSET, MM_SERVO28_OFFSET, DEFAULT_SERVO28_OFFSET, MM_SERVO28_OFFSET,
DEFAULT_SERVO29_OFFSET, MM_SERVO29_OFFSET, DEFAULT_SERVO29_OFFSET, MM_SERVO29_OFFSET,
DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET, DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET,
DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET, DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET,
DEFAULT_WALK_X_OFFSET, WALK_X_OFFSET, DEFAULT_WALK_X_OFFSET, WALK_X_OFFSET,
DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET, DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET,
DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET, DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET,
DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET, DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET,
DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET, DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET,
DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET, DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET,
DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF, DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF,
DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1, DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1,
DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME, DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME,
DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1, DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1,
DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO, DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO,
DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO, DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO,
DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT, DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT,
DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT, DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT,
DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN, DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN,
DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET, DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET,
DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN, DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN,
DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL, DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL,
DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL, DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL,
DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P, DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P,
DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1, DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1,
DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I, DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I,
DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1, DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1,
DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D, DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D,
DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1, DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP, DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP,
DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1, DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1,
DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1, DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P, DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P,
DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1, DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1,
DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I, DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I,
DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1, DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1,
DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D, DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D,
DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1, DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1,
DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP, DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP,
DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1}; DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1,
DEFAULT_GRIPPER_LEFT_ID, GRIPPER_LEFT_ID,
DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF, GRIPPER_LEFT_MAX_ANGLE,
DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8, GRIPPER_LEFT_MAX_ANGLE+1,
DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF, GRIPPER_LEFT_MIN_ANGLE,
DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8, GRIPPER_LEFT_MIN_ANGLE+1,
DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF, GRIPPER_LEFT_MAX_FORCE,
DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8, GRIPPER_LEFT_MAX_FORCE+1,
DEFAULT_GRIPPER_RIGHT_ID, GRIPPER_RIGHT_ID,
DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF, GRIPPER_RIGHT_MAX_ANGLE,
DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8, GRIPPER_RIGHT_MAX_ANGLE+1,
DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF, GRIPPER_RIGHT_MIN_ANGLE,
DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8, GRIPPER_RIGHT_MIN_ANGLE+1,
DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF, GRIPPER_RIGHT_MAX_FORCE,
DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8, GRIPPER_RIGHT_MAX_FORCE+1};
/* Private function prototypes -----------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/ /* Private functions ---------------------------------------------------------*/
......
src/grippers.c 0 → 100644
+ 151
− 0
View file @ f74fe90b
#include "darwin_dyn_master_v2.h"
#include "dyn_servos.h"
#include "grippers.h"
#include "ram.h"
// private variables
int64_t grippers_period;
int16_t grippers_period_us;
uint8_t grippers_left_opened;
uint8_t grippers_left_open;
uint8_t grippers_left_close;
uint8_t grippers_right_opened;
uint8_t grippers_right_open;
uint8_t grippers_right_close;
// public functions
void grippers_init(uint16_t period_us)
{
uint16_t force;
/* initialize private variables */
grippers_left_opened=0x00;
grippers_left_open=0x00;
grippers_left_close=0x00;
grippers_right_opened=0x00;
grippers_right_open=0x00;
grippers_right_close=0x00;
grippers_period=(period_us<<16)/1000000;
grippers_period_us=period_us;
/* configure the maximum force of the servos */
if(ram_data[DARWIN_GRIPPER_LEFT_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_ID],XL_TORQUE_LIMIT_L,force);
}
if(ram_data[DARWIN_GRIPPER_RIGHT_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_ID],XL_TORQUE_LIMIT_L,force);
}
}
inline void grippers_set_period(uint16_t period_us)
{
grippers_period=(period_us<<16)/1000000;
grippers_period_us=period_us;
}
inline uint16_t grippers_get_period(void)
{
return grippers_period_us;
}
void grippers_open(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
grippers_left_open=0x01;
else if(gripper==GRIPPER_RIGHT)
grippers_right_open=0x01;
}
void grippers_close(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
grippers_left_close=0x01;
else if(gripper==GRIPPER_RIGHT)
grippers_right_close=0x01;
}
uint8_t gripper_is_open(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
return grippers_left_opened;
else if(gripper==GRIPPER_RIGHT)
return grippers_right_opened;
else
return 0x00;
}
// 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);
}
}
// motion manager interface functions
void grippers_process(void)
{
uint16_t angle;
if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF)
{
if(grippers_left_opened==0x01)
{
if(grippers_left_close==0x01)
{
grippers_left_close=0x00;
angle=ram_data[DARWIN_GRIPPER_LEFT_MIN_ANGLE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MIN_ANGLE_H]<<8);
manager_current_angles[ram_data[DARWIN_GRIPPER_LEFT_ID]]=angle<<9;
}
else if(grippers_left_open==0x01)
{
grippers_left_open=0x00;
angle=ram_data[DARWIN_GRIPPER_LEFT_MAX_ANGLE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_ANGLE_H]<<8);
manager_current_angles[ram_data[DARWIN_GRIPPER_LEFT_ID]]=angle<<9;
}
}
}
if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF)
{
if(grippers_right_opened==0x01)
{
if(grippers_right_close==0x01)
{
grippers_right_close=0x00;
angle=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]<<8);
manager_current_angles[ram_data[DARWIN_GRIPPER_RIGHT_ID]]=angle<<9;
}
else if(grippers_right_open==0x01)
{
grippers_right_open=0x00;
angle=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]<<8);
manager_current_angles[ram_data[DARWIN_GRIPPER_RIGHT_ID]]=angle<<9;
}
}
}
}
src/motion_manager.c
+ 80
− 21
View file @ f74fe90b
...@@ -7,6 +7,7 @@ ...@@ -7,6 +7,7 @@
#include "walking.h" #include "walking.h"
#include "joint_motion.h" #include "joint_motion.h"
#include "head_tracking.h" #include "head_tracking.h"
#include "grippers.h"
#include "imu.h" #include "imu.h"
#define MANAGER_TIMER TIM5 #define MANAGER_TIMER TIM5
...@@ -39,20 +40,30 @@ void manager_send_motion_command(void) ...@@ -39,20 +40,30 @@ void manager_send_motion_command(void)
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{ {
if(manager_servos[i].enabled) if(manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==DYN_VER1)
{ {
if(manager_servos[i].model!=0x0000) servo_ids[num]=manager_servos[i].id;
{ manager_servos[i].cw_comp=(1<<(manager_current_slopes[i]&0x0F));
servo_ids[num]=manager_servos[i].id; manager_servos[i].ccw_comp=(1<<((manager_current_slopes[i]&0xF0)>>4));
manager_servos[i].cw_comp=(1<<(manager_current_slopes[i]&0x0F)); write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
manager_servos[i].ccw_comp=(1<<((manager_current_slopes[i]&0xF0)>>4)); num++;
write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
num++;
}
} }
} }
if(num>0) if(num>0)
dyn_master_sync_write(&darwin_dyn_master,num,servo_ids,P_CW_COMPLIANCE_SLOPE,4,write_data); dyn_master_sync_write(&darwin_dyn_master,num,servo_ids,P_CW_COMPLIANCE_SLOPE,4,write_data);
num=0;
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
if(manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==DYN_VER2)
{
servo_ids[num]=manager_servos[i].id;
manager_servos[i].ccw_comp=(1<<(manager_current_slopes[i]&0x0F));
write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
num++;
}
}
if(num>0)
dyn_master_sync_write(&darwin_dyn_master_v2,num,servo_ids,P_CW_COMPLIANCE_SLOPE,3,write_data);
} }
inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle) inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
...@@ -85,7 +96,10 @@ void manager_get_current_position(void) ...@@ -85,7 +96,10 @@ void manager_get_current_position(void)
{ {
if(!manager_servos[i].enabled && manager_servos[i].model!=0x0000)// servo is not enabled but it is present if(!manager_servos[i].enabled && manager_servos[i].model!=0x0000)// servo is not enabled but it is present
{ {
dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value); if(manager_servos[i].dyn_version==DYN_VER1)
dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
else
dyn_master_read_word(&darwin_dyn_master_v2,manager_servos[i].id,XL_PRESENT_POSITION_L,&manager_servos[i].current_value);
manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value); manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF); ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8); ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
...@@ -156,6 +170,8 @@ void MANAGER_TIMER_IRQHandler(void) ...@@ -156,6 +170,8 @@ void MANAGER_TIMER_IRQHandler(void)
head_tracking_process(); head_tracking_process();
// call the walking process // call the walking process
walking_process(); walking_process();
// call the grippers process
grippers_process();
// balance the robot // balance the robot
manager_balance(); manager_balance();
// get the target angles from all modules // get the target angles from all modules
...@@ -175,7 +191,7 @@ void manager_init(uint16_t period_us) ...@@ -175,7 +191,7 @@ void manager_init(uint16_t period_us)
TIM_MasterConfigTypeDef sMasterConfig; TIM_MasterConfigTypeDef sMasterConfig;
uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS]; uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
uint16_t model,value; uint16_t model,value;
uint8_t i,num=0; uint8_t i,num=0,current=0;
uint32_t present_servos=0x00000000; uint32_t present_servos=0x00000000;
/* initialize the dynamixel master module for the servos */ /* initialize the dynamixel master module for the servos */
...@@ -190,13 +206,13 @@ void manager_init(uint16_t period_us) ...@@ -190,13 +206,13 @@ void manager_init(uint16_t period_us)
dyn_master_scan(&darwin_dyn_master,&num,servo_ids); dyn_master_scan(&darwin_dyn_master,&num,servo_ids);
ram_data[DARWIN_MM_NUM_SERVOS]=num; ram_data[DARWIN_MM_NUM_SERVOS]=num;
manager_num_servos=0; manager_num_servos=0;
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) for(i=0;i<MANAGER_MAX_NUM_SERVOS && current<num;i++)
{ {
if(i==servo_ids[manager_num_servos]) if(i==servo_ids[current])
{ {
present_servos|=(0x00000001<<i); present_servos|=(0x00000001<<i);
// read the model of the i-th device // read the model of the i-th device
dyn_master_read_word(&darwin_dyn_master,servo_ids[manager_num_servos],P_MODEL_NUMBER_L,&model); dyn_master_read_word(&darwin_dyn_master,servo_ids[current],P_MODEL_NUMBER_L,&model);
switch(model) switch(model)
{ {
case SERVO_AX12A: manager_servos[i].encoder_resolution=1023; case SERVO_AX12A: manager_servos[i].encoder_resolution=1023;
...@@ -261,10 +277,11 @@ void manager_init(uint16_t period_us) ...@@ -261,10 +277,11 @@ void manager_init(uint16_t period_us)
break; break;
default: break; default: break;
} }
manager_servos[i].id=servo_ids[manager_num_servos]; manager_servos[i].id=servo_ids[current];
manager_servos[i].model=model; manager_servos[i].model=model;
manager_servos[i].module=MM_NONE; manager_servos[i].module=MM_NONE;
manager_servos[i].enabled=0x00; manager_servos[i].enabled=0x00;
manager_servos[i].dyn_version=DYN_VER1;
// get the servo's current position // get the servo's current position
dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value); dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value); manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
...@@ -278,6 +295,7 @@ void manager_init(uint16_t period_us) ...@@ -278,6 +295,7 @@ void manager_init(uint16_t period_us)
// set the action current angles // set the action current angles
manager_current_angles[i]=manager_servos[i].current_angle<<9; manager_current_angles[i]=manager_servos[i].current_angle<<9;
manager_num_servos++; manager_num_servos++;
current=0;
} }
else else
{ {
...@@ -296,15 +314,55 @@ void manager_init(uint16_t period_us) ...@@ -296,15 +314,55 @@ void manager_init(uint16_t period_us)
manager_servos[i].ccw_angle_limit=0; manager_servos[i].ccw_angle_limit=0;
} }
} }
ram_data[DARWIN_MM_PRESENT_SERVOS1]=(present_servos&0x000000FF);
ram_data[DARWIN_MM_PRESENT_SERVOS2]=((present_servos&0x0000FF00)>>8);
ram_data[DARWIN_MM_PRESENT_SERVOS3]=((present_servos&0x00FF0000)>>16);
ram_data[DARWIN_MM_PRESENT_SERVOS4]=((present_servos&0xFF000000)>>24);
darwin_dyn_master_disable_power(); darwin_dyn_master_disable_power();
// detect the servos connected on the v2 bus // detect the servos connected on the v2 bus
// dyn_master_scan(&darwin_dyn_master_v2,&num,servo_ids); dyn_master_scan(&darwin_dyn_master_v2,&num,servo_ids);
ram_data[DARWIN_MM_NUM_SERVOS]+=num;
current=0;
for(i=0;i<MANAGER_MAX_NUM_SERVOS && current<num;i++)
{
if(i==servo_ids[current])
{
present_servos|=(0x00000001<<i);
// read the model of the i-th device
dyn_master_read_word(&darwin_dyn_master_v2,servo_ids[current],XL_MODEL_NUMBER_L,&model);
switch(model)
{
case SERVO_XL320: manager_servos[i].encoder_resolution=1023;
manager_servos[i].gear_ratio=238;
manager_servos[i].max_angle=300<<7;
manager_servos[i].center_angle=150<<7;
manager_servos[i].max_speed=354;
break;
default: break;
}
manager_servos[i].id=servo_ids[current];
manager_servos[i].model=model;
manager_servos[i].module=MM_NONE;
manager_servos[i].enabled=0x00;
manager_servos[i].dyn_version=DYN_VER2;
// get the servo's current position
dyn_master_read_word(&darwin_dyn_master_v2,manager_servos[i].id,XL_PRESENT_POSITION_L,&manager_servos[i].current_value);
manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
// read the servo limits
dyn_master_read_word(&darwin_dyn_master_v2,manager_servos[i].id,XL_CW_ANGLE_LIMIT_L,&value);
manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
dyn_master_read_word(&darwin_dyn_master_v2,manager_servos[i].id,XL_CCW_ANGLE_LIMIT_L,&value);
manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
// set the action current angles
manager_current_angles[i]=manager_servos[i].current_angle<<9;
manager_num_servos++;
current++;
}
}
ram_data[DARWIN_MM_PRESENT_SERVOS1]=(present_servos&0x000000FF);
ram_data[DARWIN_MM_PRESENT_SERVOS2]=((present_servos&0x0000FF00)>>8);
ram_data[DARWIN_MM_PRESENT_SERVOS3]=((present_servos&0x00FF0000)>>16);
ram_data[DARWIN_MM_PRESENT_SERVOS4]=((present_servos&0xFF000000)>>24);
ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_SCANNING); ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_SCANNING);
/* configure timer */ /* configure timer */
...@@ -343,6 +401,7 @@ void manager_init(uint16_t period_us) ...@@ -343,6 +401,7 @@ void manager_init(uint16_t period_us)
walking_init(period_us); walking_init(period_us);
joint_motion_init(period_us); joint_motion_init(period_us);
head_tracking_init(period_us); head_tracking_init(period_us);
grippers_init(period_us);
} }
uint16_t manager_get_period(void) uint16_t manager_get_period(void)
......
src/ram.c
+ 28
− 0
View file @ f74fe90b
...@@ -119,6 +119,34 @@ void ram_init(void) ...@@ -119,6 +119,34 @@ void ram_init(void)
ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF); ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0) if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0)
ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF); ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_LEFT_ID,&eeprom_data)==0)
ram_data[GRIPPER_LEFT_ID]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE,&eeprom_data)==0)
ram_data[GRIPPER_LEFT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE+1,&eeprom_data)==0)
ram_data[GRIPPER_LEFT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE,&eeprom_data)==0)
ram_data[GRIPPER_LEFT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE+1,&eeprom_data)==0)
ram_data[GRIPPER_LEFT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE,&eeprom_data)==0)
ram_data[GRIPPER_LEFT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE+1,&eeprom_data)==0)
ram_data[GRIPPER_LEFT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_RIGHT_ID,&eeprom_data)==0)
ram_data[GRIPPER_RIGHT_ID]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE,&eeprom_data)==0)
ram_data[GRIPPER_RIGHT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE+1,&eeprom_data)==0)
ram_data[GRIPPER_RIGHT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE,&eeprom_data)==0)
ram_data[GRIPPER_RIGHT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE+1,&eeprom_data)==0)
ram_data[GRIPPER_RIGHT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE,&eeprom_data)==0)
ram_data[GRIPPER_RIGHT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE+1,&eeprom_data)==0)
ram_data[GRIPPER_RIGHT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
} }
inline void ram_read_byte(uint16_t address,uint8_t *data) inline void ram_read_byte(uint16_t address,uint8_t *data)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment