JAUS++: jaus++/2.0/src/jaus/mobility/drivers/setwrencheffort.cpp Source 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/setwrencheffort.h"
00041 #include "jaus/core/scaledinteger.h"
00042 #include <iostream>
00043 
00044 using namespace JAUS;
00045 
00046 
00055 SetWrenchEffort::SetWrenchEffort(const Address& dest, const Address& src) : Message(SET_WRENCH_EFFORT, dest, src)
00056 {
00057     mPresenceVector = 0;
00058     mPropulsiveLinearEffortX = 0;             
00059     mPropulsiveLinearEffortY = 0;             
00060     mPropulsiveLinearEffortZ = 0;             
00061     mPropulsiveRotationalEffortX = 0;             
00062     mPropulsiveRotationalEffortY = 0;             
00063     mPropulsiveRotationalEffortZ = 0;             
00064     mResistiveLinearEffortX = 0;             
00065     mResistiveLinearEffortY = 0;             
00066     mResistiveLinearEffortZ = 0;             
00067     mResistiveRotationalEffortX = 0;             
00068     mResistiveRotationalEffortY = 0;             
00069     mResistiveRotationalEffortZ = 0;   
00070 }
00071 
00072 
00078 SetWrenchEffort::SetWrenchEffort(const SetWrenchEffort& message) : Message(SET_WRENCH_EFFORT)
00079 {
00080     mPresenceVector = 0;
00081     mPropulsiveLinearEffortX = 0;             
00082     mPropulsiveLinearEffortY = 0;             
00083     mPropulsiveLinearEffortZ = 0;             
00084     mPropulsiveRotationalEffortX = 0;             
00085     mPropulsiveRotationalEffortY = 0;             
00086     mPropulsiveRotationalEffortZ = 0;             
00087     mResistiveLinearEffortX = 0;             
00088     mResistiveLinearEffortY = 0;             
00089     mResistiveLinearEffortZ = 0;             
00090     mResistiveRotationalEffortX = 0;             
00091     mResistiveRotationalEffortY = 0;             
00092     mResistiveRotationalEffortZ = 0;   
00093     *this = message;
00094 }
00095 
00096 
00102 SetWrenchEffort::~SetWrenchEffort()
00103 {
00104 }
00105 
00106 
00117 bool SetWrenchEffort::SetPropulsiveLinearEffortX(const double percent)
00118 {
00119     if(percent >= -100.0 && percent <= 100.0)
00120     {
00121         mPropulsiveLinearEffortX = percent;
00122         mPresenceVector |= PresenceVector::PropulsiveLinearEffortX;
00123         return true;
00124     }
00125     return false;
00126 }
00127 
00128 
00139 bool SetWrenchEffort::SetPropulsiveLinearEffortY(const double percent)
00140 {
00141     if(percent >= -100.0 && percent <= 100.0)
00142     {
00143         mPropulsiveLinearEffortY = percent;
00144         mPresenceVector |= PresenceVector::PropulsiveLinearEffortY;
00145         return true;
00146     }
00147     return false;
00148 }
00149 
00150 
00161 bool SetWrenchEffort::SetPropulsiveLinearEffortZ(const double percent)
00162 {
00163     if(percent >= -100.0 && percent <= 100.0)
00164     {
00165         mPropulsiveLinearEffortZ = percent;
00166         mPresenceVector |= PresenceVector::PropulsiveLinearEffortZ;
00167         return true;
00168     }
00169     return false;
00170 }
00171 
00172 
00183 bool SetWrenchEffort::SetPropulsiveRotationalEffortX(const double percent)
00184 {
00185     if(percent >= -100.0 && percent <= 100.0)
00186     {
00187         mPropulsiveRotationalEffortX = percent;
00188         mPresenceVector |= PresenceVector::PropulsiveRotationalEffortX;
00189         return true;
00190     }
00191     return false;
00192 }
00193 
00194 
00205 bool SetWrenchEffort::SetPropulsiveRotationalEffortY(const double percent)
00206 {
00207     if(percent >= -100.0 && percent <= 100.0)
00208     {
00209         mPropulsiveRotationalEffortY = percent;
00210         mPresenceVector |= PresenceVector::PropulsiveRotationalEffortY;
00211         return true;
00212     }
00213     return false;
00214 }
00215 
00216 
00227 bool SetWrenchEffort::SetPropulsiveRotationalEffortZ(const double percent)
00228 {
00229     if(percent >= -100.0 && percent <= 100.0)
00230     {
00231         mPropulsiveRotationalEffortZ = percent;
00232         mPresenceVector |= PresenceVector::PropulsiveRotationalEffortZ;
00233         return true;
00234     }
00235     return false;
00236 }
00237 
00238 
00249 bool SetWrenchEffort::SetResistiveLinearEffortX(const double percent)
00250 {
00251     if(percent >= 0 && percent <= 100.0)
00252     {
00253         mResistiveLinearEffortX = percent;
00254         mPresenceVector |= PresenceVector::ResistiveLinearEffortX;
00255         return true;
00256     }
00257     return false;
00258 }
00259 
00260 
00271 bool SetWrenchEffort::SetResistiveLinearEffortY(const double percent)
00272 {
00273     if(percent >= 0 && percent <= 100.0)
00274     {
00275         mResistiveLinearEffortY = percent;
00276         mPresenceVector |= PresenceVector::ResistiveLinearEffortY;
00277         return true;
00278     }
00279     return false;
00280 }
00281 
00282 
00293 bool SetWrenchEffort::SetResistiveLinearEffortZ(const double percent)
00294 {
00295     if(percent >= 0 && percent <= 100.0)
00296     {
00297         mResistiveLinearEffortZ = percent;
00298         mPresenceVector |= PresenceVector::ResistiveLinearEffortZ;
00299         return true;
00300     }
00301     return false;
00302 }
00303 
00304 
00315 bool SetWrenchEffort::SetResistiveRotationalEffortX(const double percent)
00316 {
00317     if(percent >= 0 && percent <= 100.0)
00318     {
00319         mResistiveRotationalEffortX = percent;
00320         mPresenceVector |= PresenceVector::ResistiveRotationalEffortX;
00321         return true;
00322     }
00323     return false;
00324 }
00325 
00326 
00337 bool SetWrenchEffort::SetResistiveRotationalEffortY(const double percent)
00338 {
00339     if(percent >= 0 && percent <= 100.0)
00340     {
00341         mResistiveRotationalEffortY = percent;
00342         mPresenceVector |= PresenceVector::ResistiveRotationalEffortY;
00343         return true;
00344     }
00345     return false;
00346 }
00347 
00348 
00359 bool SetWrenchEffort::SetResistiveRotationalEffortZ(const double percent)
00360 {
00361     if(percent >= 0 && percent <= 100.0)
00362     {
00363         mResistiveRotationalEffortZ = percent;
00364         mPresenceVector |= PresenceVector::ResistiveRotationalEffortZ;
00365         return true;
00366     }
00367     return false;
00368 }
00369 
00370 
00371 
00383 int SetWrenchEffort::WriteMessageBody(Packet& packet) const
00384 {
00385     int expected = USHORT_SIZE;
00386     int written = 0;
00387 
00388     written += packet.Write(mPresenceVector);
00389 
00390     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortX) > 0)
00391     {
00392         expected += USHORT_SIZE;
00393         written += ScaledInteger::Write(packet, mPropulsiveLinearEffortX, 100.0, -100.0, ScaledInteger::UShort);
00394     }
00395     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortY) > 0)
00396     {
00397         expected += USHORT_SIZE;
00398         written += ScaledInteger::Write(packet, mPropulsiveLinearEffortY, 100.0, -100.0, ScaledInteger::UShort);
00399     }
00400     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortZ) > 0)
00401     {
00402         expected += USHORT_SIZE;
00403         written += ScaledInteger::Write(packet, mPropulsiveLinearEffortZ, 100.0, -100.0, ScaledInteger::UShort);
00404     }
00405     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortX) > 0)
00406     {
00407         expected += USHORT_SIZE;
00408         written += ScaledInteger::Write(packet, mPropulsiveRotationalEffortX, 100.0, -100.0, ScaledInteger::UShort);
00409     }
00410     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortY) > 0)
00411     {
00412         expected += USHORT_SIZE;
00413         written += ScaledInteger::Write(packet, mPropulsiveRotationalEffortY, 100.0, -100.0, ScaledInteger::UShort);
00414     }
00415     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortZ) > 0)
00416     {
00417         expected += USHORT_SIZE;
00418         written += ScaledInteger::Write(packet, mPropulsiveRotationalEffortZ, 100.0, -100.0, ScaledInteger::UShort);
00419     }
00420     if((mPresenceVector & PresenceVector::ResistiveLinearEffortX) > 0)
00421     {
00422         expected += BYTE_SIZE;
00423         written += ScaledInteger::Write(packet, mResistiveLinearEffortX, 100.0, 0.0, ScaledInteger::Byte);
00424     }
00425     if((mPresenceVector & PresenceVector::ResistiveLinearEffortY) > 0)
00426     {
00427         expected += BYTE_SIZE;
00428         written += ScaledInteger::Write(packet, mResistiveLinearEffortY, 100.0, 0.0, ScaledInteger::Byte);
00429     }
00430     if((mPresenceVector & PresenceVector::ResistiveLinearEffortZ) > 0)
00431     {
00432         expected += BYTE_SIZE;
00433         written += ScaledInteger::Write(packet, mResistiveLinearEffortZ, 100.0, 0.0, ScaledInteger::Byte);
00434     }
00435     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortX) > 0)
00436     {
00437         expected += BYTE_SIZE;
00438         written += ScaledInteger::Write(packet, mResistiveRotationalEffortX, 100.0, 0.0, ScaledInteger::Byte);
00439     }
00440     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortY) > 0)
00441     {
00442         expected += BYTE_SIZE;
00443         written += ScaledInteger::Write(packet, mResistiveRotationalEffortY, 100.0, 0.0, ScaledInteger::Byte);
00444     }
00445     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortZ) > 0)
00446     {
00447         expected += BYTE_SIZE;
00448         written += ScaledInteger::Write(packet, mResistiveRotationalEffortZ, 100.0, 0.0, ScaledInteger::Byte);
00449     }
00450 
00451     return expected == written ? written : -1;
00452 }
00453 
00454 
00466 int SetWrenchEffort::ReadMessageBody(const Packet& packet)
00467 {
00468     int expected = USHORT_SIZE;
00469     int read = 0;
00470 
00471     read += packet.Read(mPresenceVector);
00472 
00473     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortX) > 0)
00474     {
00475         expected += USHORT_SIZE;
00476         read += ScaledInteger::Read(packet, mPropulsiveLinearEffortX, 100.0, -100.0, ScaledInteger::UShort);
00477     }
00478     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortY) > 0)
00479     {
00480         expected += USHORT_SIZE;
00481         read += ScaledInteger::Read(packet, mPropulsiveLinearEffortY, 100.0, -100.0, ScaledInteger::UShort);
00482     }
00483     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortZ) > 0)
00484     {
00485         expected += USHORT_SIZE;
00486         read += ScaledInteger::Read(packet, mPropulsiveLinearEffortZ, 100.0, -100.0, ScaledInteger::UShort);
00487     }
00488     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortX) > 0)
00489     {
00490         expected += USHORT_SIZE;
00491         read += ScaledInteger::Read(packet, mPropulsiveRotationalEffortX, 100.0, -100.0, ScaledInteger::UShort);
00492     }
00493     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortY) > 0)
00494     {
00495         expected += USHORT_SIZE;
00496         read += ScaledInteger::Read(packet, mPropulsiveRotationalEffortY, 100.0, -100.0, ScaledInteger::UShort);
00497     }
00498     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortZ) > 0)
00499     {
00500         expected += USHORT_SIZE;
00501         read += ScaledInteger::Read(packet, mPropulsiveRotationalEffortZ, 100.0, -100.0, ScaledInteger::UShort);
00502     }
00503     if((mPresenceVector & PresenceVector::ResistiveLinearEffortX) > 0)
00504     {
00505         expected += BYTE_SIZE;
00506         read += ScaledInteger::Read(packet, mResistiveLinearEffortX, 100.0, 0.0, ScaledInteger::Byte);
00507     }
00508     if((mPresenceVector & PresenceVector::ResistiveLinearEffortY) > 0)
00509     {
00510         expected += BYTE_SIZE;
00511         read += ScaledInteger::Read(packet, mResistiveLinearEffortY, 100.0, 0.0, ScaledInteger::Byte);
00512     }
00513     if((mPresenceVector & PresenceVector::ResistiveLinearEffortZ) > 0)
00514     {
00515         expected += BYTE_SIZE;
00516         read += ScaledInteger::Read(packet, mResistiveLinearEffortZ, 100.0, 0.0, ScaledInteger::Byte);
00517     }
00518     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortX) > 0)
00519     {
00520         expected += BYTE_SIZE;
00521         read += ScaledInteger::Read(packet, mResistiveRotationalEffortX, 100.0, 0.0, ScaledInteger::Byte);
00522     }
00523     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortY) > 0)
00524     {
00525         expected += BYTE_SIZE;
00526         read += ScaledInteger::Read(packet, mResistiveRotationalEffortY, 100.0, 0.0, ScaledInteger::Byte);
00527     }
00528     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortZ) > 0)
00529     {
00530         expected += BYTE_SIZE;
00531         read += ScaledInteger::Read(packet, mResistiveRotationalEffortZ, 100.0, 0.0, ScaledInteger::Byte);
00532     }    
00533 
00534     return expected == read ? read : -1;
00535 }
00536 
00537 
00543 void SetWrenchEffort::ClearMessageBody()
00544 {
00545     mPresenceVector = 0;
00546     mPropulsiveLinearEffortX = 0;             
00547     mPropulsiveLinearEffortY = 0;             
00548     mPropulsiveLinearEffortZ = 0;             
00549     mPropulsiveRotationalEffortX = 0;             
00550     mPropulsiveRotationalEffortY = 0;             
00551     mPropulsiveRotationalEffortZ = 0;             
00552     mResistiveLinearEffortX = 0;             
00553     mResistiveLinearEffortY = 0;             
00554     mResistiveLinearEffortZ = 0;             
00555     mResistiveRotationalEffortX = 0;             
00556     mResistiveRotationalEffortY = 0;             
00557     mResistiveRotationalEffortZ = 0;
00558 }
00559 
00560 
00568 int SetWrenchEffort::RunTestCase() const
00569 {
00570     int result = 0;
00571 
00572     Packet packet;
00573 
00574     SetWrenchEffort wrench;
00575 
00576     wrench.SetPropulsiveLinearEffortY(60);
00577     wrench.SetPropulsiveRotationalEffortY(-30);
00578 
00579     if(wrench.WriteMessageBody(packet))
00580     {
00581         wrench.ClearMessage();
00582         if(wrench.ReadMessageBody(packet))
00583         {
00584             if(wrench.AreFieldsPresent(PresenceVector::PropulsiveLinearEffortY | PresenceVector::PropulsiveRotationalEffortY))
00585             {
00586                 result = 1;
00587             }
00588         }
00589     }
00590 
00591     return result;
00592 }
00593 
00594 
00600 void SetWrenchEffort::PrintMessageBody() const
00601 {
00602     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortX) > 0)
00603     {
00604         std::cout << "Propulsive Linear Effort X: " << mPropulsiveLinearEffortX << std::endl;
00605     }
00606     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortY) > 0)
00607     {
00608         std::cout << "Propulsive Linear Effort Y: " << mPropulsiveLinearEffortY << std::endl;
00609     }
00610     if((mPresenceVector & PresenceVector::PropulsiveLinearEffortZ) > 0)
00611     {
00612         std::cout << "Propulsive Linear Effort Z: " << mPropulsiveLinearEffortZ << std::endl;
00613     }
00614     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortX) > 0)
00615     {
00616         std::cout << "Propulsive Rotational Effort X: " << mPropulsiveRotationalEffortX << std::endl;
00617     }
00618     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortY) > 0)
00619     {
00620         std::cout << "Propulsive Rotational Effort Y: " << mPropulsiveRotationalEffortY << std::endl;
00621     }
00622     if((mPresenceVector & PresenceVector::PropulsiveRotationalEffortZ) > 0)
00623     {
00624         std::cout << "Propulsive Rotational Effort Z: " << mPropulsiveRotationalEffortZ << std::endl;
00625     }
00626     if((mPresenceVector & PresenceVector::ResistiveLinearEffortX) > 0)
00627     {
00628         std::cout << "Resistive Linear Effort X: " << mResistiveLinearEffortX << std::endl;
00629     }
00630     if((mPresenceVector & PresenceVector::ResistiveLinearEffortY) > 0)
00631     {
00632         std::cout << "Resistive Linear Effort Y: " << mResistiveLinearEffortY << std::endl;
00633     }
00634     if((mPresenceVector & PresenceVector::ResistiveLinearEffortZ) > 0)
00635     {
00636         std::cout << "Resistive Linear Effort Z: " << mResistiveLinearEffortZ << std::endl;
00637     }
00638     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortX) > 0)
00639     {
00640         std::cout << "Resistive Rotational Effort X: " << mResistiveRotationalEffortX << std::endl;
00641     }
00642     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortY) > 0)
00643     {
00644         std::cout << "Resistive Rotational Effort Y: " << mResistiveRotationalEffortY << std::endl;
00645     }
00646     if((mPresenceVector & PresenceVector::ResistiveRotationalEffortZ) > 0)
00647     {
00648         std::cout << "Resistive Rotational Effort Z: " << mResistiveRotationalEffortZ << std::endl;
00649     } 
00650 }
00651 
00652 
00658 SetWrenchEffort& SetWrenchEffort::operator=(const SetWrenchEffort& message)
00659 {
00660     if(this != &message)
00661     {
00662         CopyHeaderData(&message);
00663         mPresenceVector = message.mPresenceVector;
00664         mPropulsiveLinearEffortX = message.mPropulsiveLinearEffortX;             
00665         mPropulsiveLinearEffortY = message.mPropulsiveLinearEffortY;             
00666         mPropulsiveLinearEffortZ = message.mPropulsiveLinearEffortZ;             
00667         mPropulsiveRotationalEffortX = message.mPropulsiveRotationalEffortX;             
00668         mPropulsiveRotationalEffortY = message.mPropulsiveRotationalEffortY;             
00669         mPropulsiveRotationalEffortZ = message.mPropulsiveRotationalEffortZ;             
00670         mResistiveLinearEffortX = message.mResistiveLinearEffortX;             
00671         mResistiveLinearEffortY = message.mResistiveLinearEffortY;             
00672         mResistiveLinearEffortZ = message.mResistiveLinearEffortZ;             
00673         mResistiveRotationalEffortX = message.mResistiveRotationalEffortX;             
00674         mResistiveRotationalEffortY = message.mResistiveRotationalEffortY;             
00675         mResistiveRotationalEffortZ = message.mResistiveRotationalEffortZ; 
00676     }
00677     return *this;
00678 }
00679 
00680 
00681 /*  End of File */