ROS_Robot/wpb_home
1
0
Fork 0
mirror of https://github.com/6-robot/wpb_home.git synced 2025-09-15 12:58:59 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

mani_test

Browse Source
This commit is contained in:
ZhangWanjie 2017-12-20 21:53:38 +08:00
parent d7443d6ee9
commit b36184e35d
6 changed files with 195 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
wpb_home_bringup/CMakeLists.txt
View File

@ -232,7 +232,6 @@ target_link_libraries(wpb_home_lidar_filter
)
## Declare a C++ executable
add_executable(wpb_home_imu_tf
src/wpb_home_imu_tf.cpp
)
@ -240,3 +239,14 @@ add_dependencies(wpb_home_imu_tf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EX
target_link_libraries(wpb_home_imu_tf
${catkin_LIBRARIES}
)
## 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}
)

3
wpb_home_bringup/include/driver/WPB_Home_driver.h
View File

@ -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);
};

8
wpb_home_bringup/launch/mani_test.launch Normal file
View File

@ -0,0 +1,8 @@
<launch>
<!-- wpb_home mani test-->
<node pkg="wpb_home_bringup" type="wpb_home_test_mani" name="wpb_home_test_mani" output="screen">
<param name="serial_port" type="string" value="/dev/ftdi"/>
</node>
</launch>

41
wpb_home_bringup/src/driver/WPB_Home_driver.cpp
View File

@ -315,3 +315,44 @@ float CWPB_Home_driver::GetYaw()
float diffYaw = fCurYaw - fFirstYaw;
return diffYaw;
}
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);
}
}

125
wpb_home_bringup/src/wpb_home_test_mani.cpp Normal file
View File

@ -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 <ros/ros.h>
#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<std::string>("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();
}
}

7
wpb_home_bringup/src/wpb_home_test_motors.cpp
View File

@ -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<std::string>("serial_port", strSerialPort, "/dev/ttyUSB0");
m_wpb_home.Open(strSerialPort.c_str(),115200);
int nTest = TEST_READY;
ros::Rate r(0.3);