00001
00002
00003
00004
00005 #include <robotican_hardware_interface/RiCBoardManager.h>
00006
00007 namespace robotican_hardware {
00008 RiCBoardManager::RiCBoardManager() : _nodeHandle(), _spinner(1) ,_transportLayer(getPort(), getBaudrate()) {
00009 _idGen = 0;
00010 resetBuff();
00011 setConnectState(ConnectEnum::Disconnected);
00012 boost::thread thread(&RiCBoardManager::handleMessage, this);
00013 _timeoutKeepAliveTimer = _nodeHandle.createTimer(ros::Duration(1.0), &RiCBoardManager::timeoutKeepAliveEvent, this);
00014 _sendKeepAliveTimer = _nodeHandle.createTimer(ros::Duration(1.0), &RiCBoardManager::sendKeepAliveEvent, this);
00015 _timeoutKeepAliveTimer.stop();
00016 _sendKeepAliveTimer.stop();
00017 _spinner.start();
00018
00019 }
00020
00021 void RiCBoardManager::connect() {
00022 ConnectState connectState;
00023 connectState.length = sizeof(connectState);
00024 connectState.state = ConnectEnum::Connected;
00025 connectState.version = PC_VERSION;
00026 uint8_t *bytes = (uint8_t *) &connectState;
00027 connectState.checkSum = 0;
00028 connectState.checkSum = _transportLayer.calcChecksum(bytes, connectState.length);
00029 _transportLayer.write(bytes, connectState.length);
00030
00031 }
00032
00033 void RiCBoardManager::disconnect() {
00034 ConnectState connectState;
00035 connectState.length = sizeof(connectState);
00036 connectState.state = ConnectEnum::Disconnected;
00037 connectState.version = PC_VERSION;
00038 uint8_t *bytes = (uint8_t *) &connectState;
00039 connectState.checkSum = 0;
00040 connectState.checkSum = _transportLayer.calcChecksum(bytes, connectState.length);
00041 _transportLayer.write(bytes, connectState.length);
00042 clear();
00043
00044 }
00045
00046 unsigned int RiCBoardManager::getBaudrate() {
00047 int baudrate;
00048 ros::param::param<int>("baudrate", baudrate, 9600);
00049 return (unsigned int) baudrate;
00050 }
00051
00052 std::string RiCBoardManager::getPort() {
00053 std::string post;
00054 ros::param::param<std::string>("port", post, "/dev/RiCBoard");
00055 return post;
00056
00057 }
00058
00059 void RiCBoardManager::handleMessage() {
00060 while(ros::ok()) {
00061 if(_transportLayer.tryToRead(_rcvBuff, MAX_BUFF_SIZE)) {
00062 Header *header = (Header*) _rcvBuff;
00063 crc prevCheckSum = header->checkSum;
00064 header->checkSum = 0;
00065
00066 crc curCheckSum = _transportLayer.calcChecksum(_rcvBuff, header->length);
00067
00068 if(curCheckSum == prevCheckSum) {
00069 switch (header->dataType) {
00070 case DataType::ConnectionState:
00071 connectionHandle((ConnectState*)header);
00072 break;
00073 case DataType::Debug:
00074 debugMsgHandler((DebugMsg*) header);
00075 break;
00076 case DataType::KeepAlive:
00077 keepAliveHandle((KeepAliveMsg*)header);
00078 break;
00079 case DataType::DeviceMessage:
00080 deviceMessageHandler((DeviceMessage *)header);
00081 break;
00082 default:
00083 break;
00084 }
00085
00086 }
00087 else {
00088 #ifdef RIC_BOARD_DEBUG
00089 char errorBuff[128] = {'\0'};
00090 sprintf(errorBuff, "Invalid checksum {cur: %d, prev: %d}, msg_data_Type: %d, msg_len: %d", curCheckSum, prevCheckSum, header->dataType, header->length);
00091 ros_utils::rosError(errorBuff);
00092 #endif
00093 }
00094 resetBuff();
00095 }
00096
00097 }
00098
00099 _timeoutKeepAliveTimer.stop();
00100 _sendKeepAliveTimer.stop();
00101 ros_utils::rosInfo("by");
00102 }
00103
00104 void RiCBoardManager::resetBuff() {
00105 for(int i = 0; i < MAX_BUFF_SIZE; ++i) {
00106 _rcvBuff[i] = 0;
00107 }
00108 }
00109
00110 ConnectEnum::ConnectEnum RiCBoardManager::getConnectState() {
00111 return _connectState;
00112 }
00113
00114 void RiCBoardManager::setConnectState(ConnectEnum::ConnectEnum connectState) {
00115 _connectState = connectState;
00116 }
00117
00118 void RiCBoardManager::connectionHandle(ConnectState *connectState) {
00119 switch(connectState->state) {
00120 case ConnectEnum::Connected:
00121 if(getConnectState() == ConnectEnum::Disconnected) {
00122 if(connectState->version != PC_VERSION) {
00123 ros_utils::rosError("Version not match");
00124 std::string hex = ros::package::getPath("robotican_common") + "/exec/firmware.hex";
00125 std::string exec = ros::package::getPath("robotican_common") + "/exec/RiCBoardLoader --mcu=mk20dx256 -sv " + hex;
00126 ros_utils::rosInfo(exec.c_str());
00127 FILE *process = popen(exec.c_str(), "r");
00128
00129 if(process == 0) {
00130 ros_utils::rosError("Can't start process");
00131 }
00132 else {
00133 ros_utils::rosInfo("Upload current firmware, this action will restart the process please wait");
00134 ros::Duration(5.0).sleep();
00135 ros_utils::rosInfo("Upload current firmware: Done");
00136 ros_utils::rosInfo("Restarting the process");
00137 }
00138 ros::shutdown();
00139 return;
00140 }
00141 setConnectState(ConnectEnum::Connected);
00142 _sendKeepAliveTimer.start();
00143 _timeoutKeepAliveTimer.setPeriod(ros::Duration(3.0), true);
00144 _timeoutKeepAliveTimer.start();
00145 ros_utils::rosInfo("Handshake complete: RiCBoard is connected");
00146
00147 }
00148 break;
00149 case ConnectEnum::NotReady:
00150 break;
00151 case ConnectEnum::AlreadyConnected:
00152 break;
00153 case ConnectEnum::Disconnected:
00154 if(getConnectState() == ConnectEnum::Connected) {
00155 setConnectState(ConnectEnum::Disconnected);
00156 ros_utils::rosInfo("RiCBoard is now disconnected");
00157 ros::shutdown();
00158 }
00159 break;
00160 case ConnectEnum::AlreadyDisconnected:
00161 break;
00162 default:break;
00163 }
00164 }
00165
00166 void RiCBoardManager::debugMsgHandler(DebugMsg *debugMsg) {
00167 switch (debugMsg->level) {
00168 case DebugLevel::Info:
00169 ros_utils::rosInfo(debugMsg->message);
00170 break;
00171 case DebugLevel::Warn:
00172 ros_utils::rosWarn(debugMsg->message);
00173 break;
00174 case DebugLevel::Error:
00175 ros_utils::rosError(debugMsg->message);
00176 break;
00177 case DebugLevel::Fatal:
00178 ros_utils::rosError(debugMsg->message);
00179 break;
00180 default:break;
00181 }
00182 }
00183
00184 void RiCBoardManager::sendKeepAliveEvent(const ros::TimerEvent &timerEvent) {
00185 KeepAliveMsg keepAliveMsg;
00186 keepAliveMsg.length = sizeof(keepAliveMsg);
00187 keepAliveMsg.checkSum = 0;
00188 keepAliveMsg.state = KeepAliveState::Ok;
00189 uint8_t* rawData = (uint8_t*) &keepAliveMsg;
00190 keepAliveMsg.checkSum = _transportLayer.calcChecksum(rawData, keepAliveMsg.length);
00191 _transportLayer.write(rawData, keepAliveMsg.length);
00192 }
00193
00194 void RiCBoardManager::timeoutKeepAliveEvent(const ros::TimerEvent &timerEvent) {
00195 ros_utils::rosError("RiCBoard not responding. Shuting down the program");
00196 ros::shutdown();
00197 clear();
00198 }
00199
00200 void RiCBoardManager::keepAliveHandle(KeepAliveMsg *keepAliveMsg) {
00201 switch(keepAliveMsg->state) {
00202 case KeepAliveState::Ok:
00203 _timeoutKeepAliveTimer.setPeriod(ros::Duration(3.0), true);
00204 break;
00205 case KeepAliveState::NeedToRestart:
00206 break;
00207 case KeepAliveState::FatalError:
00208 break;
00209 default:
00210 break;
00211 }
00212
00213 }
00214
00215 void RiCBoardManager::buildDevices() {
00216 bool haveBattery;
00217 ros::param::param<bool>("have_battery", haveBattery, false);
00218 if(haveBattery) {
00219 int pin;
00220 float voltageDividerRatio, max, min;
00221 if(_nodeHandle.getParam("battery_pin", pin)
00222 && _nodeHandle.getParam("battery_voltage_divider_ratio", voltageDividerRatio)
00223 && _nodeHandle.getParam("battery_max", max)
00224 && _nodeHandle.getParam("battery_min", min)) {
00225 Device *battery = new Battery(_idGen++, voltageDividerRatio, max, min, (byte) pin, &_transportLayer);
00226 _devices.push_back(battery);
00227 battery->buildDevice();
00228 }
00229 else {
00230 ros_utils::rosError("Can't build battery some of the parameters are missing");
00231 }
00232 }
00233
00234 int ultrasonicSize = 0;
00235 bool haveUltrasonic;
00236 ros::param::param<bool>("have_ultrasonic", haveUltrasonic, false);
00237 ros::param::param("ultrasonic_size", ultrasonicSize, 0);
00238
00239 if (haveUltrasonic) {
00240 for(int i = 0; i < ultrasonicSize; ++i) {
00241 std::string ultrasonicIdentifier = "ultrasonic" + boost::lexical_cast<std::string>(i), frameId, topicName ,sonarName;
00242 int pin;
00243 ros_utils::rosInfo(ultrasonicIdentifier.c_str());
00244 if(_nodeHandle.getParam(ultrasonicIdentifier + "_pin", pin)
00245 && _nodeHandle.getParam(ultrasonicIdentifier + "_frame_id", frameId)
00246 && _nodeHandle.getParam(ultrasonicIdentifier + "_topic_name", topicName)
00247 && _nodeHandle.getParam(ultrasonicIdentifier + "_sonar_name", sonarName)) {
00248 Device *ultrasonic = new Ultrasonic(_idGen++, &_transportLayer, pin, topicName, frameId, sonarName);
00249 _devices.push_back(ultrasonic);
00250 ultrasonic->buildDevice();
00251 }
00252 }
00253 }
00254
00255 bool haveImu;
00256 ros::param::param<bool>("have_imu", haveImu, false);
00257 if(haveImu) {
00258 int fusionHz;
00259 bool enableGyro, fuseCompass;
00260 std::vector<double> imuLinearAccFix, imuAngularVelocityFix,
00261 imuMagnetometerFix, imuRotationFix,
00262 imuRotationOffset;
00263 std::string frameId;
00264
00265 if(_nodeHandle.getParam("imu_fusion_hz", fusionHz)
00266 && _nodeHandle.getParam("imu_enable_gyro", enableGyro)
00267 && _nodeHandle.getParam("imu_enable_fuse_compass", fuseCompass)
00268 && _nodeHandle.getParam("imu_frame_id", frameId)
00269 && _nodeHandle.getParam("imu_linear_acc_fix", imuLinearAccFix)
00270 && _nodeHandle.getParam("imu_angular_velocity_fix", imuAngularVelocityFix)
00271 && _nodeHandle.getParam("imu_magnetometer_fix", imuMagnetometerFix)
00272 && _nodeHandle.getParam("imu_rotation_fix", imuRotationFix)
00273 && _nodeHandle.getParam("imu_rotation_offset", imuRotationOffset)) {
00274 if(imuLinearAccFix.size() == 9
00275 && imuAngularVelocityFix.size() == 9
00276 && imuMagnetometerFix.size() == 9
00277 && imuRotationFix.size() == 9
00278 && imuRotationOffset.size() == 3) {
00279 Device *imu = new Imu(_idGen++, &_transportLayer, (uint16_t) fusionHz, frameId, enableGyro,
00280 fuseCompass, imuLinearAccFix, imuAngularVelocityFix,
00281 imuMagnetometerFix,
00282 imuRotationFix, imuRotationOffset);
00283 _devices.push_back(imu);
00284 imu->buildDevice();
00285 }
00286 }
00287 }
00288 bool haveGps;
00289 ros::param::param<bool>("have_gps", haveGps, false);
00290 if(haveGps) {
00291 int baudrate;
00292 std::string frameId, topicName;
00293
00294
00295 if(_nodeHandle.getParam("gps_baudrate", baudrate)
00296 && _nodeHandle.getParam("gps_topic_name", topicName)
00297 && _nodeHandle.getParam("gps_frame_id", frameId)) {
00298 ROS_INFO("[%s]: trying to build gps", ros::this_node::getName().c_str());
00299 Device *gps = new Gps(_idGen++, &_transportLayer, (uint16_t) baudrate, topicName, frameId);
00300 _devices.push_back(gps);
00301 gps->buildDevice();
00302 }
00303 }
00304
00305
00306 int switchSize = 0;
00307 bool haveSwitch;
00308 ros::param::param<bool>("have_switch", haveSwitch, false);
00309 ros::param::param<int>("switch_size", switchSize, 0);
00310
00311 if (haveSwitch) {
00312 for(int i = 0; i < switchSize; ++i) {
00313 std::string switchIdentifier = "switch" + boost::lexical_cast<std::string>(i), topicName;
00314 int pin;
00315 if(_nodeHandle.getParam(switchIdentifier + "_topic_name", topicName)
00316 && _nodeHandle.getParam(switchIdentifier + "_pin", pin)) {
00317 Device *switchDev = new Switch(_idGen++, &_transportLayer, (byte) pin, topicName);
00318 _devices.push_back(switchDev);
00319 switchDev->buildDevice();
00320 }
00321 }
00322 }
00323
00324 int relaySize = 0;
00325 bool haveRelay;
00326 ros::param::param<bool>("have_relay", haveRelay, false);
00327 ros::param::param<int>("relay_size", relaySize, 0);
00328 if (haveRelay) {
00329 for(int i = 0; i < relaySize; ++i) {
00330
00331 std::string relayIdentifier = "relay" + boost::lexical_cast<std::string>(i), serviceName;
00332 int pin;
00333 if(_nodeHandle.getParam(relayIdentifier + "_service_name", serviceName)
00334 && _nodeHandle.getParam(relayIdentifier + "_pin", pin)) {
00335 Device *relay = new Relay(_idGen++, &_transportLayer, (byte) pin, serviceName);
00336 _devices.push_back(relay);
00337 relay->buildDevice();
00338
00339 }
00340 }
00341 }
00342 }
00343
00344 void RiCBoardManager::deviceMessageHandler(DeviceMessage *deviceMsg) {
00345 size_t devicesSize = _devices.size();
00346 if(devicesSize > deviceMsg->id) {
00347 switch ((DeviceMessageType::DeviceMessageType) deviceMsg->deviceMessageType) {
00348 case DeviceMessageType::BuildDevice:
00349 break;
00350 case DeviceMessageType::Ack: {
00351 DeviceAck* ack = (DeviceAck *) deviceMsg;
00352 if(_devices.size() > ack->ackId) {
00353 _devices[ack->ackId]->deviceAck((DeviceAck *) deviceMsg);
00354 }
00355 else {
00356 char buff[128] = {'\0'};
00357 sprintf(buff, "worng size ack is: %d", ack->ackId);
00358 ros_utils::rosError(buff);
00359 }
00360 }
00361 break;
00362 case DeviceMessageType::MotorSetPointMsg:
00363 break;
00364 case DeviceMessageType::MotorFeedback:
00365 _devices[deviceMsg->id]->update(deviceMsg);
00366 break;
00367 case DeviceMessageType::SetMotorParam:
00368 break;
00369 case DeviceMessageType::ServoFeedback:
00370 _devices[deviceMsg->id]->update(deviceMsg);
00371 break;
00372 case DeviceMessageType::ServoSetPoint:
00373 break;
00374 case DeviceMessageType::SwitchFeedback:
00375 _devices[deviceMsg->id]->update(deviceMsg);
00376 break;
00377 case DeviceMessageType::UltrasonicFeedback:
00378 _devices[deviceMsg->id]->update(deviceMsg);
00379 break;
00380 case DeviceMessageType::RelySetState:
00381 break;
00382 case DeviceMessageType::GpsFeedback:
00383 _devices[deviceMsg->id]->update(deviceMsg);
00384 break;
00385 case DeviceMessageType::ImuFeedback:
00386 _devices[deviceMsg->id]->update(deviceMsg);
00387 break;
00388 case DeviceMessageType::BatteryFeedback:
00389 _devices[deviceMsg->id]->update(deviceMsg);
00390 break;
00391 case DeviceMessageType::ImuClibFeedback:
00392 _devices[deviceMsg->id]->update(deviceMsg);
00393 break;
00394 }
00395 }
00396
00397 }
00398
00399 void RiCBoardManager::clear() {
00400 size_t size = _devices.size();
00401 if(size > 0) {
00402 for(int i = 0; i < size; ++i) delete _devices[i];
00403 _devices.clear();
00404 }
00405
00406 }
00407
00408
00409 void RiCBoardManager::buildDevices(hardware_interface::JointStateInterface* jointStateInterface,
00410 hardware_interface::PositionJointInterface* jointPositionInterface) {
00411 int servoSize = 0;
00412 bool haveServo;
00413 ros::param::param<bool>("have_servo", haveServo, false);
00414 ros::param::param<int>("servo_size", servoSize, 0);
00415 if (haveServo) {
00416 for(int i = 0; i < servoSize; ++i) {
00417 std::string servoIdentifier = "servo" + boost::lexical_cast<std::string>(i), servoJointName = "";
00418 float a , b, max, min, initPos;
00419 int pin;
00420
00421 if(_nodeHandle.getParam(servoIdentifier + "_a", a)
00422 &&_nodeHandle.getParam(servoIdentifier + "_b", b)
00423 &&_nodeHandle.getParam(servoIdentifier + "_max", max)
00424 &&_nodeHandle.getParam(servoIdentifier + "_min", min)
00425 &&_nodeHandle.getParam(servoIdentifier + "_init_pos", initPos)
00426 &&_nodeHandle.getParam(servoIdentifier + "_pin", pin)
00427 &&_nodeHandle.getParam(servoIdentifier + "_joint_name", servoJointName)) {
00428 Servo* servo = new Servo(_idGen++, &_transportLayer, (byte) pin, a, b, max, min, initPos, servoJointName);
00429 JointInfo_t* servoJointInfo = servo->getJointInfo();
00430 hardware_interface::JointStateHandle jointStateHandle(servoJointName,
00431 &servoJointInfo->position,
00432 &servoJointInfo->velocity,
00433 &servoJointInfo->effort);
00434
00435
00436 jointStateInterface->registerHandle(jointStateHandle);
00437 hardware_interface::JointHandle JointHandle(jointStateInterface->getHandle(servoJointName),
00438 &servoJointInfo->cmd);
00439 jointPositionInterface->registerHandle(JointHandle);
00440 _devices.push_back(servo);
00441
00442 servo->buildDevice();
00443 }
00444 }
00445 }
00446
00447 int closeMotorsWithPotentiometerSize = 0;
00448 bool haveCloseLoopPosition;
00449 ros::param::param<bool>("have_close_loop_position_motor", haveCloseLoopPosition, true);
00450 ros::param::param("position_motor_size", closeMotorsWithPotentiometerSize, 0);
00451
00452 if (haveCloseLoopPosition) {
00453 for(int i = 0; i < closeMotorsWithPotentiometerSize; ++i) {
00454 std::string motorIdentifier = "positionMotor" + boost::lexical_cast<std::string>(i), positionMotorJointName = "";
00455 int readPin, LPFHzSpeed, LPFHzInput, PIDHz, PPR, timeout, motorDirection, encoderDirection, motorAddress
00456 , eSwitchPin, eSwitchType, motorType, motorMode, toleranceTime;
00457 float LPFAlphaSpeed, LPFAlphaInput, KP, KI, KD, KFF, K,maxSetPointSpeed, minSetPointSpeed, maxSetPointPos, minSetPointPos,limit, a, b, tolerance, stopLimitMax, stopLimitMin;
00458 if(_nodeHandle.getParam(motorIdentifier + "_motor_type", motorType)) {
00459 switch ((CloseMotorType::CloseMotorType) motorType) {
00460 case CloseMotorType::CloseLoopWithEncoder:break;
00461
00462 case CloseMotorType::CloseLoopWithPotentiometer:
00463
00464 if (_nodeHandle.getParam(motorIdentifier + "_a", a)
00465 && _nodeHandle.getParam(motorIdentifier + "_b", b)
00466 && _nodeHandle.getParam(motorIdentifier + "_lpf_hz_speed", LPFHzSpeed)
00467 && _nodeHandle.getParam(motorIdentifier + "_lpf_hz_input", LPFHzInput)
00468 && _nodeHandle.getParam(motorIdentifier + "_pid_hz", PIDHz)
00469 && _nodeHandle.getParam(motorIdentifier + "_ppr", PPR)
00470 && _nodeHandle.getParam(motorIdentifier + "_timeout", timeout)
00471 && _nodeHandle.getParam(motorIdentifier + "_motor_direction", motorDirection)
00472 && _nodeHandle.getParam(motorIdentifier + "_encoder_direction", encoderDirection)
00473 && _nodeHandle.getParam(motorIdentifier + "_lpf_alpha_speed", LPFAlphaSpeed)
00474 && _nodeHandle.getParam(motorIdentifier + "_lpf_alpha_input", LPFAlphaInput)
00475 && _nodeHandle.getParam(motorIdentifier + "_kp", KP)
00476 && _nodeHandle.getParam(motorIdentifier + "_ki", KI)
00477 && _nodeHandle.getParam(motorIdentifier + "_kd", KD)
00478 && _nodeHandle.getParam(motorIdentifier + "_kff", KFF)
00479 && _nodeHandle.getParam(motorIdentifier + "_k", K)
00480 && _nodeHandle.getParam(motorIdentifier + "_max_set_point_speed", maxSetPointSpeed)
00481 && _nodeHandle.getParam(motorIdentifier + "_min_set_point_speed", minSetPointSpeed)
00482 && _nodeHandle.getParam(motorIdentifier + "_max_set_point_position", maxSetPointPos)
00483 && _nodeHandle.getParam(motorIdentifier + "_min_set_point_position", minSetPointPos)
00484 && _nodeHandle.getParam(motorIdentifier + "_limit", limit)
00485 && _nodeHandle.getParam(motorIdentifier + "_stop_limit_max", stopLimitMax)
00486 && _nodeHandle.getParam(motorIdentifier + "_stop_limit_min", stopLimitMin)
00487 && _nodeHandle.getParam(motorIdentifier + "_motor_address", motorAddress)
00488 && _nodeHandle.getParam(motorIdentifier + "_motor_emergency_pin", eSwitchPin)
00489 && _nodeHandle.getParam(motorIdentifier + "_motor_emergency_pin_type", eSwitchType)
00490 && _nodeHandle.getParam(motorIdentifier + "_joint", positionMotorJointName)
00491 && _nodeHandle.getParam(motorIdentifier + "_mode", motorMode)
00492 && _nodeHandle.getParam(motorIdentifier + "_read_pin", readPin)
00493 && _nodeHandle.getParam(motorIdentifier + "_tolerance", tolerance)
00494 && _nodeHandle.getParam(motorIdentifier + "_tolerance_time", toleranceTime)) {
00495 CloseMotorWithPotentiometerParam motorParams;
00496 motorParams.LPFHzSpeed = (uint16_t) LPFHzSpeed;
00497 motorParams.LPFHzInput = (uint16_t) LPFHzInput;
00498 motorParams.PIDHz = (uint16_t) PIDHz;
00499 motorParams.PPR = (uint16_t) PPR;
00500 motorParams.timeout = (uint16_t) timeout;
00501 motorParams.motorDirection = motorDirection;
00502 motorParams.encoderDirection = encoderDirection;
00503 motorParams.LPFAlphaSpeed = LPFAlphaSpeed;
00504 motorParams.LPFAlphaInput = LPFAlphaInput;
00505
00506 motorParams.KP = KP;
00507 motorParams.KI = KI;
00508 motorParams.KD = KD;
00509 motorParams.KFF = KFF;
00510 motorParams.K = K;
00511 motorParams.maxSetPointSpeed = maxSetPointSpeed;
00512 motorParams.minSetPointSpeed = minSetPointSpeed;
00513 motorParams.maxSetPointPos = maxSetPointPos;
00514 motorParams.minSetPointPos = minSetPointPos;
00515 motorParams.limit = limit;
00516 motorParams.stopLimitMax = stopLimitMax;
00517 motorParams.stopLimitMin = stopLimitMin;
00518 motorParams.a = a;
00519 motorParams.b = b;
00520 motorParams.pin = readPin;
00521 motorParams.tolerance = tolerance;
00522 motorParams.toleranceTime = (uint16_t) toleranceTime;
00523
00524 CloseLoopMotorWithPotentiometer *closeLoopMotor = new CloseLoopMotorWithPotentiometer(
00525 _idGen++, &_transportLayer, (byte) motorAddress, eSwitchPin, eSwitchType,
00526 CloseMotorType::CloseLoopWithPotentiometer,
00527 (CloseMotorMode::CloseMotorMode) motorMode,
00528 motorParams, positionMotorJointName);
00529 JointInfo_t *jointInfo = closeLoopMotor->getJointInfo();
00530
00531 hardware_interface::JointStateHandle jointStateHandle(positionMotorJointName,
00532 &jointInfo->position,
00533 &jointInfo->velocity,
00534 &jointInfo->effort);
00535
00536 jointStateInterface->registerHandle(jointStateHandle);
00537
00538 hardware_interface::JointHandle JointHandle(jointStateInterface->getHandle(positionMotorJointName),
00539 &jointInfo->cmd);
00540 jointPositionInterface->registerHandle(JointHandle);
00541
00542
00543 _devices.push_back(closeLoopMotor);
00544 closeLoopMotor->buildDevice();
00545
00546 }
00547 break;
00548 }
00549 }
00550 }
00551 }
00552
00553
00554 }
00555
00556 void RiCBoardManager::buildDevices(hardware_interface::JointStateInterface *jointStateInterface,
00557 hardware_interface::VelocityJointInterface *jointVelocityInterface) {
00558 int closeMotorSize = 0;
00559 bool haveCloseLoopMotors;
00560 ros::param::param<int>("close_motor_size", closeMotorSize, 0);
00561 ros::param::param<bool>("have_close_loop_motor", haveCloseLoopMotors, true);
00562 if(haveCloseLoopMotors) {
00563 for(int i = 0; i < closeMotorSize; ++i) {
00564 std::string closeMotorIdentifier = "motor" + boost::lexical_cast<std::string>(i);
00565 std::vector<std::string> jointNames;
00566 CloseMotorWithEncoderParam motorParams;
00567
00568 int encoderPinA, encoderPinB, LPFHzSpeed, LPFHzInput, PIDHz, PPR, timeout, motorDirection, encoderDirection, motorAddress
00569 , eSwitchPin, eSwitchType, motorType, motorMode, baisMin, baisMax;
00570 float LPFAlphaSpeed, LPFAlphaInput, KP, KI, KD, KFF, maxSetPointSpeed, minSetPointSpeed, maxSetPointPos, minSetPointPos, limit, stopLimitMax, stopLimitMin;
00571 if (_nodeHandle.getParam(closeMotorIdentifier + "_motor_type", motorType)) {
00572 switch ((CloseMotorType::CloseMotorType) motorType) {
00573 case CloseMotorType::CloseLoopWithEncoder:
00574 if (_nodeHandle.getParam(closeMotorIdentifier + "_encoder_pin_A", encoderPinA)
00575 && _nodeHandle.getParam(closeMotorIdentifier + "_encoder_pin_B", encoderPinB)
00576 && _nodeHandle.getParam(closeMotorIdentifier + "_lpf_hz_speed", LPFHzSpeed)
00577 && _nodeHandle.getParam(closeMotorIdentifier + "_lpf_hz_input", LPFHzInput)
00578 && _nodeHandle.getParam(closeMotorIdentifier + "_pid_hz", PIDHz)
00579 && _nodeHandle.getParam(closeMotorIdentifier + "_ppr", PPR)
00580 && _nodeHandle.getParam(closeMotorIdentifier + "_timeout", timeout)
00581 && _nodeHandle.getParam(closeMotorIdentifier + "_motor_direction", motorDirection)
00582 && _nodeHandle.getParam(closeMotorIdentifier + "_encoder_direction", encoderDirection)
00583 && _nodeHandle.getParam(closeMotorIdentifier + "_lpf_alpha_speed", LPFAlphaSpeed)
00584 && _nodeHandle.getParam(closeMotorIdentifier + "_lpf_alpha_input", LPFAlphaInput)
00585 && _nodeHandle.getParam(closeMotorIdentifier + "_kp", KP)
00586 && _nodeHandle.getParam(closeMotorIdentifier + "_ki", KI)
00587 && _nodeHandle.getParam(closeMotorIdentifier + "_kd", KD)
00588 && _nodeHandle.getParam(closeMotorIdentifier + "_kff", KFF)
00589 && _nodeHandle.getParam(closeMotorIdentifier + "_max_set_point_speed", maxSetPointSpeed)
00590 && _nodeHandle.getParam(closeMotorIdentifier + "_min_set_point_speed", minSetPointSpeed)
00591 && _nodeHandle.getParam(closeMotorIdentifier + "_max_set_point_position", maxSetPointPos)
00592 && _nodeHandle.getParam(closeMotorIdentifier + "_min_set_point_position", minSetPointPos)
00593 && _nodeHandle.getParam(closeMotorIdentifier + "_limit", limit)
00594 && _nodeHandle.getParam(closeMotorIdentifier + "_stop_limit_max", stopLimitMax)
00595 && _nodeHandle.getParam(closeMotorIdentifier + "_stop_limit_min", stopLimitMin)
00596 && _nodeHandle.getParam(closeMotorIdentifier + "_motor_address", motorAddress)
00597 && _nodeHandle.getParam(closeMotorIdentifier + "_motor_emergency_pin", eSwitchPin)
00598 && _nodeHandle.getParam(closeMotorIdentifier + "_motor_emergency_pin_type", eSwitchType)
00599 && _nodeHandle.getParam(closeMotorIdentifier + "_joint", jointNames)
00600 && _nodeHandle.getParam(closeMotorIdentifier + "_mode", motorMode)
00601 && _nodeHandle.getParam(closeMotorIdentifier + "_bais_min", baisMin)
00602 && _nodeHandle.getParam(closeMotorIdentifier + "_bais_max", baisMax)) {
00603
00604 motorParams.LPFHzSpeed = (uint16_t) LPFHzSpeed;
00605 motorParams.LPFHzInput = (uint16_t) LPFHzInput;
00606 motorParams.PIDHz = (uint16_t) PIDHz;
00607 motorParams.PPR = (uint16_t) PPR;
00608 motorParams.timeout = (uint16_t) timeout;
00609 motorParams.motorDirection = motorDirection;
00610 motorParams.encoderDirection = encoderDirection;
00611 motorParams.LPFAlphaSpeed = LPFAlphaSpeed;
00612 motorParams.LPFAlphaInput = LPFAlphaInput;
00613 motorParams.KP = KP;
00614 motorParams.KI = KI;
00615 motorParams.KD = KD;
00616 motorParams.KFF = KFF;
00617
00618 motorParams.maxSetPointSpeed = maxSetPointSpeed;
00619 motorParams.minSetPointSpeed = minSetPointSpeed;
00620 motorParams.maxSetPointPos = maxSetPointPos;
00621 motorParams.minSetPointPos = minSetPointPos;
00622 motorParams.limit = limit;
00623 motorParams.stopLimitMax = stopLimitMax;
00624 motorParams.stopLimitMin = stopLimitMin;
00625 motorParams.encoderPinA = (byte) encoderPinA;
00626 motorParams.encoderPinB = (byte) encoderPinB;
00627 motorParams.baisMin = (int16_t) baisMin;
00628 motorParams.baisMax = (int16_t) baisMax;
00629
00630 CloseLoopMotor *closeLoopMotor = new CloseLoopMotorWithEncoder(_idGen++, &_transportLayer,
00631 (byte) motorAddress,
00632 eSwitchPin, eSwitchType,
00633 CloseMotorType::CloseLoopWithEncoder,
00634 (CloseMotorMode::CloseMotorMode) motorMode,
00635 motorParams, jointNames[0]);
00636 JointInfo_t *jointInfo = closeLoopMotor->getJointInfo();
00637 for(std::vector<std::string>::iterator jointName = jointNames.begin(); jointName != jointNames.end(); ++jointName) {
00638 hardware_interface::JointStateHandle jointStateHandle((*jointName),
00639 &jointInfo->position,
00640 &jointInfo->velocity,
00641 &jointInfo->effort);
00642
00643 jointStateInterface->registerHandle(jointStateHandle);
00644
00645 hardware_interface::JointHandle JointHandle(jointStateInterface->getHandle((*jointName)),
00646 &jointInfo->cmd);
00647 jointVelocityInterface->registerHandle(JointHandle);
00648 }
00649
00650 _devices.push_back(closeLoopMotor);
00651 closeLoopMotor->buildDevice();
00652
00653 }
00654 break;
00655 }
00656 }
00657 else {
00658 ros_utils::rosError("Unable to Identify motor type");
00659 }
00660 }
00661 }
00662
00663 int openLoopSize = 0;
00664 bool haveOpenLoopMotors;
00665 ros::param::param<bool>("have_open_loop_motor", haveOpenLoopMotors, true);
00666 ros::param::param<int>("open_motor_size", openLoopSize, 0);
00667
00668 if (haveOpenLoopMotors) {
00669 for(int i = 0; i < openLoopSize; ++i) {
00670 std::string openMotorIdentifier = "motor" + boost::lexical_cast<std::string>(i), jointName;
00671 int motorAddress, eSwitchPin, eSwitchType ,encoderPinA, encoderPinB, LPFHzSpeed, LPFHzInput, PPR, motorDirection, encoderDirection;
00672 float LPFAlphaSpeed, LPFAlphaInput, maxSetPointSpeed, minSetPointSpeed;
00673 if(_nodeHandle.getParam(openMotorIdentifier + "_motor_address", motorAddress)
00674 && _nodeHandle.getParam(openMotorIdentifier + "_motor_emergency_pin", eSwitchPin)
00675 && _nodeHandle.getParam(openMotorIdentifier + "_motor_emergency_pin_type", eSwitchType)
00676 && _nodeHandle.getParam(openMotorIdentifier + "_encoder_pin_a", encoderPinA)
00677 && _nodeHandle.getParam(openMotorIdentifier + "_encoder_pin_b", encoderPinB)
00678 && _nodeHandle.getParam(openMotorIdentifier + "_lpf_hz_speed", LPFHzSpeed)
00679 && _nodeHandle.getParam(openMotorIdentifier + "_lpf_hz_input", LPFHzInput)
00680 && _nodeHandle.getParam(openMotorIdentifier + "_ppr", PPR)
00681 && _nodeHandle.getParam(openMotorIdentifier + "_motor_direction", motorDirection)
00682 && _nodeHandle.getParam(openMotorIdentifier + "_encoder_direction", encoderDirection)
00683 && _nodeHandle.getParam(openMotorIdentifier + "_lpf_alpha_speed", LPFAlphaSpeed)
00684 && _nodeHandle.getParam(openMotorIdentifier + "_lpf_alpha_input", LPFAlphaInput)
00685 && _nodeHandle.getParam(openMotorIdentifier + "_max_set_point_speed", maxSetPointSpeed)
00686 && _nodeHandle.getParam(openMotorIdentifier + "_min_set_point_speed", minSetPointSpeed)
00687 && _nodeHandle.getParam(openMotorIdentifier + "_joint", jointName)) {
00688
00689 OpenLoopMotor* openLoopMotor = new OpenLoopMotor(_idGen++, &_transportLayer, (byte) motorAddress,
00690 (byte) eSwitchPin, (byte) eSwitchType,
00691 maxSetPointSpeed, minSetPointSpeed, (byte) encoderPinA,
00692 (byte) encoderPinB, motorDirection, encoderDirection,
00693 (uint16_t) LPFHzSpeed, (uint16_t) LPFHzInput,
00694 LPFAlphaInput, LPFAlphaSpeed, (uint16_t) PPR);
00695 JointInfo_t* jointInfo = openLoopMotor->getJointInfo();
00696
00697 hardware_interface::JointStateHandle jointStateHandle(jointName,
00698 &jointInfo->position,
00699 &jointInfo->velocity,
00700 &jointInfo->effort);
00701
00702 jointStateInterface->registerHandle(jointStateHandle);
00703
00704 hardware_interface::JointHandle JointHandle(jointStateInterface->getHandle(jointName),
00705 &jointInfo->cmd);
00706 jointVelocityInterface->registerHandle(JointHandle);
00707
00708 _devices.push_back(openLoopMotor);
00709 openLoopMotor->buildDevice();
00710
00711 }
00712 }
00713 }
00714
00715 }
00716
00717 void RiCBoardManager::write() {
00718 if(ros::ok()) {
00719 size_t deviceSize = _devices.size();
00720 for(int i = 0; i < deviceSize; ++i)
00721 _devices[i]->write();
00722 }
00723
00724 }
00725
00726
00727
00728 }
00729
00730