diff --git a/Makefile b/Makefile
index 418bd7f51c320402c65399732d9439f87577caa0..67bc17e7240d4097e90dd11ae2f2ecc1b4878537 100755
--- a/Makefile
+++ b/Makefile
@@ -26,6 +26,7 @@ TARGET_FILES+=src/darwin_math.c
TARGET_FILES+=src/darwin_kinematics.c
TARGET_FILES+=src/joint_motion.c
TARGET_FILES+=src/head_tracking.c
+TARGET_FILES+=src/grippers.c
TARGET_PROCESSOR=STM32F103RE
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 3eee1019fb1f4007b0938269717d35d9c17ee4c4..3899d162c9ec3376c3cd66d2f73679239fc1b11a 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -73,6 +73,14 @@ extern "C" {
#define HEAD_TILT_I ((unsigned short int)0x004E)
#define HEAD_TILT_D ((unsigned short int)0x0050)
#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)
@@ -161,6 +169,20 @@ typedef enum {
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_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
// | | | | blink | toggle | value | internally used
DARWIN_TX_LED_PERIOD_L = 0x0101,
@@ -582,7 +604,9 @@ typedef enum {
DARWIN_BATT_CELL3_VOLTAGE_L = 0x026E,
DARWIN_BATT_CELL3_VOLTAGE_H = 0x026F,
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;
#define GPIO_BASE_ADDRESS 0x0100
@@ -664,6 +688,15 @@ typedef enum {
#define HEAD_STOP 0x02
#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
}
#endif
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index 331f2879a484098f25ae963405f06ce66a32efea..b636703311e13ee36d73b97ef44bbfa484f75d23 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -73,7 +73,14 @@ extern "C" {
#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_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
}
diff --git a/include/grippers.h b/include/grippers.h
new file mode 100644
index 0000000000000000000000000000000000000000..9df31bcba3b0a176d7c83c76f7a89e40b98b23b2
--- /dev/null
+++ b/include/grippers.h
@@ -0,0 +1,32 @@
+#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
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 7c63ee26e14a3499fd12e157ed033e0befb798bb..a764496424536f49ea63ff0ddaa96d13295c591b 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -6,6 +6,7 @@ extern "C" {
#endif
#include "stm32f1xx.h"
+#include "dynamixel_common.h"
typedef enum {
R_SHOULDER_PITCH = 1,
@@ -62,7 +63,7 @@ typedef struct{
uint8_t enabled;
int16_t cw_angle_limit;
int16_t ccw_angle_limit;
- uint8_t dyn_version;
+ TDynVersion dyn_version;
}TServoInfo;
#define MANAGER_MAX_NUM_SERVOS 31
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index 1f3924e11143221ab91b07aa5bdba5f40ae362a6..0750b0937bde2814e02ea3ab473d4a110e481fca 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -11,6 +11,7 @@
#include "walking.h"
#include "joint_motion.h"
#include "head_tracking.h"
+#include "grippers.h"
/* timer for the execution of the dynamixel slave loop */
#define DYN_SLAVE_TIMER TIM7
@@ -88,6 +89,9 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng
// head_tracking commands
if(head_tracking_in_range(address,length))
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
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]);
diff --git a/src/eeprom.c b/src/eeprom.c
index 701a3ad081d42f9a51233fbb249ead6ff147ced6..c5caef423ff350cc15ad1eeb015987de27471d87 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -53,92 +53,106 @@
uint16_t DataVar = 0;
/* Virtual address defined by the user: 0xFFFF value is prohibited */
-uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF,
- DEFAULT_DEVICE_MODEL&0xFF, DEVICE_MODEL_OFFSET,// model number LSB
- DEFAULT_DEVICE_MODEL>>8, DEVICE_MODEL_OFFSET+1,// model number MSB
- DEFAULT_FIRMWARE_VERSION, FIRMWARE_VERSION_OFFSET,// firmware version
- DEFAULT_DEVICE_ID, DEVICE_ID_OFFSET,// default device id
- DEFAULT_BAUDRATE, BAUDRATE_OFFSET,// default baudrate
- DEFAULT_RETURN_DELAY, RETURN_DELAY_OFFSET,// return delay time
- DEFAULT_MM_PERIOD&0xFF, MM_PERIOD_OFFSET,
- DEFAULT_MM_PERIOD>>8, MM_PERIOD_OFFSET+1,
- DEFAULT_BAL_KNEE_GAIN&0xFF, MM_BAL_KNEE_GAIN_OFFSET,
- 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>>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>>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>>8, MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
- DEFAULT_RETURN_LEVEL, RETURN_LEVEL_OFFSET,// return level
- DEFAULT_SERVO0_OFFSET, MM_SERVO0_OFFSET,
- DEFAULT_SERVO1_OFFSET, MM_SERVO1_OFFSET,
- DEFAULT_SERVO2_OFFSET, MM_SERVO2_OFFSET,
- DEFAULT_SERVO3_OFFSET, MM_SERVO3_OFFSET,
- DEFAULT_SERVO4_OFFSET, MM_SERVO4_OFFSET,
- DEFAULT_SERVO5_OFFSET, MM_SERVO5_OFFSET,
- DEFAULT_SERVO6_OFFSET, MM_SERVO6_OFFSET,
- DEFAULT_SERVO7_OFFSET, MM_SERVO7_OFFSET,
- DEFAULT_SERVO8_OFFSET, MM_SERVO8_OFFSET,
- DEFAULT_SERVO9_OFFSET, MM_SERVO9_OFFSET,
- DEFAULT_SERVO10_OFFSET, MM_SERVO10_OFFSET,
- DEFAULT_SERVO11_OFFSET, MM_SERVO11_OFFSET,
- DEFAULT_SERVO12_OFFSET, MM_SERVO12_OFFSET,
- DEFAULT_SERVO13_OFFSET, MM_SERVO13_OFFSET,
- DEFAULT_SERVO14_OFFSET, MM_SERVO14_OFFSET,
- DEFAULT_SERVO15_OFFSET, MM_SERVO15_OFFSET,
- DEFAULT_SERVO16_OFFSET, MM_SERVO16_OFFSET,
- DEFAULT_SERVO17_OFFSET, MM_SERVO17_OFFSET,
- DEFAULT_SERVO18_OFFSET, MM_SERVO18_OFFSET,
- DEFAULT_SERVO19_OFFSET, MM_SERVO19_OFFSET,
- DEFAULT_SERVO20_OFFSET, MM_SERVO20_OFFSET,
- DEFAULT_SERVO21_OFFSET, MM_SERVO21_OFFSET,
- DEFAULT_SERVO22_OFFSET, MM_SERVO22_OFFSET,
- DEFAULT_SERVO23_OFFSET, MM_SERVO23_OFFSET,
- DEFAULT_SERVO24_OFFSET, MM_SERVO24_OFFSET,
- DEFAULT_SERVO25_OFFSET, MM_SERVO25_OFFSET,
- DEFAULT_SERVO26_OFFSET, MM_SERVO26_OFFSET,
- DEFAULT_SERVO27_OFFSET, MM_SERVO27_OFFSET,
- DEFAULT_SERVO28_OFFSET, MM_SERVO28_OFFSET,
- DEFAULT_SERVO29_OFFSET, MM_SERVO29_OFFSET,
- DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET,
- DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET,
- DEFAULT_WALK_X_OFFSET, WALK_X_OFFSET,
- DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET,
- DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET,
- DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET,
- DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET,
- DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET,
- DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF,
- DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1,
- DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME,
- DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1,
- DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO,
- DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO,
- DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT,
- DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT,
- DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN,
- DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET,
- DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN,
- DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL,
- DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL,
- DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P,
- DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1,
- DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I,
- DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1,
- DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D,
- DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
- DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP,
- DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1,
- DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
- DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P,
- DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1,
- DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I,
- DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1,
- DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D,
- DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1,
- DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP,
- DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1};
+uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF,
+ DEFAULT_DEVICE_MODEL&0xFF, DEVICE_MODEL_OFFSET,// model number LSB
+ DEFAULT_DEVICE_MODEL>>8, DEVICE_MODEL_OFFSET+1,// model number MSB
+ DEFAULT_FIRMWARE_VERSION, FIRMWARE_VERSION_OFFSET,// firmware version
+ DEFAULT_DEVICE_ID, DEVICE_ID_OFFSET,// default device id
+ DEFAULT_BAUDRATE, BAUDRATE_OFFSET,// default baudrate
+ DEFAULT_RETURN_DELAY, RETURN_DELAY_OFFSET,// return delay time
+ DEFAULT_MM_PERIOD&0xFF, MM_PERIOD_OFFSET,
+ DEFAULT_MM_PERIOD>>8, MM_PERIOD_OFFSET+1,
+ DEFAULT_BAL_KNEE_GAIN&0xFF, MM_BAL_KNEE_GAIN_OFFSET,
+ 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>>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>>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>>8, MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
+ DEFAULT_RETURN_LEVEL, RETURN_LEVEL_OFFSET,// return level
+ DEFAULT_SERVO0_OFFSET, MM_SERVO0_OFFSET,
+ DEFAULT_SERVO1_OFFSET, MM_SERVO1_OFFSET,
+ DEFAULT_SERVO2_OFFSET, MM_SERVO2_OFFSET,
+ DEFAULT_SERVO3_OFFSET, MM_SERVO3_OFFSET,
+ DEFAULT_SERVO4_OFFSET, MM_SERVO4_OFFSET,
+ DEFAULT_SERVO5_OFFSET, MM_SERVO5_OFFSET,
+ DEFAULT_SERVO6_OFFSET, MM_SERVO6_OFFSET,
+ DEFAULT_SERVO7_OFFSET, MM_SERVO7_OFFSET,
+ DEFAULT_SERVO8_OFFSET, MM_SERVO8_OFFSET,
+ DEFAULT_SERVO9_OFFSET, MM_SERVO9_OFFSET,
+ DEFAULT_SERVO10_OFFSET, MM_SERVO10_OFFSET,
+ DEFAULT_SERVO11_OFFSET, MM_SERVO11_OFFSET,
+ DEFAULT_SERVO12_OFFSET, MM_SERVO12_OFFSET,
+ DEFAULT_SERVO13_OFFSET, MM_SERVO13_OFFSET,
+ DEFAULT_SERVO14_OFFSET, MM_SERVO14_OFFSET,
+ DEFAULT_SERVO15_OFFSET, MM_SERVO15_OFFSET,
+ DEFAULT_SERVO16_OFFSET, MM_SERVO16_OFFSET,
+ DEFAULT_SERVO17_OFFSET, MM_SERVO17_OFFSET,
+ DEFAULT_SERVO18_OFFSET, MM_SERVO18_OFFSET,
+ DEFAULT_SERVO19_OFFSET, MM_SERVO19_OFFSET,
+ DEFAULT_SERVO20_OFFSET, MM_SERVO20_OFFSET,
+ DEFAULT_SERVO21_OFFSET, MM_SERVO21_OFFSET,
+ DEFAULT_SERVO22_OFFSET, MM_SERVO22_OFFSET,
+ DEFAULT_SERVO23_OFFSET, MM_SERVO23_OFFSET,
+ DEFAULT_SERVO24_OFFSET, MM_SERVO24_OFFSET,
+ DEFAULT_SERVO25_OFFSET, MM_SERVO25_OFFSET,
+ DEFAULT_SERVO26_OFFSET, MM_SERVO26_OFFSET,
+ DEFAULT_SERVO27_OFFSET, MM_SERVO27_OFFSET,
+ DEFAULT_SERVO28_OFFSET, MM_SERVO28_OFFSET,
+ DEFAULT_SERVO29_OFFSET, MM_SERVO29_OFFSET,
+ DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET,
+ DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET,
+ DEFAULT_WALK_X_OFFSET, WALK_X_OFFSET,
+ DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET,
+ DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET,
+ DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET,
+ DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET,
+ DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET,
+ DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF,
+ DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1,
+ DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME,
+ DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1,
+ DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO,
+ DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO,
+ DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT,
+ DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT,
+ DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN,
+ DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET,
+ DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN,
+ DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL,
+ DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL,
+ DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P,
+ DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1,
+ DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I,
+ DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1,
+ DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D,
+ DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
+ DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP,
+ DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1,
+ DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
+ DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P,
+ DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1,
+ DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I,
+ DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1,
+ DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D,
+ DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1,
+ DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP,
+ 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 functions ---------------------------------------------------------*/
diff --git a/src/grippers.c b/src/grippers.c
new file mode 100644
index 0000000000000000000000000000000000000000..c5fae087946fe969081ef44b37459cbbd41ee25c
--- /dev/null
+++ b/src/grippers.c
@@ -0,0 +1,151 @@
+#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;
+ }
+ }
+ }
+}
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 2e2fa0f9152bd016ec8134712c8fb7ba31e4b2ee..8077fcd6ccc3ee42c02abbbf7ba9e1b4780af608 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -7,6 +7,7 @@
#include "walking.h"
#include "joint_motion.h"
#include "head_tracking.h"
+#include "grippers.h"
#include "imu.h"
#define MANAGER_TIMER TIM5
@@ -39,20 +40,30 @@ void manager_send_motion_command(void)
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
- if(manager_servos[i].enabled)
- {
- if(manager_servos[i].model!=0x0000)
- {
- servo_ids[num]=manager_servos[i].id;
- manager_servos[i].cw_comp=(1<<(manager_current_slopes[i]&0x0F));
- manager_servos[i].ccw_comp=(1<<((manager_current_slopes[i]&0xF0)>>4));
- write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
- num++;
- }
+ if(manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==DYN_VER1)
+ {
+ servo_ids[num]=manager_servos[i].id;
+ manager_servos[i].cw_comp=(1<<(manager_current_slopes[i]&0x0F));
+ manager_servos[i].ccw_comp=(1<<((manager_current_slopes[i]&0xF0)>>4));
+ write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
+ num++;
}
}
if(num>0)
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)
@@ -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
{
- 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);
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);
@@ -156,6 +170,8 @@ void MANAGER_TIMER_IRQHandler(void)
head_tracking_process();
// call the walking process
walking_process();
+ // call the grippers process
+ grippers_process();
// balance the robot
manager_balance();
// get the target angles from all modules
@@ -175,7 +191,7 @@ void manager_init(uint16_t period_us)
TIM_MasterConfigTypeDef sMasterConfig;
uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
uint16_t model,value;
- uint8_t i,num=0;
+ uint8_t i,num=0,current=0;
uint32_t present_servos=0x00000000;
/* initialize the dynamixel master module for the servos */
@@ -190,13 +206,13 @@ void manager_init(uint16_t period_us)
dyn_master_scan(&darwin_dyn_master,&num,servo_ids);
ram_data[DARWIN_MM_NUM_SERVOS]=num;
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);
// 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)
{
case SERVO_AX12A: manager_servos[i].encoder_resolution=1023;
@@ -261,10 +277,11 @@ void manager_init(uint16_t period_us)
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].module=MM_NONE;
manager_servos[i].enabled=0x00;
+ manager_servos[i].dyn_version=DYN_VER1;
// 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);
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)
// set the action current angles
manager_current_angles[i]=manager_servos[i].current_angle<<9;
manager_num_servos++;
+ current=0;
}
else
{
@@ -296,15 +314,55 @@ void manager_init(uint16_t period_us)
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();
// 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);
/* configure timer */
@@ -343,6 +401,7 @@ void manager_init(uint16_t period_us)
walking_init(period_us);
joint_motion_init(period_us);
head_tracking_init(period_us);
+ grippers_init(period_us);
}
uint16_t manager_get_period(void)
diff --git a/src/ram.c b/src/ram.c
index cf821c30d488f1062058d3420b40dadda31b8efc..3d77ee0e898d5c198d2a21c0bcf014127af8f21c 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -119,6 +119,34 @@ void ram_init(void)
ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0)
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)