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.cpp@30:44676e1b38f8, 2018-03-20 (annotated)
- Committer:
- ahmed_lv
- Date:
- Tue Mar 20 05:56:22 2018 +0000
- Revision:
- 30:44676e1b38f8
- Parent:
- 19:7345688260b2
Editted Input Variables to PID
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akashvibhute | 11:49344285c82a | 1 | #include "purePursuit.h" |
akashvibhute | 11:49344285c82a | 2 | |
akashvibhute | 11:49344285c82a | 3 | purePursuit::purePursuit() |
akashvibhute | 11:49344285c82a | 4 | { |
akashvibhute | 11:49344285c82a | 5 | |
akashvibhute | 11:49344285c82a | 6 | } |
akashvibhute | 11:49344285c82a | 7 | |
akashvibhute | 11:49344285c82a | 8 | |
akashvibhute | 11:49344285c82a | 9 | void purePursuit::GenVW(float* purePursuit_velocity, float* purePursuit_omega, |
akashvibhute | 11:49344285c82a | 10 | float target_waypoint[2], float target_velocity, |
akashvibhute | 11:49344285c82a | 11 | float current_position[2], float current_heading) |
akashvibhute | 11:49344285c82a | 12 | { |
akashvibhute | 11:49344285c82a | 13 | //rotate world to robot frame |
akashvibhute | 11:49344285c82a | 14 | robotFrame_targetPos[0] = target_waypoint[0] * cos(current_heading) - target_waypoint[1] * sin(current_heading); |
akashvibhute | 11:49344285c82a | 15 | robotFrame_targetPos[1] = target_waypoint[0] * sin(current_heading) + target_waypoint[1] * cos(current_heading); |
akashvibhute | 11:49344285c82a | 16 | |
akashvibhute | 11:49344285c82a | 17 | //distance to go |
akashvibhute | 11:49344285c82a | 18 | robotFrame_targetDistance= sqrt(pow((target_waypoint[0] - current_position[0]),2) + pow((target_waypoint[1] - current_position[1]),2)); |
akashvibhute | 11:49344285c82a | 19 | |
akashvibhute | 11:49344285c82a | 20 | //target_velocity velocity in mm/s |
akashvibhute | 11:49344285c82a | 21 | |
akashvibhute | 11:49344285c82a | 22 | //pure pursuit look ahead distance |
akashvibhute | 11:49344285c82a | 23 | purePursuit_ld = generalFunctions::map_f(robotFrame_targetDistance, ld_min, ld_max, trackingLimit, ld_max); |
akashvibhute | 11:49344285c82a | 24 | purePursuit_ld = generalFunctions::constrain_f(purePursuit_ld, trackingLimit, ld_max); |
akashvibhute | 11:49344285c82a | 25 | |
akashvibhute | 11:49344285c82a | 26 | //pure-pursuit tracking path radius |
akashvibhute | 11:49344285c82a | 27 | purePursuit_r = pow(purePursuit_ld, 2) / (2 * (robotFrame_targetPos[0] - current_position[0]) ); |
akashvibhute | 11:49344285c82a | 28 | |
akashvibhute | 11:49344285c82a | 29 | //calculate heading error |
akashvibhute | 11:49344285c82a | 30 | purePursuit_headingE = atan2((target_waypoint[1] - current_position[1]) , (target_waypoint[0] - current_position[0])) - (M_PI/2); //frame rotated by pi/2 rad, compilmentry angle about measured from Y axis |
akashvibhute | 11:49344285c82a | 31 | |
akashvibhute | 11:49344285c82a | 32 | //add robot heading to compute global error |
akashvibhute | 11:49344285c82a | 33 | purePursuit_headingE -= current_heading; |
akashvibhute | 11:49344285c82a | 34 | |
akashvibhute | 11:49344285c82a | 35 | //convert angle to 0 to 2*pi |
akashvibhute | 11:49344285c82a | 36 | if(purePursuit_headingE > 2*M_PI) purePursuit_headingE -= 2*M_PI; |
akashvibhute | 11:49344285c82a | 37 | if(purePursuit_headingE < 0) purePursuit_headingE += 2*M_PI; |
akashvibhute | 11:49344285c82a | 38 | |
akashvibhute | 11:49344285c82a | 39 | //convert back heading error into -pi -> +pi from 0 to 2*pi |
akashvibhute | 11:49344285c82a | 40 | if(purePursuit_headingE > M_PI) purePursuit_headingE -= 2*M_PI; |
akashvibhute | 11:49344285c82a | 41 | |
akashvibhute | 11:49344285c82a | 42 | if((generalFunctions::abs_f(purePursuit_headingE) >= M_PI/2) && (robotFrame_targetDistance > trackingLimit)) { //pure pursuit-implementation requires heading error to be under +/- 90 degree to work efficiently, lets bring it down to within 80 deg using zero turn radius |
akashvibhute | 11:49344285c82a | 43 | //rotate towards the target heading on the spot |
akashvibhute | 11:49344285c82a | 44 | *purePursuit_velocity = 150.0;//50.0; |
akashvibhute | 11:49344285c82a | 45 | *purePursuit_omega = generalFunctions::sign_f(purePursuit_headingE)*1.0*M_PI; |
akashvibhute | 11:49344285c82a | 46 | } |
akashvibhute | 11:49344285c82a | 47 | |
akashvibhute | 11:49344285c82a | 48 | else { |
akashvibhute | 11:49344285c82a | 49 | if(robotFrame_targetDistance < trackingLimit) { |
akashvibhute | 11:49344285c82a | 50 | *purePursuit_velocity = 0; |
akashvibhute | 11:49344285c82a | 51 | *purePursuit_omega = 0; |
akashvibhute | 11:49344285c82a | 52 | } |
akashvibhute | 11:49344285c82a | 53 | |
akashvibhute | 11:49344285c82a | 54 | if(robotFrame_targetDistance >= trackingLimit) { |
akashvibhute | 11:49344285c82a | 55 | //regulate velocity depending on distance to be travelled |
akashvibhute | 11:49344285c82a | 56 | *purePursuit_velocity = generalFunctions::map_f(robotFrame_targetDistance, trackingLimit, ld_max, drive_minV, target_velocity); |
akashvibhute | 11:49344285c82a | 57 | *purePursuit_velocity = generalFunctions::constrain_f(*purePursuit_velocity, drive_minV, drive_maxV); |
akashvibhute | 11:49344285c82a | 58 | |
akashvibhute | 11:49344285c82a | 59 | //omega calculated from generated path radius and command velocity |
akashvibhute | 11:49344285c82a | 60 | //*purePursuit_omega = *purePursuit_velocity / purePursuit_r; |
akashvibhute | 11:49344285c82a | 61 | |
akashvibhute | 11:49344285c82a | 62 | |
akashvibhute | 11:49344285c82a | 63 | //omega calculated from heading error and command velocity |
akashvibhute | 11:49344285c82a | 64 | *purePursuit_omega = 10.0*purePursuit_headingE * (*purePursuit_velocity / robotFrame_targetDistance); // for very large command distances, small heading errors cause huge amplification on omega, and sending the robot into oscillations. Hence an adaptive scaler to check this condition. |
akashvibhute | 11:49344285c82a | 65 | |
akashvibhute | 11:49344285c82a | 66 | |
akashvibhute | 11:49344285c82a | 67 | //check to see if omega calculated is too high |
akashvibhute | 11:49344285c82a | 68 | if(generalFunctions::abs_f(*purePursuit_omega) > purePursuit_omega_limit) { |
akashvibhute | 11:49344285c82a | 69 | *purePursuit_velocity= *purePursuit_velocity * (purePursuit_omega_limit/generalFunctions::abs_f(*purePursuit_omega)); //scale speed to keep omega within normal limits |
akashvibhute | 11:49344285c82a | 70 | //purePursuit_omega = 1.0*purePursuit_headingE * (*purePursuit_velocity / robotFrame_targetDistance); |
akashvibhute | 11:49344285c82a | 71 | } |
akashvibhute | 11:49344285c82a | 72 | } |
akashvibhute | 11:49344285c82a | 73 | } |
akashvibhute | 19:7345688260b2 | 74 | } |
akashvibhute | 19:7345688260b2 | 75 | |
akashvibhute | 19:7345688260b2 | 76 | |
akashvibhute | 19:7345688260b2 | 77 | void purePursuit:: GenVGW(float* purePursuit_velocity, float* purePursuit_gamma, float* purePursuit_omega, |
akashvibhute | 19:7345688260b2 | 78 | float target_waypoint[2], float target_velocity, |
akashvibhute | 19:7345688260b2 | 79 | float current_position[2], float current_heading) |
akashvibhute | 19:7345688260b2 | 80 | { |
akashvibhute | 19:7345688260b2 | 81 | float gamma_out; |
akashvibhute | 19:7345688260b2 | 82 | float gamma_error; |
akashvibhute | 19:7345688260b2 | 83 | |
akashvibhute | 19:7345688260b2 | 84 | //distance to go |
akashvibhute | 19:7345688260b2 | 85 | robotFrame_targetDistance= sqrt(pow((target_waypoint[0] - current_position[0]),2) + pow((target_waypoint[1] - current_position[1]),2)); |
akashvibhute | 19:7345688260b2 | 86 | |
akashvibhute | 19:7345688260b2 | 87 | //target_velocity velocity in mm/s |
akashvibhute | 19:7345688260b2 | 88 | |
akashvibhute | 19:7345688260b2 | 89 | //pure pursuit look ahead distance |
akashvibhute | 19:7345688260b2 | 90 | purePursuit_ld = generalFunctions::map_f(robotFrame_targetDistance, ld_min, ld_max, trackingLimit, ld_max); |
akashvibhute | 19:7345688260b2 | 91 | purePursuit_ld = generalFunctions::constrain_f(purePursuit_ld, trackingLimit, ld_max); |
akashvibhute | 19:7345688260b2 | 92 | |
akashvibhute | 19:7345688260b2 | 93 | |
akashvibhute | 19:7345688260b2 | 94 | //calculate global heading to track, frame needs to be rotated by pi/2 rad for compilmentry angle about measured from Y axis |
akashvibhute | 19:7345688260b2 | 95 | gamma_out = atan2((target_waypoint[1] - current_position[1]) , (target_waypoint[0] - current_position[0])) - (M_PI/2); |
akashvibhute | 19:7345688260b2 | 96 | |
akashvibhute | 19:7345688260b2 | 97 | //convert angle to 0 to 2*pi |
akashvibhute | 19:7345688260b2 | 98 | if(gamma_out > 2*M_PI) gamma_out -= 2*M_PI; |
akashvibhute | 19:7345688260b2 | 99 | if(gamma_out < 0) gamma_out += 2*M_PI; |
akashvibhute | 19:7345688260b2 | 100 | |
akashvibhute | 19:7345688260b2 | 101 | ////convert back heading error into -pi -> +pi from 0 to 2*pi |
akashvibhute | 19:7345688260b2 | 102 | //if(gamma_out > M_PI) gamma_out -= 2*M_PI; |
akashvibhute | 19:7345688260b2 | 103 | |
akashvibhute | 19:7345688260b2 | 104 | gamma_error = gamma_out - current_heading; |
akashvibhute | 19:7345688260b2 | 105 | //convert heading error into -pi -> +pi |
akashvibhute | 19:7345688260b2 | 106 | if(gamma_error < -1*M_PI) gamma_error += 2*M_PI; |
akashvibhute | 19:7345688260b2 | 107 | if(gamma_error > M_PI) gamma_error -= 2*M_PI; |
akashvibhute | 19:7345688260b2 | 108 | |
akashvibhute | 19:7345688260b2 | 109 | |
akashvibhute | 19:7345688260b2 | 110 | if(robotFrame_targetDistance < trackingLimit) { |
akashvibhute | 19:7345688260b2 | 111 | *purePursuit_velocity = 0; |
akashvibhute | 19:7345688260b2 | 112 | } |
akashvibhute | 19:7345688260b2 | 113 | |
akashvibhute | 19:7345688260b2 | 114 | else { |
akashvibhute | 19:7345688260b2 | 115 | //regulate velocity depending on distance to be travelled |
akashvibhute | 19:7345688260b2 | 116 | *purePursuit_velocity = generalFunctions::map_f(robotFrame_targetDistance, trackingLimit, ld_max, drive_minV, target_velocity); |
akashvibhute | 19:7345688260b2 | 117 | *purePursuit_velocity = generalFunctions::constrain_f(*purePursuit_velocity, drive_minV, drive_maxV); |
akashvibhute | 19:7345688260b2 | 118 | |
akashvibhute | 19:7345688260b2 | 119 | //omega calculated from heading error and command velocity |
akashvibhute | 19:7345688260b2 | 120 | *purePursuit_omega = 1.0*(gamma_error) * (*purePursuit_velocity / robotFrame_targetDistance); // for very large command distances, small heading errors cause huge amplification on omega, and sending the robot into oscillations. Hence an adaptive scaler to check this condition. |
akashvibhute | 19:7345688260b2 | 121 | |
akashvibhute | 19:7345688260b2 | 122 | |
akashvibhute | 19:7345688260b2 | 123 | //check to see if omega calculated is too high |
akashvibhute | 19:7345688260b2 | 124 | if(generalFunctions::abs_f(*purePursuit_omega) > purePursuit_omega_limit) { |
akashvibhute | 19:7345688260b2 | 125 | *purePursuit_velocity= *purePursuit_velocity * (purePursuit_omega_limit/generalFunctions::abs_f(*purePursuit_omega)); //scale speed to keep omega within normal limits |
akashvibhute | 19:7345688260b2 | 126 | } |
akashvibhute | 19:7345688260b2 | 127 | |
akashvibhute | 19:7345688260b2 | 128 | //convert angle to 0 to 2*pi |
akashvibhute | 19:7345688260b2 | 129 | if(gamma_out > 2*M_PI) gamma_out -= 2*M_PI; |
akashvibhute | 19:7345688260b2 | 130 | if(gamma_out < 0) gamma_out += 2*M_PI; |
akashvibhute | 19:7345688260b2 | 131 | |
akashvibhute | 19:7345688260b2 | 132 | *purePursuit_gamma = gamma_out; |
akashvibhute | 19:7345688260b2 | 133 | } |
akashvibhute | 11:49344285c82a | 134 | } |