Target_Planer.h

00001 #ifndef Target_Planer_
00002 #define Target_Planer_
00003 
00004 #include <vector>
00005 
00006 #include <mbed.h>
00007 
00008 class Target_Planer
00009 {
00010 public:
00011 
00012     Target_Planer(double *Pathx, double *Pathy, uint16_t N, double R);
00013     
00014     virtual ~Target_Planer();
00015     
00016     void     initialize(double RobotPosx, double RobotPosy);
00017     void     update(double RobotPosx, double RobotPosy);
00018     void     readTargetPos(double *TargetPosx, double *TargetPosy);
00019     void     readTargetAng(double *TargetAng);
00020     void     readTarget(double *TargetPosx, double *TargetPosy, double *TargetAng);
00021     double   readTargetPosx();
00022     double   readTargetPosy();
00023     double   readTargetAng();
00024     void     updateAndReturnTarget(double RobotPosx, double RobotPosy, double *TargetPosx, double *TargetPosy, double *TargetAng);
00025         
00026     /* debug */
00027     double   returnPathxAtIndex(int i);
00028     double   returnPathyAtIndex(int i);
00029     uint16_t returnPathLength();
00030     uint16_t returnClosestPointOnPath(double RobotPosx, double RobotPosy);
00031     uint16_t return_i_min_();
00032     
00033 private:
00034 
00035     struct PATH{
00036         vector<double> x;
00037         vector<double> y;
00038         uint16_t N;
00039     } Path_;
00040     
00041     struct ROBOTPOS{
00042         double x;
00043         double y;
00044     } RobotPos_;
00045     
00046     struct TARGETPOS{
00047         double x;
00048         double y;
00049     } TargetPos_;
00050     
00051     double TargetAng_;
00052     
00053     double   R_;
00054     double   del_;
00055     uint16_t i_min_;
00056     
00057     uint16_t findClosestPointOnPath(PATH Path, ROBOTPOS RobotPos);
00058     double   calcDistSquared(PATH Path, ROBOTPOS RobotPos, uint16_t i);
00059     double   calcNormPathsegment_i(PATH Path, uint16_t i);
00060     void     projectOnPath(PATH Path, ROBOTPOS RobotPos, uint16_t i, double *a_i, double *sx_i, double *sy_i, double *dl1_i);
00061     void     findIntersectionWithPath(double px_i, double py_i, double px_ip1, double py_ip1, double rx, double ry, bool *isvalid, double *x_intsec_valid, double *y_intsec_valid);
00062     void     transformDataOnPath(double px_i, double py_i, double px_ip1, double py_ip1, double rx, double ry, double *wp1x, double *wp1y, double *x_act, double *y_act, double *s_phi, double *c_phi);
00063     void     inverseTransformDataOnPath(double px_i, double py_i, double s_phi, double c_phi, double *x_intsec_valid_temp, double *y_intsec_valid_temp);
00064     
00065 };
00066 
00067 #endif