diff --git a/wpb_home_bringup/CMakeLists.txt b/wpb_home_bringup/CMakeLists.txt
index a9a7671..f35a10c 100644
--- a/wpb_home_bringup/CMakeLists.txt
+++ b/wpb_home_bringup/CMakeLists.txt
@@ -232,11 +232,21 @@ target_link_libraries(wpb_home_lidar_filter
)
## Declare a C++ executable
-
add_executable(wpb_home_imu_tf
src/wpb_home_imu_tf.cpp
)
add_dependencies(wpb_home_imu_tf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(wpb_home_imu_tf
${catkin_LIBRARIES}
-)
\ No newline at end of file
+)
+
+## Declare a C++ executable
+add_executable(wpb_home_test_mani
+ src/wpb_home_test_mani.cpp
+ src/driver/SerialCom.cpp
+ src/driver/WPB_Home_driver.cpp
+)
+add_dependencies(wpb_home_test_mani ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(wpb_home_test_mani
+ ${catkin_LIBRARIES}
+)
diff --git a/wpb_home_bringup/include/driver/WPB_Home_driver.h b/wpb_home_bringup/include/driver/WPB_Home_driver.h
index 33da314..90c781b 100644
--- a/wpb_home_bringup/include/driver/WPB_Home_driver.h
+++ b/wpb_home_bringup/include/driver/WPB_Home_driver.h
@@ -47,6 +47,7 @@ public:
void Velocity(float inX, float inY, float inAngular);
void SendMotors(int inMotor1, int inMotor2, int inMotor3, int inMotor4);
float GetYaw();
+ void ManiCtrl(float inHeight, int inRaiseSpeed, float inGripper, int inGripperSpeed);
float fLinearAccLimit;
float fAngularAccLimit;
@@ -87,4 +88,6 @@ protected:
void m_ParseFrame();
void m_DisRecv();
int GenCmd(int inBuffOffset, int inDevID, int inModule, int inMethod, unsigned char* inData, int inDataLen);
+ void MotorCmd(int inMethod, int inID1, int inValue1, int inID2, int inValue2);
+ void MotorCmd2(int inMethod, int inID1, int inValue1_1, int inValue1_2, int inID2, int inValue2_1, int inValue2_2);
};
\ No newline at end of file
diff --git a/wpb_home_bringup/launch/mani_test.launch b/wpb_home_bringup/launch/mani_test.launch
new file mode 100644
index 0000000..ee19b5c
--- /dev/null
+++ b/wpb_home_bringup/launch/mani_test.launch
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
diff --git a/wpb_home_bringup/src/driver/WPB_Home_driver.cpp b/wpb_home_bringup/src/driver/WPB_Home_driver.cpp
index 31e3155..09b8511 100644
--- a/wpb_home_bringup/src/driver/WPB_Home_driver.cpp
+++ b/wpb_home_bringup/src/driver/WPB_Home_driver.cpp
@@ -314,4 +314,45 @@ float CWPB_Home_driver::GetYaw()
{
float diffYaw = fCurYaw - fFirstYaw;
return diffYaw;
-}
\ No newline at end of file
+}
+
+
+void CWPB_Home_driver::MotorCmd(int inMethod, int inID1, int inValue1, int inID2, int inValue2)
+{
+ static unsigned char arMotorSpeedData[12];
+ m_Split2Bytes(arMotorSpeedData,inID1);
+ m_Split4Bytes(arMotorSpeedData + 2, inValue1);
+
+ m_Split2Bytes(arMotorSpeedData + 6, inID2);
+ m_Split4Bytes(arMotorSpeedData + 8, inValue2);
+ int nCmdLenght = GenCmd(0, 0x40, 0x08, inMethod, arMotorSpeedData, 12);
+ Send(m_SendBuf, nCmdLenght);
+}
+
+
+void CWPB_Home_driver::MotorCmd2(int inMethod, int inID1, int inValue1_1, int inValue1_2, int inID2, int inValue2_1, int inValue2_2)
+{
+ static unsigned char arMotorCmdData[20];
+ m_Split2Bytes(arMotorCmdData, inID1);
+ m_Split4Bytes(arMotorCmdData + 2, inValue1_1);
+ m_Split4Bytes(arMotorCmdData + 6, inValue1_2);
+
+ m_Split2Bytes(arMotorCmdData + 10, inID2);
+ m_Split4Bytes(arMotorCmdData + 12, inValue2_1);
+ m_Split4Bytes(arMotorCmdData + 16, inValue2_2);
+ int nCmdLenght = GenCmd(0, 0x40, 0x08, inMethod, arMotorCmdData, 20);
+ Send(m_SendBuf, nCmdLenght);
+}
+
+void CWPB_Home_driver::ManiCtrl(float inHeight, int inRaiseSpeed, float inGripper, int inGripperSpeed)
+{
+ if(inHeight > 0.01)
+ {
+ MotorCmd2(0x63, 5, inRaiseSpeed, inHeight, 6, inGripperSpeed, inGripper);
+ }
+ else
+ {
+ // 折叠
+ MotorCmd2(0x64, 5, inRaiseSpeed, inHeight, 6, inGripperSpeed, inGripper);
+ }
+}
diff --git a/wpb_home_bringup/src/wpb_home_test_mani.cpp b/wpb_home_bringup/src/wpb_home_test_mani.cpp
new file mode 100644
index 0000000..e8159d9
--- /dev/null
+++ b/wpb_home_bringup/src/wpb_home_test_mani.cpp
@@ -0,0 +1,125 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the WaterPlus nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+/*!******************************************************************
+ @author ZhangWanjie
+ ********************************************************************/
+
+#include
+#include "driver/WPB_Home_driver.h"
+
+#define WPBH_TEST_MANI_ZERO 0
+#define WPBH_TEST_MANI_DOWN 1
+#define WPBH_TEST_MANI_UP 2
+#define WPBH_TEST_MANI_FOLD 3
+
+#define CMD_WAIT_SEC 6
+
+static int nState = WPBH_TEST_MANI_ZERO;
+static int nCount = 0;
+
+int main(int argc, char** argv)
+{
+ ros::init(argc,argv,"wpb_home_test_mani");
+ ros::NodeHandle n;
+
+ ROS_WARN("[TEST] Manipulator...");
+ CWPB_Home_driver m_wpb_home;
+ ros::NodeHandle n_param("~");
+ std::string strSerialPort;
+ n_param.param("serial_port", strSerialPort, "/dev/ttyUSB0");
+ m_wpb_home.Open(strSerialPort.c_str(),115200);
+
+ ros::Rate r(1);
+
+ while(ros::ok())
+ {
+ switch(nState)
+ {
+ case WPBH_TEST_MANI_ZERO: //收起状态,先变成初始状态
+ ROS_INFO("[Mani] ZERO -> DOWN... %d",nCount);
+ if (nCount == 0)
+ {
+ m_wpb_home.ManiCtrl( 11200, 1000, 50000, 1000);
+ }
+ nCount ++;
+ if(nCount >= CMD_WAIT_SEC)
+ {
+ nState = WPBH_TEST_MANI_DOWN;
+ nCount = 0;
+ }
+ break;
+ case WPBH_TEST_MANI_DOWN:
+ ROS_INFO("[Mani] DOWN -> UP ... %d",nCount);
+ if (nCount == 0)
+ {
+ m_wpb_home.ManiCtrl( 55000, 1000, 0, 1000);
+ }
+ nCount ++;
+ if(nCount >= CMD_WAIT_SEC)
+ {
+ nState = WPBH_TEST_MANI_UP;
+ nCount = 0;
+ }
+ break;
+ case WPBH_TEST_MANI_UP:
+ ROS_INFO("[Mani] UP -> DOWN ... %d",nCount);
+ if (nCount == 0)
+ {
+ m_wpb_home.ManiCtrl( 11200, 1000, 0, 1000);
+ }
+ nCount ++;
+ if(nCount >= CMD_WAIT_SEC)
+ {
+ nState = WPBH_TEST_MANI_FOLD;
+ nCount = 0;
+ }
+ break;
+ case WPBH_TEST_MANI_FOLD:
+ ROS_INFO("[Mani] DOWN -> ZERO ... %d",nCount);
+ if (nCount == 0)
+ {
+ m_wpb_home.ManiCtrl( 0, 1000, 0, 1000);
+ }
+ nCount ++;
+ if(nCount >= CMD_WAIT_SEC)
+ {
+ nState = WPBH_TEST_MANI_ZERO;
+ nCount = 0;
+ }
+ break;
+ }
+
+ r.sleep();
+ }
+}
\ No newline at end of file
diff --git a/wpb_home_bringup/src/wpb_home_test_motors.cpp b/wpb_home_bringup/src/wpb_home_test_motors.cpp
index 491789d..6f368cf 100644
--- a/wpb_home_bringup/src/wpb_home_test_motors.cpp
+++ b/wpb_home_bringup/src/wpb_home_test_motors.cpp
@@ -51,9 +51,12 @@ int main(int argc, char** argv)
ros::init(argc,argv,"wpb_home_test_motors");
ros::NodeHandle n;
- ROS_INFO("TEST Start...");
+ ROS_INFO("[TEST] Motors...");
CWPB_Home_driver m_wpb_home;
- m_wpb_home.Open("/dev/ttyUSB1",115200);
+ ros::NodeHandle n_param("~");
+ std::string strSerialPort;
+ n_param.param("serial_port", strSerialPort, "/dev/ttyUSB0");
+ m_wpb_home.Open(strSerialPort.c_str(),115200);
int nTest = TEST_READY;
ros::Rate r(0.3);