SVHController.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 // This file is part of the SCHUNK SVH Driver suite.
00005 //
00006 // This program is free software licensed under the LGPL
00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00008 // You can find a copy of this license in LICENSE folder in the top
00009 // directory of the source code.
00010 //
00011 // © Copyright 2014 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
00012 // © Copyright 2014 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00013 //
00014 // -- END LICENSE BLOCK ------------------------------------------------
00015 
00016 //----------------------------------------------------------------------
00038 //----------------------------------------------------------------------
00039 #include "driver_svh/SVHController.h"
00040 
00041 #include <driver_svh/Logging.h>
00042 #include <icl_comm/ByteOrderConversion.h>
00043 #include <boost/bind/bind.hpp>
00044 
00045 using icl_comm::ArrayBuilder;
00046 
00047 namespace driver_svh {
00048 
00049 
00051 const char * SVHController::m_channel_description[]= {
00052   "Thumb_Flexion",
00053   "Thumb_Opposition", // wrist
00054   "Index_Finger_Distal",
00055   "Index_Finger_Proximal",
00056   "Middle_Finger_Distal",
00057   "Middle_Finger_Proximal",
00058   "Ring_Finger",
00059   "Pinky",
00060   "Finger_Spread",
00061   NULL
00062 };
00063 
00065 const float SVHController::channel_effort_constants[]={
00066   0.0000232,  //Thumb Flexion
00067   0.0000232,  // Thumb Opposition
00068   0.000009,  // Index Finger Distal
00069   0.000016,  // Index Finger Proximal
00070   0.000009,  // Middle Finger Distal
00071   0.000016,  // Middle Finger Proximal
00072   0.000009,  // Ring Finger
00073   0.000009,  // Pinky
00074   0.000016   // Finger Spread
00075 };
00076 
00077 SVHController::SVHController():
00078   m_current_settings(eSVH_DIMENSION),  // Vectors have to be filled with objects for correct deserialization
00079   m_position_settings(eSVH_DIMENSION),
00080   m_controller_feedback(eSVH_DIMENSION),
00081   m_serial_interface(new SVHSerialInterface(boost::bind(&SVHController::receivedPacketCallback,this,_1,_2))),
00082   m_enable_mask(0),
00083   m_received_package_count(0)
00084 {
00085   LOGGING_TRACE_C(DriverSVH, SVHController, "SVH Controller started"<< endl);
00086 }
00087 
00088 SVHController::~SVHController()
00089 {
00090   if (m_serial_interface != NULL)
00091   {
00092     disconnect();
00093     delete m_serial_interface;
00094     m_serial_interface = NULL;
00095   }
00096 
00097   LOGGING_DEBUG_C(DriverSVH, SVHController, "SVH Controller terminated" << endl);
00098 }
00099 
00100 bool SVHController::connect(const std::string &dev_name)
00101 {
00102   LOGGING_DEBUG_C(DriverSVH, SVHController, "Connect was called, starting the serial interface..." << endl);
00103   if (m_serial_interface != NULL)
00104   {
00105     bool success = m_serial_interface->connect(dev_name);
00106     LOGGING_DEBUG_C(DriverSVH, SVHController, "Connect finished " << ((success)?"succesfully":"with an error") << endl);
00107     return success ;
00108   }
00109   else
00110   {
00111     LOGGING_DEBUG_C(DriverSVH, SVHController, "Connect failed" << endl);
00112     return false;
00113   }
00114 }
00115 
00116 void SVHController::disconnect()
00117 {
00118   LOGGING_TRACE_C(DriverSVH, SVHController, "Disconnect called, disabling all channels and closing interface..."<< endl);
00119   if (m_serial_interface != NULL && m_serial_interface->isConnected())
00120   {
00121     // Disable all channels
00122     disableChannel(eSVH_ALL);
00123     m_serial_interface->close();
00124   }
00125   LOGGING_TRACE_C(DriverSVH, SVHController, "Disconnect finished"<< endl);
00126 }
00127 
00128 void SVHController::setControllerTarget(const SVHChannel& channel, const int32_t& position)
00129 {
00130   // No Sanity Checks for out of bounds positions at this point as the finger manager has already handled these
00131   if ((channel != eSVH_ALL) && (channel >=0 && channel < eSVH_DIMENSION))
00132   {
00133     // The channel is encoded in the index byte
00134     SVHSerialPacket serial_packet(0,SVH_SET_CONTROL_COMMAND|static_cast<uint8_t>(channel << 4));
00135     SVHControlCommand control_command(position);
00136     // Note the 40 byte ArrayBuilder initialization -> this is needed to get a zero padding in the serialpacket. Otherwise it would be shorter
00137     ArrayBuilder ab(40);
00138     ab << control_command;
00139     serial_packet.data = ab.array;
00140     m_serial_interface ->sendPacket(serial_packet);
00141 
00142     // Debug Disabled as it is way to noisy
00143     //LOGGING_TRACE_C(DriverSVH, SVHController, "Control command was given for channel: "<< channel << "Driving motor to position: "<< position << endl);
00144   }
00145   else
00146   {
00147     LOGGING_WARNING_C(DriverSVH, SVHController, "Control command was given for unknown (or all) channel: "<< channel << "- ignoring request"<< endl);
00148   }
00149 }
00150 
00151 void SVHController::setControllerTargetAllChannels(const std::vector<int32_t> &positions)
00152 {
00153   if (positions.size() >= eSVH_DIMENSION)
00154   {
00155    SVHSerialPacket serial_packet(0,SVH_SET_CONTROL_COMMAND_ALL);
00156    SVHControlCommandAllChannels control_command(positions);
00157    ArrayBuilder ab(40);
00158    ab << control_command;
00159    serial_packet.data = ab.array;
00160    m_serial_interface ->sendPacket(serial_packet);
00161 
00162    // Debug Disabled as it is way to noisy
00163    //LOGGING_TRACE_C(DriverSVH, SVHController, "Control command was given for all channels: Driving motors to positions: "<< positions[0] << " , " << positions[1] << " , " << positions[2] << " , " << positions[3] << " , " << positions[4] << " , " << positions[5] << " , " << positions[6] << " , " << positions[7] << " , " << positions[8] << " , " << endl);
00164   }
00165   // We could theoretically allow fewer channels but this leaves some questions. Are the given channels in right order?
00166   // was it realy intented to just give fewer positions? What to do witht the ones that did not get anything?
00167   // Just disallowing it seems to be the most understandable decission.
00168   else
00169   {
00170     LOGGING_WARNING_C(DriverSVH, SVHController, "Control command was given for all channels but with to few points. Expected at least "<< eSVH_DIMENSION << " values but only got " << positions.size() << "use the individual setTarget function for this" << endl);
00171   }
00172 }
00173 
00174 void SVHController::enableChannel(const SVHChannel &channel)
00175 {
00176   SVHSerialPacket serial_packet(0,SVH_SET_CONTROLLER_STATE);
00177   SVHControllerState controller_state;
00178   ArrayBuilder ab(40);
00179 
00180   LOGGING_TRACE_C(DriverSVH, SVHController, "Enable of channel " << channel << " requested."<< endl);
00181 
00182   // In case no channel was enabled we need to enable the 12V dc drivers first
00183   if (m_enable_mask == 0)
00184   {
00185     LOGGING_TRACE_C(DriverSVH, SVHController, "Enable was called and no channel was previously activated, commands are sent individually......" << endl);
00186     LOGGING_TRACE_C(DriverSVH, SVHController, "Sending pwm_fault and pwm_otw...(0x001F) to reset software warnings" << endl);
00187     // Reset faults and overtemperature warnings saved in the controller
00188     controller_state.pwm_fault = 0x001F;
00189     controller_state.pwm_otw   = 0x001F;
00190     ab << controller_state;
00191     serial_packet.data = ab.array;
00192     m_serial_interface ->sendPacket(serial_packet);
00193     ab.reset(40);
00194 
00195     // Small delays seem to make communication at this point more reliable although they SHOULD NOT be necessary
00196     icl_core::os::usleep(2000);
00197 
00198     LOGGING_TRACE_C(DriverSVH, SVHController, "Enabling 12V Driver (pwm_reset and pwm_active = =0x0200)..." << endl);
00199     // enable +12v supply driver
00200     controller_state.pwm_reset = 0x0200;
00201     controller_state.pwm_active = 0x0200;
00202     ab << controller_state;
00203     serial_packet.data = ab.array;
00204     m_serial_interface ->sendPacket(serial_packet);
00205     ab.reset(40);
00206 
00207     icl_core::os::usleep(2000);
00208 
00209      LOGGING_TRACE_C(DriverSVH, SVHController, "Enabling pos_ctrl and cur_ctrl..." << endl);
00210     // enable controller
00211     controller_state.pos_ctrl = 0x0001;
00212     controller_state.cur_ctrl = 0x0001;
00213     ab << controller_state;
00214     serial_packet.data = ab.array;
00215     m_serial_interface ->sendPacket(serial_packet);
00216     ab.reset(40);
00217 
00218      icl_core::os::usleep(2000);
00219 
00220     LOGGING_TRACE_C(DriverSVH, SVHController, "...Done" << endl);
00221   }
00222 
00223   // enable actual channels (again we only accept individual channels for safety)
00224   if (channel >=0 && channel < eSVH_DIMENSION)
00225   {
00226     LOGGING_TRACE_C(DriverSVH, SVHController, "Enabling motor: "<< channel << endl);
00227     // oring all channels to create the activation mask-> high = channel active
00228     m_enable_mask |= (1<<channel);
00229 
00230     // SENDING EVERYTHING AT ONCE WILL LEAD TO "Jumping" behaviour of the controller on faster Systems ---> this has to
00231     // do with the initialization of the hardware controllers. If we split it in two calls we will reset them first and then activate
00232     // making sure that all values are initialized properly effectively preventing any jumping behaviour
00233     ab.reset(40);
00234     controller_state.pwm_fault = 0x001F;
00235     controller_state.pwm_otw   = 0x001F;
00236     controller_state.pwm_reset = (0x0200 | (m_enable_mask & 0x01FF));
00237     controller_state.pwm_active =(0x0200 | (m_enable_mask & 0x01FF));
00238     ab << controller_state;
00239     serial_packet.data = ab.array;
00240     m_serial_interface ->sendPacket(serial_packet);
00241     ab.reset(40);
00242 
00243     // WARNING: DO NOT ! REMOVE THESE DELAYS OR THE HARDWARE WILL! FREAK OUT! (see reason above)
00244     icl_core::os::usleep(500);
00245 
00246     controller_state.pos_ctrl = 0x0001;
00247     controller_state.cur_ctrl = 0x0001;
00248     ab << controller_state;
00249     serial_packet.data = ab.array;
00250     m_serial_interface ->sendPacket(serial_packet);
00251     ab.reset(40);
00252 
00253     LOGGING_DEBUG_C(DriverSVH, SVHController, "Enabled channel: " << channel << endl);
00254   }
00255   else
00256   {
00257     LOGGING_ERROR_C(DriverSVH, SVHController, "Activation request for ALL or unknown channel: "<< channel << "- ignoring request"<< endl);
00258   }
00259 
00260 }
00261 
00262 void SVHController::disableChannel(const SVHChannel& channel)
00263 {
00264   LOGGING_TRACE_C(DriverSVH, SVHController, "Disable of channel " << channel << " requested."<< endl);
00265 
00266   if (m_serial_interface != NULL && m_serial_interface->isConnected())
00267   {
00268     // prepare general packet
00269     SVHSerialPacket serial_packet(0,SVH_SET_CONTROLLER_STATE);
00270     SVHControllerState controller_state;
00271     ArrayBuilder ab(40);
00272 
00273     // we just accept it at this point because it makes no difference in the calls
00274     if (channel == eSVH_ALL)
00275     {
00276       m_enable_mask = 0;
00277       controller_state.pwm_fault = 0x001F;
00278       controller_state.pwm_otw   = 0x001F;
00279 
00280       // default initialization to zero -> controllers are deactivated
00281       ab << controller_state;
00282       serial_packet.data = ab.array;
00283       m_serial_interface->sendPacket(serial_packet);
00284 
00285       LOGGING_DEBUG_C(DriverSVH, SVHController, "Disabled all channels"<< endl);
00286     }
00287     else if (channel >=0 && channel < eSVH_DIMENSION)
00288     {
00289       controller_state.pwm_fault = 0x001F;
00290       controller_state.pwm_otw   = 0x001F;
00291       //Disable the finger in the bitmask
00292       m_enable_mask &= ~(1<<channel);
00293 
00294       if (m_enable_mask != 0) // pos and current control stay on then
00295       {
00296         controller_state.pwm_reset  = (0x0200 | (m_enable_mask & 0x01FF));
00297         controller_state.pwm_active = (0x0200 | (m_enable_mask & 0x01FF));
00298         controller_state.pos_ctrl   = 0x0001;
00299         controller_state.cur_ctrl   = 0x0001;
00300       }
00301 
00302       ab << controller_state;
00303       serial_packet.data = ab.array;
00304       m_serial_interface->sendPacket(serial_packet);
00305 
00306       LOGGING_DEBUG_C(DriverSVH, SVHController, "Disabled channel: " << channel << endl);
00307     }
00308     else
00309     {
00310       LOGGING_WARNING_C(DriverSVH, SVHController, "Disable was requestet for unknown channel: "<< channel << "- ignoring request" << endl);
00311     }
00312   }
00313   else
00314   {
00315     LOGGING_ERROR_C(DriverSVH, SVHController, "Disabling Channel not possible. Serial interface is not connected!" << endl);
00316   }
00317 }
00318 
00319 void SVHController::requestControllerState()
00320 {
00321   LOGGING_TRACE_C(DriverSVH, SVHController, "Requesting ControllerStatefrom Hardware"<< endl);
00322   SVHSerialPacket serial_packet(40,SVH_GET_CONTROLLER_STATE);
00323   m_serial_interface ->sendPacket(serial_packet);
00324 }
00325 
00326 void SVHController::requestControllerFeedback(const SVHChannel& channel)
00327 {
00328   if ((channel != eSVH_ALL) && (channel >=0 && channel < eSVH_DIMENSION))
00329   {
00330     SVHSerialPacket serial_packet(40,SVH_GET_CONTROL_FEEDBACK|static_cast<uint8_t>(channel << 4));
00331     m_serial_interface ->sendPacket(serial_packet);
00332 
00333     // Disabled as it spams the output to much
00334     //LOGGING_TRACE_C(DriverSVH, SVHController, "Controller feedback was requested for channel: "<< channel << endl);
00335 
00336   }
00337   else if (channel == eSVH_ALL)
00338   {
00339     SVHSerialPacket serial_packet(40,SVH_GET_CONTROL_FEEDBACK_ALL);
00340     m_serial_interface ->sendPacket(serial_packet);
00341 
00342     // Disabled as it spams the output to much
00343     //LOGGING_TRACE_C(DriverSVH, SVHController, "Controller feedback was requested for all channels " << endl);
00344   }
00345   else
00346   {
00347     LOGGING_WARNING_C(DriverSVH, SVHController, "Controller feedback was requestet for unknown channel: "<< channel << "- ignoring request"<< endl);
00348   }
00349 }
00350 
00351 void SVHController::requestPositionSettings(const SVHChannel& channel)
00352 {
00353   LOGGING_TRACE_C(DriverSVH, SVHController, "Requesting PositionSettings from Hardware for channel: " << channel << endl);
00354   SVHSerialPacket serial_packet((SVH_GET_POSITION_SETTINGS| static_cast<uint8_t>(channel << 4)),40);
00355   m_serial_interface ->sendPacket(serial_packet);
00356 }
00357 
00358 void SVHController::setPositionSettings(const SVHChannel& channel,const SVHPositionSettings& position_settings)
00359 {
00360   if ((channel != eSVH_ALL) && (channel >=0 && channel < eSVH_DIMENSION))
00361   {
00362     SVHSerialPacket serial_packet(0,SVH_SET_POSITION_SETTINGS|static_cast<uint8_t>(channel << 4));
00363     ArrayBuilder ab;
00364     ab << position_settings;
00365     serial_packet.data = ab.array;
00366     m_serial_interface ->sendPacket(serial_packet);
00367 
00368     // Save already in case we dont get immediate response
00369     m_position_settings[channel] = position_settings;
00370 
00371     LOGGING_DEBUG_C(DriverSVH, SVHController, "Position controller settings where send to change channel: "<< channel << " : ");
00372     LOGGING_DEBUG_C(DriverSVH, SVHController,  "wmn " << position_settings.wmn << " " << "wmx " << position_settings.wmx << " " << "dwmx "<< position_settings.dwmx << " "
00373                                             << "ky "  << position_settings.ky  << " " << "dt "  << position_settings.dt  << " " << "imn " << position_settings.imn << " "
00374                                             << "imx " << position_settings.imx << " " << "kp "  << position_settings.kp  << " " << "ki "  << position_settings.ki  << " "
00375                                             << "kd "  << position_settings.kd << " " << endl );
00376 
00377   }
00378   else
00379   {
00380     LOGGING_WARNING_C(DriverSVH, SVHController, "Position controller settings where given for unknown channel: "<< channel << "- ignoring request"<< endl);
00381   }
00382 }
00383 
00384 void SVHController::requestCurrentSettings(const SVHChannel& channel)
00385 {
00386   LOGGING_TRACE_C(DriverSVH, SVHController, "Requesting CurrentSettings for channel: " << channel << endl);
00387 
00388   if ((channel != eSVH_ALL) && (channel >=0 && channel < eSVH_DIMENSION))
00389   {
00390     SVHSerialPacket serial_packet(40,(SVH_GET_CURRENT_SETTINGS|static_cast<uint8_t>(channel << 4)));
00391     m_serial_interface ->sendPacket(serial_packet);
00392   }
00393   else
00394   {
00395     LOGGING_WARNING_C(DriverSVH, SVHController, "Get Current Settings can only be requested with a specific channel, ALL or unknown channel:" << channel << "was selected " << endl);
00396   }
00397 }
00398 
00399 void SVHController::setCurrentSettings(const SVHChannel& channel,const SVHCurrentSettings& current_settings)
00400 {
00401   if ((channel != eSVH_ALL) && (channel >=0 && channel < eSVH_DIMENSION))
00402   {
00403     SVHSerialPacket serial_packet(0,SVH_SET_CURRENT_SETTINGS|static_cast<uint8_t>(channel << 4));
00404     ArrayBuilder ab;
00405     ab << current_settings;
00406     serial_packet.data = ab.array;
00407     m_serial_interface ->sendPacket(serial_packet);
00408 
00409     // Save already in case we dont get immediate response
00410     m_current_settings[channel] = current_settings;
00411 
00412     LOGGING_DEBUG_C(DriverSVH, SVHController, "Current controller settings where send to change channel: "<< channel << " : ");
00413     LOGGING_DEBUG_C(DriverSVH, SVHController, "wmn "<< current_settings.wmn << " " << "wmx "<< current_settings.wmx << " " << "ky " << current_settings.ky  << " "
00414                                            << "dt " << current_settings.dt  << " " << "imn "<< current_settings.imn << " " << "imx "<< current_settings.imx << " "
00415                                            << "kp " << current_settings.kp  << " " << "ki " << current_settings.ki  << " " << "umn "<< current_settings.umn << " "
00416                                            << "umx "<< current_settings.umx << endl);
00417 
00418   }
00419   else
00420   {
00421     LOGGING_WARNING_C(DriverSVH, SVHController, "Current controller settings where given for unknown channel: "<< channel << "- ignoring request"<< endl);
00422   }
00423 }
00424 
00425 void SVHController::requestEncoderValues()
00426 {
00427   LOGGING_TRACE_C(DriverSVH, SVHController, "Requesting EncoderValues from hardware"<< endl);
00428   SVHSerialPacket serial_packet(40,SVH_GET_ENCODER_VALUES);
00429   m_serial_interface ->sendPacket(serial_packet);
00430 }
00431 
00432 void SVHController::setEncoderValues(const SVHEncoderSettings &encoder_settings)
00433 {
00434   LOGGING_TRACE_C(DriverSVH, SVHController, "Setting new Encoder values : ");
00435   for (size_t i = 0; i < encoder_settings.scalings.size(); i++)
00436   {
00437     // log stream unfortunately does not support the standard to stream operators yet
00438     LOGGING_TRACE_C(DriverSVH, SVHController, "[" << (int)i << "] " << ": " <<encoder_settings.scalings[i] << " " );
00439   }
00440   LOGGING_TRACE_C(DriverSVH, SVHController, endl);
00441 
00442   SVHSerialPacket serial_packet(0,SVH_SET_ENCODER_VALUES);
00443   ArrayBuilder ab;
00444   ab << encoder_settings;
00445   serial_packet.data = ab.array;
00446   m_serial_interface ->sendPacket(serial_packet);
00447 
00448   // Save already in case we dont get imediate response
00449   m_encoder_settings = encoder_settings;
00450 }
00451 
00452 void SVHController::requestFirmwareInfo()
00453 {
00454   LOGGING_TRACE_C(DriverSVH, SVHController, "Requesting firmware Information from hardware" << endl);
00455 
00456   SVHSerialPacket serial_packet(40,SVH_GET_FIRMWARE_INFO);
00457   m_serial_interface->sendPacket(serial_packet);
00458 }
00459 
00460 void SVHController::receivedPacketCallback(const SVHSerialPacket& packet, unsigned int packet_count)
00461 {
00462   // Extract Channel
00463   uint8_t channel = (packet.address >> 4 ) & 0x0F;
00464   // Prepare Data for conversion
00465   ArrayBuilder ab;
00466   ab.appendWithoutConversion(packet.data);
00467 
00468   SVHControllerFeedbackAllChannels feedback_all;
00469 
00470   m_received_package_count = packet_count;
00471 
00472   // Packet meaning is encoded in the lower nibble of the adress byte
00473   switch (packet.address & 0x0F)
00474   {
00475     case SVH_GET_CONTROL_FEEDBACK:
00476     case SVH_SET_CONTROL_COMMAND:
00477       if (channel >=0 && channel < eSVH_DIMENSION)
00478       {
00479         //std::cout << "Recieved: Controllerfeedback RAW Data: " << ab;
00480         ab >> m_controller_feedback[channel];
00481         // Disabled as this is spamming the output to much
00482         //LOGGING_TRACE_C(DriverSVH, SVHController, "Received a Control Feedback/Control Command packet for channel "<< channel << " Position: "<< (int)m_controller_feedback[channel].position  << " Current: "<< (int)m_controller_feedback[channel].current << endl);
00483       }
00484       else
00485       {
00486         LOGGING_ERROR_C(DriverSVH, SVHController, "Received a Control Feedback/Control Command packet for ILLEGAL channel "<< channel << "- packet ignored!" << endl);
00487       }
00488       break;
00489     case SVH_GET_CONTROL_FEEDBACK_ALL:
00490     case SVH_SET_CONTROL_COMMAND_ALL:
00491       // We cannot just read them all into the vector (which would have been nice) because the feedback of all channels is structured
00492       // different from the feedback of one channel. So the SVHControllerFeedbackAllChannels is used as an intermediary ( handles the deserialization)
00493       ab >> feedback_all;
00494       m_controller_feedback = feedback_all.feedbacks;
00495       // Disabled as this is spannimg the output to much
00496       //LOGGING_TRACE_C(DriverSVH, SVHController, "Received a Control Feedback/Control Command packet for channel all channels "<<  endl);
00497       break;
00498     case SVH_GET_POSITION_SETTINGS:
00499     case SVH_SET_POSITION_SETTINGS:
00500       if (channel >=0 && channel < eSVH_DIMENSION)
00501       {
00502         //std::cout << "Recieved: Postitionsettings RAW Data: " << ab; // for really intensive debugging
00503         ab >> m_position_settings[channel];
00504         LOGGING_TRACE_C(DriverSVH, SVHController, "Received a get/set position setting packet for channel "<< channel  << endl);
00505         LOGGING_TRACE_C(DriverSVH, SVHController, "wmn " << m_position_settings[channel].wmn << " "<< "wmx " << m_position_settings[channel].wmx << " "<< "dwmx "<< m_position_settings[channel].dwmx << " "<< "ky "  << m_position_settings[channel].ky  << " "<< "dt "  << m_position_settings[channel].dt  << " "<< "imn " << m_position_settings[channel].imn << " "<< "imx " << m_position_settings[channel].imx << " " << "kp "  << m_position_settings[channel].kp  << " " << "ki "  << m_position_settings[channel].ki  << " " << "kd "  << m_position_settings[channel].kd  << endl);
00506 
00507       }
00508       else
00509       {
00510         LOGGING_ERROR_C(DriverSVH, SVHController, "Received a get/set position setting packet for ILLEGAL channel "<< channel << "- packet ignored!" << endl);
00511       }
00512       break;
00513     case SVH_GET_CURRENT_SETTINGS:
00514     case SVH_SET_CURRENT_SETTINGS:
00515       if (channel >=0 && channel < eSVH_DIMENSION)
00516       {
00517         //std::cout << "Recieved: Current Settings RAW Data: " << ab; // for really intensive debugging
00518         ab >> m_current_settings[channel];
00519         LOGGING_TRACE_C(DriverSVH, SVHController, "Received a get/set current setting packet for channel "<< channel << endl);
00520         LOGGING_TRACE_C(DriverSVH, SVHController, "wmn "<< m_current_settings[channel].wmn << " " << "wmx "<< m_current_settings[channel].wmx << " " << "ky " << m_current_settings[channel].ky  << " " << "dt " << m_current_settings[channel].dt  << " " << "imn "<< m_current_settings[channel].imn << " " << "imx "<< m_current_settings[channel].imx << " "                   << "kp " << m_current_settings[channel].kp  << " " << "ki " << m_current_settings[channel].ki  << " " << "umn "<< m_current_settings[channel].umn << " " << "umx "<< m_current_settings[channel].umx << " "<< endl);
00521       }
00522       else
00523       {
00524         LOGGING_ERROR_C(DriverSVH, SVHController, "Received a get/set current setting packet for ILLEGAL channel "<< channel << "- packet ignored!" << endl);
00525       }
00526       break;
00527     case SVH_GET_CONTROLLER_STATE:
00528     case SVH_SET_CONTROLLER_STATE:
00529         //std::cout << "Recieved: Controller State RAW Data: " << ab; // for really intensive debugging
00530         ab >> m_controller_state;
00531         //std::cout << "Received controllerState interpreded data: "<< m_controller_state << std::endl; // for really intensive debugging
00532         LOGGING_TRACE_C(DriverSVH, SVHController, "Received a get/set controler state packet " << endl);
00533         LOGGING_TRACE_C(DriverSVH, SVHController, "Controllerstate (NO HEX):" << "pwm_fault " << "0x" << static_cast<int>(m_controller_state.pwm_fault) << " " << "pwm_otw " << "0x" << static_cast<int>(m_controller_state.pwm_otw) << " "  << "pwm_reset " << "0x" << static_cast<int>(m_controller_state.pwm_reset) << " " << "pwm_active " << "0x" << static_cast<int>(m_controller_state.pwm_active) << " " << "pos_ctr " << "0x" <<  static_cast<int>(m_controller_state.pos_ctrl) << " " << "cur_ctrl " << "0x" << static_cast<int>(m_controller_state.cur_ctrl) << endl);
00534       break;
00535     case SVH_GET_ENCODER_VALUES:
00536     case SVH_SET_ENCODER_VALUES:
00537         LOGGING_TRACE_C(DriverSVH, SVHController, "Received a get/set encoder settings packet " << endl);
00538         ab >> m_encoder_settings;
00539       break;
00540     case SVH_GET_FIRMWARE_INFO:
00541         //std::cout << "Recieved: Firmware Settings RAW Data: " << ab; // for really intensive debugging
00542         ab >> m_firmware_info;
00543         LOGGING_INFO(DriverSVH, "Hardware is using the following Firmware: " );
00544         LOGGING_INFO(DriverSVH, m_firmware_info.svh  << " Version: " << m_firmware_info.version_major << "." << m_firmware_info.version_minor << " : " << m_firmware_info.text << endl);
00545       break;
00546     default:
00547         LOGGING_ERROR_C(DriverSVH, SVHController, "Received a Packet with unknown address: "<< (packet.address & 0x0F) << " - ignoring packet" << endl);
00548       break;
00549   }
00550 
00551 }
00552 
00553 bool SVHController::getControllerFeedback(const SVHChannel &channel,SVHControllerFeedback& controller_feedback)
00554 {
00555   if(channel >= 0 && static_cast<uint8_t>(channel) < m_controller_feedback.size())
00556   {
00557     controller_feedback = m_controller_feedback[channel];
00558     return true;
00559   }
00560   else
00561   {
00562     LOGGING_WARNING_C(DriverSVH, SVHController, "GetFeedback was requested for unknown channel: "<< channel<< "- ignoring request" << endl);
00563     return false;
00564   }
00565 }
00566 
00567 void SVHController::getControllerFeedbackAllChannels(SVHControllerFeedbackAllChannels& controller_feedback)
00568 {
00569   controller_feedback.feedbacks = m_controller_feedback;
00570 }
00571 
00572 bool SVHController::getPositionSettings(const SVHChannel &channel, SVHPositionSettings &position_settings)
00573 {
00574   if(channel >= 0 && static_cast<uint8_t>(channel) < m_position_settings.size())
00575   {
00576     position_settings = m_position_settings[channel];
00577     return true;
00578   }
00579   else
00580   {
00581     LOGGING_WARNING_C(DriverSVH, SVHController, "GetPositionSettings was requested for unknown channel: "<< channel<< "- ignoring request" << endl);
00582     return false;
00583   }
00584 }
00585 
00586 bool SVHController::getCurrentSettings(const SVHChannel &channel, SVHCurrentSettings &current_settings)
00587 {
00588   if(channel >= 0 && static_cast<uint8_t>(channel) < m_current_settings.size())
00589   {
00590     current_settings = m_current_settings[channel];
00591     return true;
00592   }
00593   else
00594   {
00595     LOGGING_WARNING_C(DriverSVH, SVHController, "GetCurrentSettings was requested for unknown channel: "<< channel<< "- ignoring request" << endl);
00596     return false;
00597   }
00598 }
00599 
00600 SVHFirmwareInfo SVHController::getFirmwareInfo()
00601 {
00602   return m_firmware_info;
00603 }
00604 
00605 void SVHController::resetPackageCounts()
00606 {
00607   m_received_package_count = 0;
00608   // The serial interface also keeps track about these counts
00609   m_serial_interface->resetTransmitPackageCount();
00610   LOGGING_TRACE_C(DriverSVH, SVHController, "Received package count resetted" << endl);
00611 }
00612 
00613 unsigned int SVHController::getSentPackageCount()
00614 {
00615   if (m_serial_interface != NULL)
00616   {
00617     return m_serial_interface->transmittedPacketCount();
00618   }
00619   else
00620   {
00621     LOGGING_WARNING_C(DriverSVH, SVHController, "Request for transmit packet count could not be answered as the device is not connected - ignoring request" << endl);
00622     return 0;
00623   }
00624 }
00625 
00626 unsigned int SVHController::getReceivedPackageCount()
00627 {
00628   return m_received_package_count;
00629 }
00630 
00631 bool SVHController::isEnabled(const SVHChannel &channel)
00632 {
00633   return ((1 << channel & m_enable_mask) > 0);
00634 }
00635 
00636 
00637 
00638 
00639 }


schunk_svh_driver
Author(s): Georg Heppner
autogenerated on Fri Aug 28 2015 12:59:19