TrajControl.h
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
00030
00031
00033
00034
00035 #ifndef __TRAJ_CONTORL_H
00036 #define __TRAJ_CONTORL_H
00037
00038 #ifdef MIP_HOST_APPLE
00039 #include <applePatch.h>
00040 #endif
00041
00042 #include "Trajectory.h"
00043
00044 using namespace MipBaselib;
00049 class VelVec{
00050 private:
00052 Decimal _linVel;
00054 Decimal _angVel;
00056 Time _time;
00057 public:
00059 VelVec();
00060
00062 VelVec(Decimal linVel, Decimal angVel, Time time);
00063
00065 VelVec(const VelVec& vel);
00066
00068 VelVec& operator=(const VelVec& vel);
00069
00070
00072 void setLinVel(Decimal value);
00073
00075 void setAngVel(Decimal value);
00076
00078 void setTime(Time value);
00079
00080
00082 Decimal linVel();
00083
00085 Decimal angVel();
00086
00088 Time time();
00089 };
00090
00091
00096 class OpenLoopIn{
00097 protected:
00099 Position _desiredPosition;
00101 Position _desiredDPosition;
00102
00103 public:
00105 OpenLoopIn();
00106
00110 OpenLoopIn(Position& desP, Position& desDP);
00111
00114 OpenLoopIn(OpenLoopIn& other);
00115
00118 OpenLoopIn operator=(OpenLoopIn& other);
00119
00120
00123 void setDesiredPosition(Position& p);
00124
00127 void setDesiredDPosition(Position& p);
00128
00129
00132 Position desiredPosition();
00133
00136 Position desiredDPosition();
00137 };
00138
00139
00144 class NonlinearIn : public OpenLoopIn{
00145 protected:
00147 Pose _robotPose;
00148
00149 public:
00151 NonlinearIn();
00152
00157 NonlinearIn(Pose& rP, Position& desP, Position& desDP);
00158
00161 NonlinearIn(NonlinearIn& other);
00162
00165 NonlinearIn operator=(NonlinearIn& other);
00166
00167
00170 void setRobotPose(Pose& p);
00171
00172
00175 Pose robotPose();
00176 };
00177
00178
00179
00184 class DFLIn: public NonlinearIn{
00186 Pose _robotDPose;
00188 Position _desiredDDPosition;
00190 Decimal _timeOfSampling;
00191
00192 public:
00194 DFLIn();
00195
00203 DFLIn(Pose& rP, Pose& rDP, Position& desP, Position& desDP, Position& desDDP, Decimal tos = 0.0);
00204
00207 DFLIn(DFLIn& other);
00208
00211 DFLIn operator=(DFLIn& other);
00212
00213
00216 void setRobotDPose(Pose& p);
00217
00220 void setDesiredDDPosition(Position& p);
00221
00224 void setTimeOfSampling(Decimal tos);
00225
00226
00229 Pose robotDPose();
00230
00233 Position desiredDDPosition();
00234
00237 Decimal timeOfSampling();
00238 };
00239
00240
00245 class TrajectoryState{
00246
00247 private:
00249 Pose _statusPose;
00251 Pose _statusPosep;
00253 Position _dxy;
00255 Decimal _PSpace;
00257 VelVec _statusVel;
00259 Position _wheelVel;
00261 Position _dtWheel;
00262
00263 public:
00265 TrajectoryState();
00266
00268 TrajectoryState(const TrajectoryState& status);
00269
00271 TrajectoryState& operator=(const TrajectoryState& status);
00272
00273
00275 void setStatusPose(Pose value);
00276
00278 void setStatusPosep(Pose value);
00279
00281 void setDxy(Position value);
00282
00284 void setPSpace(Decimal value);
00285
00287 void setStatusVel(VelVec value);
00288
00290 void setWheelVel(Position value);
00291
00293 void setDtWheel(Position value);
00294
00295
00297 Pose statusPose();
00298
00300 Pose statusPosep();
00301
00303 Position dxy();
00304
00306 Decimal PSpace();
00307
00309 VelVec statusVel();
00310
00312 Position wheelVel();
00313
00315 Position dtWheel();
00316
00317 };
00318
00319
00328 class TrajControl{
00329 private:
00330
00331
00332
00333
00334 Decimal _oldDrive;
00335 Decimal _oldTurnRate;
00336
00337 Decimal _csi;
00339 Decimal _prevErr;
00340 Decimal _prevU;
00341
00342 Timer _timer;
00343 public:
00344
00346 TrajControl();
00347
00354 void openLoop(AnalyticalTrajectory ref, VelVec &Vr, Time time);
00355
00360 void nonlinear(AnalyticalTrajectory refTraj, VelVec &vr, Time time, TrajectoryState mState);
00361
00367 void DFL(AnalyticalTrajectory &trif, VelVec &vr, Decimal tos, Time time, TrajectoryState mState);
00368
00374 void FL(AnalyticalTrajectory &trif, VelVec &vr, Decimal tos, Time time, TrajectoryState mState);
00375
00382 void openLoop(OpenLoopIn &input, VelVec &vr, Time &time);
00383
00388 void nonlinear(NonlinearIn &input, VelVec &vr, Time &time);
00389
00393 void DFL(DFLIn &input, VelVec &vr);
00394
00400 void Tustin(Angle angReff, VelVec &Vr, TrajectoryState mState);
00401
00403 void reset(){
00404 _csi = 0.0;
00405 _timer.reset();
00406 }
00407 };
00408
00409 #endif
00410
00411
00412
00413
00414
00415
00416