virtual bool updateModule()
{
if (control_thr)
{
Odometry* pOdometry=0;
control_thr->printStats();
control_thr->get_motor_handler()->updateControlMode();
pOdometry = control_thr->get_odometry_handler();
if (pOdometry) pOdometry->printStats();
control_thr->get_motor_handler()->printStats();
control_thr->get_input_handler()->printStats();
}
else
{
yDebug("* Motor thread:not running");
}
static int life_counter=0;
yInfo( "* Life: %d\n\n", life_counter);
life_counter++;
return true;
}
//This function parses the user commands received through the RPC port
bool respond(const Bottle& command, Bottle& reply)
{
reply.clear();
if (command.get(0).asString()=="help")
{
reply.addVocab(Vocab::encode("many"));
reply.addString("Available commands are:");
reply.addString("run");
reply.addString("idle");
reply.addString("reset_odometry");
reply.addString("set_prefilter 0/1/2/4/8");
reply.addString("set_motors_filter 0/1/2/4/8");
reply.addString("change_pid <identif> <kp> <ki> <kd>");
reply.addString("change_ctrl_mode <type_string>");
reply.addString("set_debug_mode 0/1");
return true;
}
else if (command.get(0).asString()=="set_debug_mode")
{
if (control_thr)
{
if (command.get(1).asInt()>0)
{control_thr->enable_debug(true); reply.addString("debug mode on");}
else
{control_thr->enable_debug(false); reply.addString("debug mode off");}
}
return true;
}
else if (command.get(0).asString()=="set_prefilter")
{
if (control_thr)
{
if (command.get(1).asInt()>0)
{control_thr->set_input_filter(command.get(1).asInt()); reply.addString("Prefilter on");}
else
{control_thr->set_input_filter(0); reply.addString("Prefilter off");}
}
return true;
}
else if (command.get(0).asString()=="set_motors_filter")
{
if (control_thr)
{
int f= command.get(1).asInt();
if (f==1)
{control_thr->get_motor_handler()->set_motors_filter(MotorControl::HZ_1); reply.addString("Motors filter on");}
if (f==2)
{control_thr->get_motor_handler()->set_motors_filter(MotorControl::HZ_2); reply.addString("Motors filter on");}
if (f==4)
{control_thr->get_motor_handler()->set_motors_filter(MotorControl::HZ_4); reply.addString("Motors filter on");}
if (f==8)
{control_thr->get_motor_handler()->set_motors_filter(MotorControl::HZ_8); reply.addString("Motors filter on");}
else
{control_thr->get_motor_handler()->set_motors_filter(MotorControl::DISABLED); reply.addString("Motors filter off");}
}
return true;
}
else if (command.get(0).asString()=="run")
{
if (control_thr)
{
if (control_thr->get_control_type() == BASE_CONTROL_NONE) {control_thr->get_motor_handler()->set_control_idle();}
else if (control_thr->get_control_type() == BASE_CONTROL_VELOCITY_NO_PID) {control_thr->get_motor_handler()->set_control_velocity();}
else if (control_thr->get_control_type() == BASE_CONTROL_OPENLOOP_NO_PID) {control_thr->get_motor_handler()->set_control_openloop();}
else if (control_thr->get_control_type() == BASE_CONTROL_VELOCITY_PID) {control_thr->get_motor_handler()->set_control_velocity();}
else if (control_thr->get_control_type() == BASE_CONTROL_OPENLOOP_PID) {control_thr->get_motor_handler()->set_control_openloop();}
if (control_thr->get_motor_handler()->check_motors_on())
{reply.addString("Motors now on");}
else
{reply.addString("Unable to turn motors on! fault pressed?");}
}
return true;
}
else if (command.get(0).asString()=="idle")
{
if (control_thr)
{
control_thr->get_motor_handler()->set_control_idle();
{reply.addString("Motors now off.");}
}
return true;
}
else if (command.get(0).asString()=="change_ctrl_mode")
{
if (control_thr)
{
if (control_thr->set_control_type(command.get(1).asString().c_str()))
{reply.addString("control mode changed");}
else
{reply.addString("invalid control mode request");}
}
return true;
}
else if (command.get(0).asString()=="change_pid")
{
if (control_thr)
{
string identif = command.get(1).asString().c_str();
double kp = command.get(2).asDouble();
double ki = command.get(3).asDouble();
double kd = command.get(4).asDouble();
control_thr->set_pid(identif,kp,ki,kd);
reply.addString("New pid parameters set.");
yInfo("New pid parameters set.");
}
return true;
}
else if (command.get(0).asString()=="reset_odometry")
{
if (control_thr)
{
Odometry* pOdometry=0;
pOdometry = control_thr->get_odometry_handler();
if (pOdometry)
{pOdometry->reset_odometry(); reply.addString("Odometry reset done.");}
else
{reply.addString("No Odometry available.");}
}
return true;
}
reply.addString("Unknown command.");
return true;
}
