purePursuit.h

00001 #ifndef purePursuit_H
00002 #define purePursuit_H
00003 
00004 #include "mbed.h"
00005 #include "generalFunctions.h"
00006 #include "config.h"
00007 
00008 class purePursuit
00009 {
00010 public:
00011 
00012     purePursuit();
00013 
00014     /// generate velocity (mm/s) and omega (rad/s) for diff drive to track
00015     void GenVW(float* purePursuit_velocity, float* purePursuit_omega,
00016                float target_waypoint[2], float target_velocity,
00017                float current_position[2], float current_heading);
00018 
00019     /// generate velocity (mm/s), gamma (rad) and omega (rad/s) for non-linear controller to track
00020     void GenVGW(float* purePursuit_velocity, float* purePursuit_gamma, float* purePursuit_omega,
00021                 float target_waypoint[2], float target_velocity,
00022                 float current_position[2], float current_heading);
00023 
00024     float robotFrame_targetDistance;
00025     float purePursuit_headingE;
00026     float purePursuit_ld;
00027     float purePursuit_r;
00028 
00029 private:
00030     static float const ld_min = 200.0; //min purepursuit lookahead distance
00031     static float const ld_max = 2000.0; //max purepursuit lookahead distance
00032     static float const trackingLimit = 50.0; //purepursuit will track only until the target distance is greater than this limit
00033 
00034     static float const drive_minV = driveTrain_minV;
00035     static float const drive_maxV = driveTrain_maxV;
00036 
00037     static float const purePursuit_omega_limit = DEG_TO_RAD(90);
00038 
00039     float robotFrame_targetPos[2];
00040 
00041 };
00042 
00043 
00044 #endif