/*
* This is the main program.
* The arguments of the main function can be specified by the
* "controllerArgs" field of the Robot node
*/
int main(int argc, char **argv)
{
//int cont=0;
using_shared_memory();
/* necessary to initialize webots stuff */
wb_robot_init();
// do this once only Darwin
WbNodeRef robot_node = wb_supervisor_node_get_from_def("Darwin");
WbFieldRef trans_field = wb_supervisor_node_get_field(robot_node, "translation");
WbFieldRef rot_field = wb_supervisor_node_get_field(robot_node, "rotation");
int caiu_cont = 0;
// do this once only Darwin2
WbNodeRef robot_node_2 = wb_supervisor_node_get_from_def("Darwin2");
WbFieldRef trans_field_2 = wb_supervisor_node_get_field(robot_node_2, "translation");
WbFieldRef rot_field_2 = wb_supervisor_node_get_field(robot_node_2, "rotation");
// do this once only Darwin3
WbNodeRef robot_node_3 = wb_supervisor_node_get_from_def("Darwin3");
WbFieldRef trans_field_3 = wb_supervisor_node_get_field(robot_node_3, "translation");
WbFieldRef rot_field_3 = wb_supervisor_node_get_field(robot_node_3, "rotation");
while (1) {
// this is done repeatedly
const double *trans = wb_supervisor_field_get_sf_vec3f(trans_field);
const double *trans_2 = wb_supervisor_field_get_sf_vec3f(trans_field_2);
const double *trans_3 = wb_supervisor_field_get_sf_vec3f(trans_field_3);
TRANS1 = trans[0];
TRANS2 = trans[1];
TRANS3 = trans[2];
TRANS1_2 = trans_2[0];
TRANS2_2 = trans_2[1];
TRANS3_2 = trans_2[2];
TRANS1_3 = trans_3[0];
TRANS2_3 = trans_3[1];
TRANS3_3 = trans_3[2];
//printf("MY_ROBOT is at position: %g %g %g\n", trans[0], trans[1], trans[2]);
wb_robot_step(32);
if(RESET_ROBOT == 1)
{
caiu_cont++;
CAIU_CONT = caiu_cont;
// reset robot position
const double INITIAL[3] = { 0, 0.32004, 0 };
const double INITIAL_ROT[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field, INITIAL);
wb_supervisor_field_set_sf_rotation(rot_field, INITIAL_ROT);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT = 0;
if(caiu_cont > 25)
{
caiu_cont = 0;
wb_supervisor_simulation_revert(); // restart the simulation
// A medida que o tempo vai passando o robo Darwin vai ficando
// cada vez mais devagar como se houvesse algum tipo de desgaste fisico
//entao e necessario restarta a simulaçao
}
}
if(RESET_ROBOT_2 == 1)
{
// reset robot position
const double INITIAL_2[3] = { 0.2, 0.32004, 4.7 };
const double INITIAL_ROT_2[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field_2, INITIAL_2);
wb_supervisor_field_set_sf_rotation(rot_field_2, INITIAL_ROT_2);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT_2 = 0;
}
if(RESET_ROBOT_3 == 1)
{
// reset robot position
const double INITIAL_3[3] = { 3.2, 0.32004, -3.7 };
const double INITIAL_ROT_3[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field_3, INITIAL_3);
wb_supervisor_field_set_sf_rotation(rot_field_3, INITIAL_ROT_3);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT_3 = 0;
}
}
/*
* You should declare here WbDeviceTag variables for storing
* robot devices like this:
* WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
* WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
*/
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}
/**
* Node entry-point. Handles ROS setup, and serial port connection/reconnection.
*/
int main(int argc, char **argv)
{
//ros::init(argc, argv, "um7_driver");
using_shared_memory();
// Load parameters from private node handle.
std::string port("/dev/robot/imu");
int32_t baud = 115200;
//ros::param::param<std::string>("~port", port, "/dev/ttyUSB0");
//ros::param::param<int32_t>("~baud", baud, 115200);
serial::Serial ser;
ser.setPort(port);
ser.setBaudrate(baud);
serial::Timeout to = serial::Timeout(50, 50, 0, 50, 0);
ser.setTimeout(to);
//ros::NodeHandle n;
//std_msgs::Header header;
//ros::param::param<std::string>("~frame_id", header.frame_id, "imu_link");
// Initialize covariance. The UM7 sensor does not provide covariance values so,
// by default, this driver provides a covariance array of all zeros indicating
// "covariance unknown" as advised in sensor_msgs/Imu.h.
// This param allows the user to specify alternate covariance values if needed.
// std::string covariance;
// char cov[200];
// char *ptr1;
// ros::param::param<std::string>("~covariance", covariance, "0 0 0 0 0 0 0 0 0");
// snprintf(cov, sizeof(cov), "%s", covariance.c_str());
// char* p = strtok_r(cov, " ", &ptr1); // point to first value
// for (int iter = 0; iter < 9; iter++)
// {
// if (p) covar[iter] = atof(p); // covar[] is global var
// else covar[iter] = 0.0;
// p = strtok_r(NULL, " ", &ptr1); // point to next value (nil if none)
// }
// Real Time Loop
bool first_failure = true;
while (1)
{
try
{
ser.open();
}
catch(const serial::IOException& e)
{
std::cout<<"Unable to connect to port."<<std::endl;
}
if (ser.isOpen())
{
std::cout<<"Successfully connected to serial port."<<std::endl;
first_failure = true;
try
{
um7::Comms sensor(&ser);
configureSensor(&sensor);
um7::Registers registers;
//ros::ServiceServer srv = n.advertiseService<um7::Reset::Request, um7::Reset::Response>(
// "reset", boost::bind(handleResetService, &sensor, _1, _2));
handleResetService(&sensor);
int t=0;
int contador = 0;
float med_accel_z = 0, ac_med_accel_z = 0;
while (1)
{
//--------- calcula a média do accel em Z-----------------------------
if(contador>=40)
{
med_accel_z = ac_med_accel_z/40; // calcula a média do accel em Z
contador = 0;
ac_med_accel_z = 0;
}
ac_med_accel_z = ac_med_accel_z + IMU_ACCEL_Z;
contador++;
//--------------------------------------------------------------------
if(med_accel_z>0.70) // Identifica se o robô esta caido ou em pé
IMU_STATE = 0; // Robo caido
else
IMU_STATE = 1; // Robo em pé
if(t>20)
{
std::cout<<"Robo caido = "<<std::fixed<<IMU_STATE<<std::endl;
std::cout<<"med_acc_z = "<<std::fixed<<med_accel_z<<std::endl;
std::cout<<"giros_x = "<<std::fixed<<IMU_GYRO_X<<std::endl;
std::cout<<"giros_y = "<<std::fixed<<IMU_GYRO_Y<<std::endl;
std::cout<<"giros_z = "<<std::fixed<<IMU_GYRO_Z<<std::endl;
std::cout<<"accel_x = "<<std::fixed<<IMU_ACCEL_X<<std::endl;
std::cout<<"accel_y = "<<std::fixed<<IMU_ACCEL_Y<<std::endl;
std::cout<<"accel_z = "<<std::fixed<<IMU_ACCEL_Z<<std::endl;
std::cout<<"magne_x = "<<std::fixed<<IMU_COMPASS_X<<std::endl;
std::cout<<"magne_y = "<<std::fixed<<IMU_COMPASS_Y<<std::endl;
std::cout<<"magne_z = "<<std::fixed<<IMU_COMPASS_Z<<std::endl;
std::cout<<"Quat_x = "<<std::fixed<<IMU_QUAT_X<<std::endl;
std::cout<<"Quat_y = "<<std::fixed<<IMU_QUAT_Y<<std::endl;
std::cout<<"Quat_z = "<<std::fixed<<IMU_QUAT_Z<<std::endl;
std::cout<<"Euler_x = "<<std::fixed<<IMU_EULER_X<<std::endl;
std::cout<<"Euler_y = "<<std::fixed<<IMU_EULER_Y<<std::endl;
std::cout<<"Euler_z = "<<std::fixed<<IMU_EULER_Z<<std::endl<<std::endl;
t=0;
}
t++;
// triggered by arrival of last message packet
if (sensor.receive(®isters) == TRIGGER_PACKET)
{
//header.stamp = ros::Time::now();
publishMsgs(registers);
//ros::spinOnce();
}
}
}
catch(const std::exception& e)
{
if (ser.isOpen()) ser.close();
//ROS_ERROR_STREAM(e.what());
//ROS_INFO("Attempting reconnection after error.");
std::cout<<"Attempting reconnection after error."<<std::endl;
//ros::Duration(1.0).sleep();
}
}
else
{
//ROS_WARN_STREAM_COND(first_failure, "Could not connect to serial device "
// << port << ". Trying again every 1 second.");
std::cout<< "Could not connect to serial device "
<< port << ". Trying again every 1 second."<< std::endl;
first_failure = false;
//ros::Duration(1.0).sleep();
}
}
}
