33 #include <std_msgs/String.h>
40 #include <sys/types.h>
45 #include <hitec5980sg/hitec5980sg.h>
47 #include <phantom_control/State.h>
48 #include <humanoid_control/Humanoid.h>
65 const short int id_list[12]={11,21,12,22,13,23,15,25,16,26,31,32};
69 double joint_angle, current_POS;
70 double z_scale = 0.5, x_scale = 5, y_scale=0.8;
80 joint_angle = acos ((
g_ZZ + current_POS * z_scale)/
g_ZZ) * 180.0 /
PI;
82 if(isnan(joint_angle)) joint_angle=0;
83 if(joint_angle>20.) joint_angle=20.0;
92 cout<<
"Ankle angle = "<<joint_angle<<endl;
98 joint_angle = atan2(current_POS * x_scale,
g_ZZ) * 180.0 /
PI;
100 if(isnan(joint_angle)) joint_angle=0;
101 if(joint_angle>30.) joint_angle=30.0;
102 if(joint_angle<-30.) joint_angle=-30.0;
109 cout<<
"Ankle angle = "<<joint_angle<<endl;
115 cout<<
"First Time"<<endl;
130 joint_angle = asin((current_POS * y_scale)/
THIGH_LENGTH) * 180.0 /
PI + 20.;
143 cout<<
"Hip angle = "<<joint_angle<<endl;
152 for (
int i = 0; i < 13; i++)
163 int main(
int argc,
char** argv)
165 short unsigned int resp=65535;
168 ros::init( argc, argv,
"humanoid_node" );
170 ros::NodeHandle n(
"~");
172 ros::Publisher pub_state= n.advertise< humanoid_control::Humanoid >(
"/humanoid_state", 1000 );
174 ros::Subscriber sub_force = n.subscribe (
"/phantom_state", 1,
PhantomCallBk );
177 int op_mode_default = 0;
178 n.param(
"op_mode", op_mode, op_mode_default );
184 ROS_INFO(
"Operation mode of humanoid robot is 0");
185 ROS_INFO(
"STOP MODE");
203 ROS_INFO(
"Operation mode of humanoid robot is 1");
204 ROS_INFO(
"BOUNCY MODE");
208 ROS_INFO(
"Operation mode of humanoid robot is 2");
209 ROS_INFO(
"SHAKY MODE");
213 ROS_INFO(
"Operation mode of humanoid robot is 3");
214 ROS_INFO(
"HUMPING MODE");
218 for (
int i = 0; i < 12; i++)
247 int pid = getpid(), rpid;
249 boost::format fmt(
"sudo renice -10 %d");
252 rpid = std::system(fmt.str().c_str());
256 for (uint i = 0; i <12 ; i++)
269 for (
int i = 0; i < 13 ; i++)
276 g_RState.header.stamp=ros::Time::now();
short unsigned int SetJointSpeed(short int id, unsigned int speed)
int main(int argc, char **argv)
double ConvertServoValueByID(int id, short unsigned int servo_value)
phantom_control::State g_PState
humanoid_control::Humanoid g_RState
void PhantomCallBk(const phantom_control::State &phantom_state)
short unsigned int MoveJoint(short int id, double joint_angle)
struct ServoHumanoid::RoboState RoboState
const short int id_list[12]
short unsigned int op_mode
Class develop to control PHUA robot's servomotors using hitec_5980SG Lar3's' module.