SegwayRMP.hh
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 #ifndef SEGWAYRMP_HH
00028 #define SEGWAYRMP_HH
00029
00030 #include "Model.hh"
00031
00032
00033 class Body;
00034 class HingeJoint;
00035 class SliderJoint;
00036 class Geom;
00037 class SphereGeom;
00038 class GeomData;
00039
00040 typedef struct gz_position gz_position_t;
00041
00042
00043 class SegwayRMP : public Model
00044 {
00045
00046 public: SegwayRMP(World *world);
00047 public: virtual ~SegwayRMP();
00048
00049
00050 public: virtual int Load( WorldFile *file, WorldFileNode *node );
00051
00052
00053 public: virtual int Init( WorldFile *file, WorldFileNode *node );
00054
00055
00056 public: virtual int Fini();
00057
00058
00059 public: virtual void Update( double step );
00060
00061
00062 private: void UpdateOdometry( double step );
00063
00064
00065 private: int OdeLoad( WorldFile *file, WorldFileNode *node );
00066
00067
00068 private: int OdeInit( WorldFile *file, WorldFileNode *node );
00069
00070
00071 private: int OdeFini();
00072
00073
00074 private: int IfaceInit();
00075
00076
00077 private: int IfaceFini();
00078
00079
00080 private: void IfaceGetCmd();
00081
00082
00083 private: void IfacePutData();
00084
00085
00086 private: double wheelSep, wheelDiam;
00087
00088
00089 private: Body *base, *top, *tires[2];
00090 private: SliderJoint *topJoint;
00091 private: HingeJoint *tireJoints[2];
00092
00093
00094 private: gz_position_t *iface;
00095
00096
00097 private: double wheelSpeed[2];
00098 double desLinVel, curLinVel;
00099 double desAngVel;
00100 double maxLinVel;
00101 double maxLinAcc;
00102
00103
00104 private: class PID
00105 {
00106 public:
00107 PID(double Kp, double Ki, double Kd) : Kp(Kp), Ki(Ki), Kd(Kd) , e(0), de(0), ie(0) {}
00108 double Kp,Ki,Kd;
00109 double e, de, ie;
00110 double s(double e) { de = e - this->e; ie += e; this->e = e; return Kp*e + Ki*ie + Kd*de; }
00111 };
00112
00113
00114 class ControlState
00115 {
00116 public:
00117 ControlState() : dist(0), vel(0), pitch(0), dpitch(0), yaw(0), dyaw(0) {}
00118 double dist, vel;
00119 double pitch, dpitch;
00120 double yaw, dyaw;
00121 };
00122
00123
00124 private: PID* angVelPID;
00125 PID* linVelPID;
00126 ControlState* desState;
00127 ControlState* curState;
00128
00129
00130 private: double odomPose[3];
00131 private: double odomOrient[2];
00132 };
00133
00134 #endif
Last updated 12 September 2005 21:38:45
|