sideparkcalc.h 1.6 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182
  1. #ifndef SIDEPARKCALC_H
  2. #define SIDEPARKCALC_H
  3. #include <math.h>
  4. #include <vector>
  5. enum SideParkMode
  6. {
  7. ParkAtLeft =1,
  8. ParkAtRight =2
  9. };
  10. enum SideParkType
  11. {
  12. FiveStep = 1, //Wh
  13. TwoStep = 2, //When hdg is big, y is small, use 2 steps is ok. 1 point
  14. NoSolution = 3
  15. };
  16. namespace iv {
  17. struct SideParkPoint
  18. {
  19. SideParkPoint(double x,double y,double fhdg)
  20. {
  21. mx = x;
  22. my = y;
  23. mhdg = fhdg;
  24. }
  25. double mx;
  26. double my;
  27. double mhdg;
  28. };
  29. }
  30. class SideParkCalc
  31. {
  32. public:
  33. SideParkCalc(double x,double y,double hdg,double fRadius = 5.0,double MaxWheel = 430.0,double MaxAngle = 45.0,double fLastDirectDis = 0.3);
  34. public:
  35. void CalcPark();
  36. SideParkType GetSolution(std::vector<iv::SideParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen);
  37. private:
  38. double mfLastDirectDis = 0.3;
  39. double mfRaidus = 5.0;
  40. double mfMaxAngle = 45.0 *M_PI/180.0;
  41. double mfMaxWheel = 430.0;
  42. std::vector<iv::SideParkPoint> mvectorpoint;
  43. SideParkType mSideParkType; //five steps / 2 steps / no solution
  44. std::vector<double> mvectorWheel; // 5 values / 2 values /no value
  45. std::vector<double> mvectorlen; //5 values / 2 values /no value
  46. double mfTotalLen = 0.0;
  47. private:
  48. double mx,my,mhdg;
  49. bool mbPark = false;
  50. private:
  51. SideParkMode CalcParkMode();
  52. void ParkAtRightCalc();
  53. void ParkAtRightCalc_Model1();
  54. void ParkAtRightCalc_Model2();
  55. void ParkAtLeftCalc();
  56. void ParkAtLeftCalc_Model1();
  57. void ParkAtLeftCalc_Model2();
  58. void normalhdg(double & fhdg);
  59. };
  60. #endif // SIDEPARKCALC_H