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