Team Virgo v3 / Orion_newPCB_test_LV

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 Team Virgo v3

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?

UserRevisionLine numberNew 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 }