[zumo-cc3200/zumo-cc3200.git] / src / Energia / libraries / ZumoCC3200 / examples / SplineDrive / MotionPlanner.h
1 #ifndef MotionPlanner_h
2 #define MotionPlanner_h
4 #include <vector>
6 #include <ZumoMotors.h>
7 #include <spline.h>
8 #include <PIDController.h>
10 class MotionPlanner
11 {
12 public:
13 struct Point {
14 double x;
15 double y;
16 };
18 MotionPlanner();
20 void setPoints(std::vector<int> inputPoints);
21 void generateSpline();
22 void followSpline(ZumoMotors motors, PIDController pid, double rate);
23 tk::spline intervalInterpolate(MotionPlanner::Point leftPoint, MotionPlanner::Point rightPoint, double leftTangent, double rightTangent);
24 void generateTrajectory();
25 void generateParameterizedSpline();
26 bool ready();
28 private:
29 int iteration; /* current iteration of followSpline */
30 std::vector<Point> points;
31 std::vector<double> trajectory; /* list of headings to regulate to while driving spline */
32 bool dataReceived;
33 bool trajCalculated;
35 /* private member methods */
37 void slowSort(int X[], int Y[], int length); /* sort data before creating spline */
38 double dist(MotionPlanner::Point a, MotionPlanner::Point b);
40 /* constants */
42 /* 800 pixels = 8 ft = 800 iterations -> 1 pixel = 1 iteration */
43 static const float INTERVAL_LENGTH = 1.0f;
45 /* number of segments to evaulate between each pair of input points */
46 static const int RESOLUTION;
47 };
49 #endif /* MotionPlanner_h */