diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index c91a8321d5c29ef6ede370b25527631cfc9128af..8b5c1913202a02e71889b2a537a98d92ded6df50 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -75,15 +75,17 @@ 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)0x0059)
-#define GRIPPER_RIGHT_ID ((unsigned short int)0x005B)
-#define GRIPPER_RIGHT_MAX_ANGLE ((unsigned short int)0x005C)
-#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x005E)
-#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0060)
-#define SMART_CHARGER_PERIOD ((unsigned short int)0x0062)
+#define GRIPPER_LEFT_TOP_ID ((unsigned short int)0x0054)
+#define GRIPPER_LEFT_BOT_ID ((unsigned short int)0x0055)
+#define GRIPPER_LEFT_MAX_ANGLE ((unsigned short int)0x0056)
+#define GRIPPER_LEFT_MIN_ANGLE ((unsigned short int)0x0058)
+#define GRIPPER_LEFT_MAX_FORCE ((unsigned short int)0x005A)
+#define GRIPPER_RIGHT_TOP_ID ((unsigned short int)0x005C)
+#define GRIPPER_RIGHT_BOT_ID ((unsigned short int)0x005D)
+#define GRIPPER_RIGHT_MAX_ANGLE ((unsigned short int)0x005E)
+#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x0060)
+#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0062)
+#define SMART_CHARGER_PERIOD ((unsigned short int)0x0064)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
@@ -172,14 +174,16 @@ 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_TOP_ID = GRIPPER_LEFT_TOP_ID,
+ DARWIN_GRIPPER_LEFT_BOT_ID = GRIPPER_LEFT_BOT_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_TOP_ID = GRIPPER_RIGHT_TOP_ID,
+ DARWIN_GRIPPER_RIGHT_BOT_ID = GRIPPER_RIGHT_BOT_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,
@@ -580,8 +584,8 @@ typedef enum {
DARWIN_SMART_CHARGER_BATT_STATUS_H = 0x024F,
DARWIN_SMART_CHARGER_CNTRL = 0x0250, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | detected | enable
- DARWIN_GRIPPER_CNTRL = 0x0251 // 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_GRIPPER_CNTRL = 0x0251 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // left opened | right opened | left moving | right moving | close left | open left | close right | open right
}darwin_registers;
#define GPIO_BASE_ADDRESS 0x0100
@@ -668,11 +672,15 @@ typedef enum {
#define GRIPPER_BASE_ADDRESS 0x0251
#define GRIPPER_MEM_LENGTH 1
#define GRIPPER_EEPROM_ADDRESS 0x0054
-#define GRIPPER_EEPROM_LENGTH 14
+#define GRIPPER_EEPROM_LENGTH 16
#define GRIPPER_OPEN_RIGHT 0x01
#define GRIPPER_CLOSE_RIGHT 0x02
#define GRIPPER_OPEN_LEFT 0x04
#define GRIPPER_CLOSE_LEFT 0x08
+#define GRIPPER_MOVING_LEFT 0x20
+#define GRIPPER_MOVING_RIGHT 0x10
+#define GRIPPER_OPENED_LEFT 0x80
+#define GRIPPER_OPENED_RIGHT 0x40
#define SMART_CHARGER_BASE_ADDRESS 0x0247
#define SMART_CHARGER_MEM_LENGTH 11
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index dafe22333ba7cca14136c3498c54dc9a1905e9b5..e7ae5eb6f83eb5c9810fc8ea54067e0a04101119 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -73,11 +73,13 @@ 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_TOP_ID 0x0015 // ID 21 for the left gripper
+#define DEFAULT_GRIPPER_LEFT_BOT_ID 0x0016 // ID 22 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_TOP_ID 0x0017 // ID 23 for the left gripper
+#define DEFAULT_GRIPPER_RIGHT_BOT_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
diff --git a/include/grippers.h b/include/grippers.h
index 9df31bcba3b0a176d7c83c76f7a89e40b98b23b2..9a1aa5fd90e5c64d1cc75a9bac5fe51f73109634 100644
--- a/include/grippers.h
+++ b/include/grippers.h
@@ -16,13 +16,15 @@ 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);
+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);
-// motion manager interface functions
void grippers_process(void);
#ifdef __cplusplus
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 641c39d8debe1b66a42cb213e7f7faff44104e63..d0f234ce1e61bd2eefffab36a056d07729157523 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -29,7 +29,11 @@ typedef enum {
R_ANKLE_ROLL = 17,
L_ANKLE_ROLL = 18,
L_PAN = 19,
- L_TILT = 20} servo_id_t;
+ L_TILT = 20,
+ L_GRIPPER_TOP = 21,
+ L_GRIPPER_BOT = 22,
+ R_GRIPPER_TOP = 23,
+ R_GRIPPER_BOT = 24} servo_id_t;
typedef enum {MM_NONE = 0,
@@ -38,9 +42,9 @@ typedef enum {MM_NONE = 0,
MM_JOINTS = 3,
MM_HEAD = 4} TModules;
-typedef enum {MM_FWD_FALL =0,
- MM_BWD_FALL =1,
- MM_STANDING =2} TFall;
+typedef enum {MM_FWD_FALL = 0,
+ MM_BWD_FALL = 1,
+ MM_STANDING = 2} TFall;
typedef struct
{
diff --git a/src/eeprom.c b/src/eeprom.c
index 4fa6f8e2edc57ca67b5fee4864dcde49817fbf91..fbb5a1a8caf6dd6e796739c795ad05376e2c41d6 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -139,14 +139,16 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
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_TOP_ID, GRIPPER_LEFT_TOP_ID,
+ DEFAULT_GRIPPER_LEFT_BOT_ID, GRIPPER_LEFT_BOT_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_TOP_ID, GRIPPER_RIGHT_TOP_ID,
+ DEFAULT_GRIPPER_RIGHT_BOT_ID, GRIPPER_RIGHT_BOT_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,
diff --git a/src/grippers.c b/src/grippers.c
index c5fae087946fe969081ef44b37459cbbd41ee25c..5544346b9a44459ee306563a05523dfe20bb66d9 100644
--- a/src/grippers.c
+++ b/src/grippers.c
@@ -2,42 +2,48 @@
#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 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;
+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 */
- grippers_left_opened=0x00;
- grippers_left_open=0x00;
- grippers_left_close=0x00;
- grippers_right_opened=0x00;
- grippers_right_open=0x00;
- grippers_right_close=0x00;
+ 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 */
- if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF)
+ 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_ID],XL_TORQUE_LIMIT_L,force);
+ 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);
}
- if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF)
+ 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_ID],XL_TORQUE_LIMIT_L,force);
+ 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);
}
inline void grippers_set_period(uint16_t period_us)
@@ -54,27 +60,101 @@ inline uint16_t grippers_get_period(void)
void grippers_open(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
- grippers_left_open=0x01;
+ {
+ 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)
- grippers_right_open=0x01;
+ {
+ 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)
- grippers_left_close=0x01;
+ {
+ 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)
- grippers_right_close=0x01;
+ {
+ 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)
- return grippers_left_opened;
- else if(gripper==GRIPPER_RIGHT)
- return grippers_right_opened;
+ {
+ if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
+ return 0x01;
+ else
+ return 0x00;
+ }
else
- return 0x00;
+ {
+ 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
@@ -107,45 +187,32 @@ void grippers_process_write_cmd(unsigned short int address,unsigned short int le
}
}
-// motion manager interface functions
void grippers_process(void)
{
- uint16_t angle;
-
- if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF)
+ 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
{
- 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;
- }
- }
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
+ ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_LEFT;
}
- if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF)
+ 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
{
- 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;
- }
- }
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
+ ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_RIGHT;
}
}
diff --git a/src/motion_manager.c b/src/motion_manager.c
index b6bb218c0f2ca6f328eddaa2d5a5401b8bcd01d5..65f06bf2d20f20592bc365d76ab4193dcb766fa5 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -196,7 +196,7 @@ void MANAGER_TIMER_IRQHandler(void)
head_tracking_process();
// call the walking process
walking_process();
- // call the grippers process
+ // call the gripper process
grippers_process();
// balance the robot
manager_balance();
diff --git a/src/ram.c b/src/ram.c
index e061c05b1e42cf5b5247a9c9b61043bc74410a6d..e708a462c3ed4c18b529f11d3f129342c2886ce2 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -119,8 +119,10 @@ 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_TOP_ID,&eeprom_data)==0)
+ ram_data[GRIPPER_LEFT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(GRIPPER_LEFT_BOT_ID,&eeprom_data)==0)
+ ram_data[GRIPPER_LEFT_BOT_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)
@@ -133,8 +135,10 @@ void ram_init(void)
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_TOP_ID,&eeprom_data)==0)
+ ram_data[GRIPPER_RIGHT_TOP_ID]=(uint8_t)(eeprom_data&0x00FF);
+ if(EE_ReadVariable(GRIPPER_RIGHT_BOT_ID,&eeprom_data)==0)
+ ram_data[GRIPPER_RIGHT_BOT_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)