GT2005: Platform/Aperios1.3.2/MotorCommandsSender.h Source File

00001 /**
00002  * @file
00003  * This file contains a sender for motor commands.
00004  */
00005 #ifndef __MOTORCOMMANDSSENDER_H__
00006 #define __MOTORCOMMANDSSENDER_H__
00007 
00008 #ifndef __ProcessFramework_h__
00009 #error Never include this file directly. Include ProcessFramework.h instead.
00010 #endif
00011 
00012 #include "Platform/ProcessFramework.h"
00013 #include "Representations/Motion/MotorCommands.h"
00014 #include <OPENR/OPENRAPI.h>
00015 
00016 /**
00017  * This class implements a sender for motor commands.
00018  */
00019 class MotorCommandsSender : public SenderBase<MotorCommands>
00020 {
00021 private:
00022   OPrimitiveID jointId[JointData::numOfJoint]; /**< The Open-R IDs of all joints. */
00023   bool jointGainsSet; /**< A flag that states whether the joint gains were initialized. */
00024   MemoryRegionID memID; /**< The memory region of the only package instance. */
00025   OCommandVectorData* cmdVec; /**< A pointer to the data in the package. */
00026   RCRegion* package; /**< The only instance of a package. */
00027   PIDData lastPidData; /**< The last joint gains sent. */
00028   int numOfLED; /**< The number of LEDs of the robot. */
00029 
00030   /**
00031     * Returns OPENR Primitive Joint Name
00032     */
00033   static const char* getPrimitiveJointName(int i) 
00034   {
00035     if(SystemCall::getRobotDesign() == RobotDesign::ERS210)
00036       switch (i) 
00037       {
00038         case 0: return "PRM:/r1/c1-Joint2:j1";            // head tilt
00039         case 1: return "PRM:/r1/c1/c2-Joint2:j2";         // head pan
00040         case 2: return "PRM:/r1/c1/c2/c3-Joint2:j3";      // head roll
00041         case 3: return "PRM:/r1/c1/c2/c3/c4-Joint2:j4";   // mouth
00042         case 4: return "PRM:/r1/c1/c2/c3/e1-Joint3:j5";   // ear L
00043         case 5: return "PRM:/r1/c1/c2/c3/e2-Joint3:j6";   // ear R
00044         case 6: return "PRM:/r4/c1-Joint2:j1";            // leg FR joint
00045         case 7: return "PRM:/r4/c1/c2-Joint2:j2";         // leg FR shoulder
00046         case 8: return "PRM:/r4/c1/c2/c3-Joint2:j3";      // leg FR knee
00047         case 9: return "PRM:/r2/c1-Joint2:j1";            // leg FL joint
00048         case 10: return "PRM:/r2/c1/c2-Joint2:j2";        // leg FL shoulder
00049         case 11: return "PRM:/r2/c1/c2/c3-Joint2:j3";     // leg FL knee
00050         case 12: return "PRM:/r5/c1-Joint2:j1";           // leg RR joint
00051         case 13: return "PRM:/r5/c1/c2-Joint2:j2";        // leg RR shoulder
00052         case 14: return "PRM:/r5/c1/c2/c3-Joint2:j3";     // leg RR knee
00053         case 15: return "PRM:/r3/c1-Joint2:j1";           // leg RL joint
00054         case 16: return "PRM:/r3/c1/c2-Joint2:j2";        // leg RL shoulder
00055         case 17: return "PRM:/r3/c1/c2/c3-Joint2:j3";     // leg RL knee
00056         case 18: return "PRM:/r6/c1-Joint2:j1";           // tail pan
00057         case 19: return "PRM:/r6/c2-Joint2:j2";           // tail tilt
00058         default: return ""; // not defined
00059       }
00060     else
00061       switch (i) 
00062       {
00063         case 0: return "PRM:/r1/c1-Joint2:11";            // head tilt 1
00064         case 1: return "PRM:/r1/c1/c2-Joint2:12";         // head pan
00065         case 2: return "PRM:/r1/c1/c2/c3-Joint2:13";      // head tilt 2
00066         case 3: return "PRM:/r1/c1/c2/c3/c4-Joint2:14";   // mouth
00067         case 4: return "PRM:/r1/c1/c2/c3/e1-Joint3:15";   // ear L
00068         case 5: return "PRM:/r1/c1/c2/c3/e2-Joint3:16";   // ear R
00069         case 6: return "PRM:/r4/c1-Joint2:41";            // leg FR joint
00070         case 7: return "PRM:/r4/c1/c2-Joint2:42";         // leg FR shoulder
00071         case 8: return "PRM:/r4/c1/c2/c3-Joint2:43";      // leg FR knee
00072         case 9: return "PRM:/r2/c1-Joint2:21";            // leg FL joint
00073         case 10: return "PRM:/r2/c1/c2-Joint2:22";        // leg FL shoulder
00074         case 11: return "PRM:/r2/c1/c2/c3-Joint2:23";     // leg FL knee
00075         case 12: return "PRM:/r5/c1-Joint2:51";           // leg RR joint
00076         case 13: return "PRM:/r5/c1/c2-Joint2:52";        // leg RR shoulder
00077         case 14: return "PRM:/r5/c1/c2/c3-Joint2:53";     // leg RR knee
00078         case 15: return "PRM:/r3/c1-Joint2:31";           // leg RL joint
00079         case 16: return "PRM:/r3/c1/c2-Joint2:32";        // leg RL shoulder
00080         case 17: return "PRM:/r3/c1/c2/c3-Joint2:33";     // leg RL knee
00081         case 18: return "PRM:/r6/c2-Joint2:62";           // tail pan
00082         case 19: return "PRM:/r6/c1-Joint2:61";           // tail tilt
00083         default: return ""; // not defined
00084       }
00085 
00086   }
00087 
00088   /**
00089     * Return OPENR Primitive LED Name
00090     */
00091   static const char* getPrimitiveLEDName(int i) 
00092   {
00093     if(SystemCall::getRobotDesign() == RobotDesign::ERS210)
00094       switch (i) 
00095       {
00096         case 0: return "PRM:/r1/c1/c2/c3/l1-LED2:l1";
00097         case 1: return "PRM:/r1/c1/c2/c3/l2-LED2:l2";
00098         case 2: return "PRM:/r1/c1/c2/c3/l3-LED2:l3";
00099         case 3: return "PRM:/r1/c1/c2/c3/l4-LED2:l4";
00100         case 4: return "PRM:/r1/c1/c2/c3/l5-LED2:l5";
00101         case 5: return "PRM:/r1/c1/c2/c3/l6-LED2:l6";
00102         case 6: return "PRM:/r1/c1/c2/c3/l7-LED2:l7";
00103         case 7: return "RPM:/r6/l1-LED2:l1"; //Blue LED
00104         case 8: return "RPM:/r6/l2-LED2:l2"; //Orange LED
00105         default: return ""; //not defined
00106       }
00107     else
00108       switch (i) 
00109       {
00110         case 0: return "PRM:/r1/c1/c2/c3/l1-LED2:l1"; // Head light(color)
00111         case 1: return "PRM:/r1/c1/c2/c3/l2-LED2:l2"; // Head light(white)
00112         case 2: return "PRM:/r1/c1/c2/c3/l3-LED2:l3"; // Mode Indicator(red)
00113         case 3: return "PRM:/r1/c1/c2/c3/l4-LED2:l4"; // Mode Indicator(green)
00114         case 4: return "PRM:/r1/c1/c2/c3/l5-LED2:l5"; // Mode Indicator(blue)
00115         case 5: return "PRM:/r1/c1/c2/c3/l6-LED2:l6"; // Wireless light
00116         case 6: return "PRM:/r1/c1/c2/c3/la-LED3:la"; // Face light1
00117         case 7: return "PRM:/r1/c1/c2/c3/lb-LED3:lb"; // Face light2
00118         case 8: return "PRM:/r1/c1/c2/c3/lc-LED3:lc"; // Face light3
00119         case 9: return "PRM:/r1/c1/c2/c3/ld-LED3:ld"; // Face light4
00120         case 10: return "PRM:/r1/c1/c2/c3/le-LED3:le"; // Face light5
00121         case 11: return "PRM:/r1/c1/c2/c3/lf-LED3:lf"; // Face light6
00122         case 12: return "PRM:/r1/c1/c2/c3/lg-LED3:lg"; // Face light7
00123         case 13: return "PRM:/r1/c1/c2/c3/lh-LED3:lh"; // Face light8
00124         case 14: return "PRM:/r1/c1/c2/c3/li-LED3:li"; // Face light9
00125         case 15: return "PRM:/r1/c1/c2/c3/lj-LED3:lj"; // Face light10
00126         case 16: return "PRM:/r1/c1/c2/c3/lk-LED3:lk"; // Face light11
00127         case 17: return "PRM:/r1/c1/c2/c3/ll-LED3:ll"; // Face light12
00128         case 18: return "PRM:/r1/c1/c2/c3/lm-LED3:lm"; // Face light13
00129         case 19: return "PRM:/r1/c1/c2/c3/ln-LED3:ln"; // Face light14
00130         case 20: return "PRM:/lu-LED3:lu"; // Back light(front, color)
00131         case 21: return "PRM:/lv-LED3:lv"; // Back light(front, white)
00132         case 22: return "PRM:/lw-LED3:lw"; // Back light(middle, color)
00133         case 23: return "PRM:/lx-LED3:lx"; // Back light(middle, white)
00134         case 24: return "PRM:/ly-LED3:ly"; // Back light(rear, color)
00135         case 25: return "PRM:/lz-LED3:lz"; // Back light(rear, white)
00136         default: return ""; //not defined
00137       }
00138   }
00139 
00140 protected:
00141   /**
00142     * The function prepares a package.
00143     */
00144   virtual void preparePackage();
00145 
00146   /**
00147     * The functions sets a package for a receiver.
00148     * @param receiver The receive the package will be sent to.
00149     */
00150   virtual void setPackage(const ObserverID& receiver);
00151 
00152   /**
00153     * The function frees the package.
00154     */
00155   virtual void freePackage();
00156 
00157 public:
00158   /**
00159     * The constructor.
00160     * @param process The process this sender is associated with.
00161     * @param blocking Decides whether this sender blocks the execution of the next frame
00162     *                 until all connected receivers have requested a new package.
00163     */
00164   MotorCommandsSender(PlatformProcess* process,bool blocking);
00165 
00166   /**
00167     * Destructor.
00168     */
00169   ~MotorCommandsSender();
00170 };
00171 
00172 /**
00173  * The macro declares a sender for motor commands.
00174  * It must be used inside a declaration of a process, after the macro DEBUGGING.
00175  */
00176 #define SENDER_MOTORCOMMANDS \
00177   MotorCommandsSender theMotorCommandsSender
00178 
00179 /**
00180  * The macro instantiates a sender for MotorCommands.
00181  * @param blocking Decides whether this sender blocks the execution of the next frame
00182  *                 until all connected receivers have requested a new package.
00183  */
00184 #define INIT_SENDER_MOTORCOMMANDS(blocking) \
00185   theMotorCommandsSender(this,blocking)
00186 
00187 #endif