JAUS++: jaus++/2.0/src/jaus/mobility/drivers/reportwrencheffort.cpp Source File

Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 
00030 
00031 
00032 
00033 
00034 
00035 
00036 
00037 
00038 
00039 
00040 #include "jaus/mobility/drivers/reportwrencheffort.h"
00041 #include "jaus/core/scaledinteger.h"
00042 
00043 using namespace JAUS;
00044 
00045 
00054 ReportWrenchEffort::ReportWrenchEffort(const Address& dest, const Address& src) : Message(REPORT_WRENCH_EFFORT, dest, src)
00055 {
00056     mPresenceVector = 0;
00057     mPropulsiveLinearEffortX = 0;             
00058     mPropulsiveLinearEffortY = 0;             
00059     mPropulsiveLinearEffortZ = 0;             
00060     mPropulsiveRotationalEffortX = 0;             
00061     mPropulsiveRotationalEffortY = 0;             
00062     mPropulsiveRotationalEffortZ = 0;             
00063     mResistiveLinearEffortX = 0;             
00064     mResistiveLinearEffortY = 0;             
00065     mResistiveLinearEffortZ = 0;             
00066     mResistiveRotationalEffortX = 0;             
00067     mResistiveRotationalEffortY = 0;             
00068     mResistiveRotationalEffortZ = 0;   
00069 }
00070 
00071 
00077 ReportWrenchEffort::ReportWrenchEffort(const ReportWrenchEffort& message) : Message(REPORT_WRENCH_EFFORT)
00078 {
00079     mPresenceVector = 0;
00080     mPropulsiveLinearEffortX = 0;             
00081     mPropulsiveLinearEffortY = 0;             
00082     mPropulsiveLinearEffortZ = 0;             
00083     mPropulsiveRotationalEffortX = 0;             
00084     mPropulsiveRotationalEffortY = 0;             
00085     mPropulsiveRotationalEffortZ = 0;             
00086     mResistiveLinearEffortX = 0;             
00087     mResistiveLinearEffortY = 0;             
00088     mResistiveLinearEffortZ = 0;             
00089     mResistiveRotationalEffortX = 0;             
00090     mResistiveRotationalEffortY = 0;             
00091     mResistiveRotationalEffortZ = 0;   
00092     *this = message;
00093 }
00094 
00095 
00101 ReportWrenchEffort::~ReportWrenchEffort()
00102 {
00103 }
00104 
00105 
00116 bool ReportWrenchEffort::SetPropulsiveLinearEffortX(const double percent)
00117 {
00118     if(percent >= -100.0 && percent <= 100.0)
00119     {
00120         mPropulsiveLinearEffortX = percent;
00121         mPresenceVector |= PresenceVector::PropulsiveLinearEffortX;
00122         return true;
00123     }
00124     return false;
00125 }
00126 
00127 
00138 bool ReportWrenchEffort::SetPropulsiveLinearEffortY(const double percent)
00139 {
00140     if(percent >= -100.0 && percent <= 100.0)
00141     {
00142         mPropulsiveLinearEffortY = percent;
00143         mPresenceVector |= PresenceVector::PropulsiveLinearEffortY;
00144         return true;
00145     }
00146     return false;
00147 }
00148 
00149 
00160 bool ReportWrenchEffort::SetPropulsiveLinearEffortZ(const double percent)
00161 {
00162     if(percent >= -100.0 && percent <= 100.0)
00163     {
00164         mPropulsiveLinearEffortZ = percent;
00165         mPresenceVector |= PresenceVector::PropulsiveLinearEffortZ;
00166         return true;
00167     }
00168     return false;
00169 }
00170 
00171 
00182 bool ReportWrenchEffort::SetPropulsiveRotationalEffortX(const double percent)
00183 {
00184     if(percent >= -100.0 && percent <= 100.0)
00185     {
00186         mPropulsiveRotationalEffortX = percent;
00187         mPresenceVector |= PresenceVector::PropulsiveRotationalEffortX;
00188         return true;
00189     }
00190     return false;
00191 }
00192 
00193 
00204 bool ReportWrenchEffort::SetPropulsiveRotationalEffortY(const double percent)
00205 {
00206     if(percent >= -100.0 && percent <= 100.0)
00207     {
00208         mPropulsiveRotationalEffortY = percent;
00209         mPresenceVector |= PresenceVector::PropulsiveRotationalEffortY;
00210         return true;
00211     }
00212     return false;
00213 }
00214 
00215 
00226 bool ReportWrenchEffort::SetPropulsiveRotationalEffortZ(const double percent)
00227 {
00228     if(percent >= -100.0 && percent <= 100.0)
00229     {
00230         mPropulsiveRotationalEffortZ = percent;
00231         mPresenceVector |= PresenceVector::PropulsiveRotationalEffortZ;
00232         return true;
00233     }
00234     return false;
00235 }
00236 
00237 
00248 bool ReportWrenchEffort::SetResistiveLinearEffortX(const double percent)
00249 {
00250     if(percent >= 0 && percent <= 100.0)
00251     {
00252         mResistiveLinearEffortX = percent;
00253         mPresenceVector |= PresenceVector::ResistiveLinearEffortX;
00254         return true;
00255     }
00256     return false;
00257 }
00258 
00259 
00270 bool ReportWrenchEffort::SetResistiveLinearEffortY(const double percent)
00271 {
00272     if(percent >= 0 && percent <= 100.0)
00273     {
00274         mResistiveLinearEffortY = percent;
00275         mPresenceVector |= PresenceVector::ResistiveLinearEffortY;
00276         return true;
00277     }
00278     return false;
00279 }
00280 
00281 
00292 bool ReportWrenchEffort::SetResistiveLinearEffortZ(const double percent)
00293 {
00294     if(percent >= 0 && percent <= 100.0)
00295     {
00296         mResistiveLinearEffortZ = percent;
00297         mPresenceVector |= PresenceVector::ResistiveLinearEffortZ;
00298         return true;
00299     }
00300     return false;
00301 }
00302 
00303 
00314 bool ReportWrenchEffort::SetResistiveRotationalEffortX(const double percent)
00315 {
00316     if(percent >= 0 && percent <= 100.0)
00317     {
00318         mResistiveRotationalEffortX = percent;
00319         mPresenceVector |= PresenceVector::ResistiveRotationalEffortX;
00320         return true;
00321     }
00322     return false;
00323 }
00324 
00325 
00336 bool ReportWrenchEffort::SetResistiveRotationalEffortY(const double percent)
00337 {
00338     if(percent >= 0 && percent <= 100.0)
00339     {
00340         mResistiveRotationalEffortY = percent;
00341         mPresenceVector |= PresenceVector::ResistiveRotationalEffortY;
00342         return true;
00343     }
00344     return false;
00345 }
00346 
00347 
00358 bool ReportWrenchEffort::SetResistiveRotationalEffortZ(const double percent)
00359 {
00360     if(percent >= 0 && percent <= 100.0)
00361     {
00362         mResistiveRotationalEffortZ = percent;
00363         mPresenceVector |= PresenceVector::ResistiveRotationalEffortZ;
00364         return true;
00365     }
00366     return false;
00367 }
00368 
00369 
00370 
00382 int ReportWrenchEffort::WriteMessageBody(Packet& packet) const
00383 {
00384     int expected = USHORT_SIZE;
00385     int written = 0;
00386 
00387     written += packet.Write(mPresenceVector);
00388 
00389     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortX) > 0)
00390     {
00391         expected += USHORT_SIZE;
00392         written += ScaledInteger::Write(packet, mPropulsiveLinearEffortX, 100.0, -100.0, ScaledInteger::UShort);
00393     }
00394     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortY) > 0)
00395     {
00396         expected += USHORT_SIZE;
00397         written += ScaledInteger::Write(packet, mPropulsiveLinearEffortY, 100.0, -100.0, ScaledInteger::UShort);
00398     }
00399     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortZ) > 0)
00400     {
00401         expected += USHORT_SIZE;
00402         written += ScaledInteger::Write(packet, mPropulsiveLinearEffortZ, 100.0, -100.0, ScaledInteger::UShort);
00403     }
00404     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortX) > 0)
00405     {
00406         expected += USHORT_SIZE;
00407         written += ScaledInteger::Write(packet, mPropulsiveRotationalEffortX, 100.0, -100.0, ScaledInteger::UShort);
00408     }
00409     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortY) > 0)
00410     {
00411         expected += USHORT_SIZE;
00412         written += ScaledInteger::Write(packet, mPropulsiveRotationalEffortY, 100.0, -100.0, ScaledInteger::UShort);
00413     }
00414     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortZ) > 0)
00415     {
00416         expected += USHORT_SIZE;
00417         written += ScaledInteger::Write(packet, mPropulsiveRotationalEffortZ, 100.0, -100.0, ScaledInteger::UShort);
00418     }
00419     if((mPresenceVector & PresenceVector::ResistiveLinearEffortX) > 0)
00420     {
00421         expected += BYTE_SIZE;
00422         written += ScaledInteger::Write(packet, mResistiveLinearEffortX, 100.0, 0.0, ScaledInteger::Byte);
00423     }
00424     if((mPresenceVector & PresenceVector::ResistiveLinearEffortY) > 0)
00425     {
00426         expected += BYTE_SIZE;
00427         written += ScaledInteger::Write(packet, mResistiveLinearEffortY, 100.0, 0.0, ScaledInteger::Byte);
00428     }
00429     if((mPresenceVector & PresenceVector::ResistiveLinearEffortZ) > 0)
00430     {
00431         expected += BYTE_SIZE;
00432         written += ScaledInteger::Write(packet, mResistiveLinearEffortZ, 100.0, 0.0, ScaledInteger::Byte);
00433     }
00434     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortX) > 0)
00435     {
00436         expected += BYTE_SIZE;
00437         written += ScaledInteger::Write(packet, mResistiveRotationalEffortX, 100.0, 0.0, ScaledInteger::Byte);
00438     }
00439     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortY) > 0)
00440     {
00441         expected += BYTE_SIZE;
00442         written += ScaledInteger::Write(packet, mResistiveRotationalEffortY, 100.0, 0.0, ScaledInteger::Byte);
00443     }
00444     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortZ) > 0)
00445     {
00446         expected += BYTE_SIZE;
00447         written += ScaledInteger::Write(packet, mResistiveRotationalEffortZ, 100.0, 0.0, ScaledInteger::Byte);
00448     }
00449 
00450     return expected == written ? written : -1;
00451 }
00452 
00453 
00465 int ReportWrenchEffort::ReadMessageBody(const Packet& packet)
00466 {
00467     int expected = USHORT_SIZE;
00468     int read = 0;
00469 
00470     read += packet.Read(mPresenceVector);
00471 
00472     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortX) > 0)
00473     {
00474         expected += USHORT_SIZE;
00475         read += ScaledInteger::Read(packet, mPropulsiveLinearEffortX, 100.0, -100.0, ScaledInteger::UShort);
00476     }
00477     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortY) > 0)
00478     {
00479         expected += USHORT_SIZE;
00480         read += ScaledInteger::Read(packet, mPropulsiveLinearEffortY, 100.0, -100.0, ScaledInteger::UShort);
00481     }
00482     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortZ) > 0)
00483     {
00484         expected += USHORT_SIZE;
00485         read += ScaledInteger::Read(packet, mPropulsiveLinearEffortZ, 100.0, -100.0, ScaledInteger::UShort);
00486     }
00487     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortX) > 0)
00488     {
00489         expected += USHORT_SIZE;
00490         read += ScaledInteger::Read(packet, mPropulsiveRotationalEffortX, 100.0, -100.0, ScaledInteger::UShort);
00491     }
00492     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortY) > 0)
00493     {
00494         expected += USHORT_SIZE;
00495         read += ScaledInteger::Read(packet, mPropulsiveRotationalEffortY, 100.0, -100.0, ScaledInteger::UShort);
00496     }
00497     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortZ) > 0)
00498     {
00499         expected += USHORT_SIZE;
00500         read += ScaledInteger::Read(packet, mPropulsiveRotationalEffortZ, 100.0, -100.0, ScaledInteger::UShort);
00501     }
00502     if((mPresenceVector & PresenceVector::ResistiveLinearEffortX) > 0)
00503     {
00504         expected += BYTE_SIZE;
00505         read += ScaledInteger::Read(packet, mResistiveLinearEffortX, 100.0, 0.0, ScaledInteger::Byte);
00506     }
00507     if((mPresenceVector & PresenceVector::ResistiveLinearEffortY) > 0)
00508     {
00509         expected += BYTE_SIZE;
00510         read += ScaledInteger::Read(packet, mResistiveLinearEffortY, 100.0, 0.0, ScaledInteger::Byte);
00511     }
00512     if((mPresenceVector & PresenceVector::ResistiveLinearEffortZ) > 0)
00513     {
00514         expected += BYTE_SIZE;
00515         read += ScaledInteger::Read(packet, mResistiveLinearEffortZ, 100.0, 0.0, ScaledInteger::Byte);
00516     }
00517     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortX) > 0)
00518     {
00519         expected += BYTE_SIZE;
00520         read += ScaledInteger::Read(packet, mResistiveRotationalEffortX, 100.0, 0.0, ScaledInteger::Byte);
00521     }
00522     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortY) > 0)
00523     {
00524         expected += BYTE_SIZE;
00525         read += ScaledInteger::Read(packet, mResistiveRotationalEffortY, 100.0, 0.0, ScaledInteger::Byte);
00526     }
00527     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortZ) > 0)
00528     {
00529         expected += BYTE_SIZE;
00530         read += ScaledInteger::Read(packet, mResistiveRotationalEffortZ, 100.0, 0.0, ScaledInteger::Byte);
00531     }    
00532 
00533     return expected == read ? read : -1;
00534 }
00535 
00536 
00542 void ReportWrenchEffort::ClearMessageBody()
00543 {
00544     mPresenceVector = 0;
00545     mPropulsiveLinearEffortX = 0;             
00546     mPropulsiveLinearEffortY = 0;             
00547     mPropulsiveLinearEffortZ = 0;             
00548     mPropulsiveRotationalEffortX = 0;             
00549     mPropulsiveRotationalEffortY = 0;             
00550     mPropulsiveRotationalEffortZ = 0;             
00551     mResistiveLinearEffortX = 0;             
00552     mResistiveLinearEffortY = 0;             
00553     mResistiveLinearEffortZ = 0;             
00554     mResistiveRotationalEffortX = 0;             
00555     mResistiveRotationalEffortY = 0;             
00556     mResistiveRotationalEffortZ = 0;
00557 }
00558 
00559 
00567 int ReportWrenchEffort::RunTestCase() const
00568 {
00569     int result = 0;
00570 
00571     Packet packet;
00572 
00573     ReportWrenchEffort wrench;
00574 
00575     wrench.SetPropulsiveLinearEffortX(60);
00576     wrench.SetPropulsiveRotationalEffortZ(-30);
00577 
00578     if(wrench.WriteMessageBody(packet))
00579     {
00580         wrench.ClearMessage();
00581         if(wrench.ReadMessageBody(packet))
00582         {
00583             if(wrench.AreFieldsPresent(PresenceVector::PropulsiveLinearEffortX | PresenceVector::PropulsiveRotationalEffortZ))
00584             {
00585                 result = 1;
00586             }
00587         }
00588     }
00589 
00590     return result;
00591 }
00592 
00593 
00599 ReportWrenchEffort& ReportWrenchEffort::operator=(const ReportWrenchEffort& message)
00600 {
00601     if(this != &message)
00602     {
00603         CopyHeaderData(&message);
00604         mPresenceVector = message.mPresenceVector;
00605         mPropulsiveLinearEffortX = message.mPropulsiveLinearEffortX;             
00606         mPropulsiveLinearEffortY = message.mPropulsiveLinearEffortY;             
00607         mPropulsiveLinearEffortZ = message.mPropulsiveLinearEffortZ;             
00608         mPropulsiveRotationalEffortX = message.mPropulsiveRotationalEffortX;             
00609         mPropulsiveRotationalEffortY = message.mPropulsiveRotationalEffortY;             
00610         mPropulsiveRotationalEffortZ = message.mPropulsiveRotationalEffortZ;             
00611         mResistiveLinearEffortX = message.mResistiveLinearEffortX;             
00612         mResistiveLinearEffortY = message.mResistiveLinearEffortY;             
00613         mResistiveLinearEffortZ = message.mResistiveLinearEffortZ;             
00614         mResistiveRotationalEffortX = message.mResistiveRotationalEffortX;             
00615         mResistiveRotationalEffortY = message.mResistiveRotationalEffortY;             
00616         mResistiveRotationalEffortZ = message.mResistiveRotationalEffortZ; 
00617     }
00618     return *this;
00619 }
00620 
00621 
00622 /*  End of File */