From 58aa3e4e745a0f91a20537dd282d993c46352a67 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 30 Jun 2017 12:52:20 +0200
Subject: [PATCH] Set the stop signal to 0 when a new motion is started to
avoid false cancelations. Added a new variable to hold the angle required by
the deacceleration phase with the initial command data, instead of computing
it each cycle. Changed the max force default value of the grippers.
---
include/eeprom_init.h | 4 ++--
src/joint_motion.c | 8 +++++++-
2 files changed, 9 insertions(+), 3 deletions(-)
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index e7ae5eb..cb4543c 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -77,12 +77,12 @@ extern "C" {
#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_LEFT_MAX_FORCE 0x0080 // 1023 max force in binary format
#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
+#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x0080 // 1023 max force in binary format
#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC //1500 ms
diff --git a/src/joint_motion.c b/src/joint_motion.c
index 8e8147a..395bfc0 100644
--- a/src/joint_motion.c
+++ b/src/joint_motion.c
@@ -6,6 +6,7 @@
int64_t joint_target_angles[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_target_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_target_accels[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_target_stop_angles[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
@@ -120,6 +121,8 @@ void joint_motion_start(uint8_t group_id)
ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16;
}
}
+ // stop angles
+ joint_target_stop_angles[i]=joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i]);
// the current angles, speeds and accelerations are in RAM
if(joint_target_angles[i]>=joint_current_angles[i])
joint_dir[i]=1;
@@ -129,6 +132,7 @@ void joint_motion_start(uint8_t group_id)
}
}
joint_moving[group_id]=0x01;
+ joint_stop[group_id]=0x00;
ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS;
}
@@ -236,7 +240,7 @@ void joint_motion_process(void)
}
else
{
- if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i])))// constant speed phase
+ if(abs(joint_target_angles[i]-joint_current_angles[i])>joint_target_stop_angles[i])// constant speed phase
{
joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
@@ -256,6 +260,8 @@ void joint_motion_process(void)
}
}
}
+ else
+ joint_current_speeds[i]=0;
manager_current_angles[i]=joint_current_angles[i];
manager_current_slopes[i]=5;
}
--
GitLab