add init tool pose
@@ -125,6 +125,8 @@ void initManipulator()
manipulator.initJointTrajectory(CHAIN);
manipulator.setControlTime(ACTUATOR_CONTROL_TIME);
+ manipulator.toolMove(CHAIN, TOOL, false);
+
#ifdef PLATFORM////////////////////////////////////Actuator init
manipulator.setAllActiveJointAngle(CHAIN, manipulator.receiveAllActuatorAngle(CHAIN));
#endif/////////////////////////////////////////////
@@ -163,7 +165,7 @@ void THREAD::Actuator_Control(void const *argument)
{
(void)argument;
- static uint16_t last_time = 0;
+ // static uint16_t last_time = 0;
for (;;)
{
