Implement new controller
Dependencies: mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo
Fork of Orion_newPCB_test by
05_TrajectoryTracking/purePursuit.h@19:7345688260b2, 2016-06-07 (annotated)
- Committer:
- akashvibhute
- Date:
- Tue Jun 07 03:48:56 2016 +0000
- Revision:
- 19:7345688260b2
- Parent:
- 11:49344285c82a
working code to control speedster under SMC with waypoint tracking
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akashvibhute | 11:49344285c82a | 1 | #ifndef purePursuit_H |
akashvibhute | 11:49344285c82a | 2 | #define purePursuit_H |
akashvibhute | 11:49344285c82a | 3 | |
akashvibhute | 11:49344285c82a | 4 | #include "mbed.h" |
akashvibhute | 11:49344285c82a | 5 | #include "generalFunctions.h" |
akashvibhute | 11:49344285c82a | 6 | #include "config.h" |
akashvibhute | 11:49344285c82a | 7 | |
akashvibhute | 11:49344285c82a | 8 | class purePursuit |
akashvibhute | 11:49344285c82a | 9 | { |
akashvibhute | 11:49344285c82a | 10 | public: |
akashvibhute | 11:49344285c82a | 11 | |
akashvibhute | 11:49344285c82a | 12 | purePursuit(); |
akashvibhute | 19:7345688260b2 | 13 | |
akashvibhute | 19:7345688260b2 | 14 | /// generate velocity (mm/s) and omega (rad/s) for diff drive to track |
akashvibhute | 11:49344285c82a | 15 | void GenVW(float* purePursuit_velocity, float* purePursuit_omega, |
akashvibhute | 19:7345688260b2 | 16 | float target_waypoint[2], float target_velocity, |
akashvibhute | 11:49344285c82a | 17 | float current_position[2], float current_heading); |
akashvibhute | 19:7345688260b2 | 18 | |
akashvibhute | 19:7345688260b2 | 19 | /// generate velocity (mm/s), gamma (rad) and omega (rad/s) for non-linear controller to track |
akashvibhute | 19:7345688260b2 | 20 | void GenVGW(float* purePursuit_velocity, float* purePursuit_gamma, float* purePursuit_omega, |
akashvibhute | 19:7345688260b2 | 21 | float target_waypoint[2], float target_velocity, |
akashvibhute | 19:7345688260b2 | 22 | float current_position[2], float current_heading); |
akashvibhute | 19:7345688260b2 | 23 | |
akashvibhute | 19:7345688260b2 | 24 | float robotFrame_targetDistance; |
akashvibhute | 11:49344285c82a | 25 | float purePursuit_headingE; |
akashvibhute | 11:49344285c82a | 26 | float purePursuit_ld; |
akashvibhute | 19:7345688260b2 | 27 | float purePursuit_r; |
akashvibhute | 11:49344285c82a | 28 | |
akashvibhute | 11:49344285c82a | 29 | private: |
akashvibhute | 19:7345688260b2 | 30 | static float const ld_min = 200.0; //min purepursuit lookahead distance |
akashvibhute | 19:7345688260b2 | 31 | static float const ld_max = 2000.0; //max purepursuit lookahead distance |
akashvibhute | 19:7345688260b2 | 32 | static float const trackingLimit = 50.0; //purepursuit will track only until the target distance is greater than this limit |
akashvibhute | 19:7345688260b2 | 33 | |
akashvibhute | 11:49344285c82a | 34 | static float const drive_minV = driveTrain_minV; |
akashvibhute | 11:49344285c82a | 35 | static float const drive_maxV = driveTrain_maxV; |
akashvibhute | 19:7345688260b2 | 36 | |
akashvibhute | 11:49344285c82a | 37 | static float const purePursuit_omega_limit = DEG_TO_RAD(90); |
akashvibhute | 19:7345688260b2 | 38 | |
akashvibhute | 11:49344285c82a | 39 | float robotFrame_targetPos[2]; |
akashvibhute | 19:7345688260b2 | 40 | |
akashvibhute | 11:49344285c82a | 41 | }; |
akashvibhute | 11:49344285c82a | 42 | |
akashvibhute | 11:49344285c82a | 43 | |
akashvibhute | 11:49344285c82a | 44 | #endif |