pidBearing.cpp

00001 #include "pidBearing.h"
00002 
00003 pidBearing::pidBearing()
00004     {
00005         kp = 1;
00006         kd = 50;
00007         ki = 0.0;
00008     }
00009 
00010 void pidBearing::setSpeedChange(float* wl, float* wr, float* speedChange, float* Error, 
00011                                 float target[2], 
00012                                 float local[2],
00013                                 float yaw, 
00014                                 float* robotFrame, 
00015                                 float xx, float yy, 
00016                                 float* k3,
00017                                 float lGain)
00018     {
00019             bearingToWaypoint = 90 - 180/3.142 * atan2((target[1]-local[1]),(target[0]-local[0])); //angle due X-axis of IMU
00020             
00021             if(bearingToWaypoint>=0.0)
00022                 imuToAchieve = 360 - bearingToWaypoint; //Since YAW decreases in the clockwise direction, if target is to the right IMU to achieve should be 360 minus the bearing to the waypoint
00023             else 
00024                 imuToAchieve = 0 - bearingToWaypoint; //Since YAW decreases in the anticlockwise direction, if target is to the left IMU to acieve should 0 minus (negative sign) bearing to the waypoint
00025                 
00026             offset = yaw - imuToAchieve; //this is to calculate how far off the bot is from it's current IMU yaw to the one it needs to be.
00027             
00028             if (offset >=180)
00029                 angleToRotate = offset - 360; //this is to determine the shortest angle to rotate.
00030             else
00031                 angleToRotate = offset - 0;
00032                 
00033             if(generalFunctions::abs_f(angleToRotate)>180)
00034                 *Error = 360 - generalFunctions::abs_f(angleToRotate);
00035             else
00036                 *Error = angleToRotate;
00037                 
00038             sumError = sumError + *Error;  
00039             *speedChange = kd * (*Error - prevaError) + kp * (*Error) + ki * (sumError);
00040             prevaError = *Error;   
00041             
00042             *robotFrame= sqrt(pow((target[0] - local[0]),2) + pow((target[1] - local[1]),2));
00043 //            robotFrame_targetDistance= sqrt(pow((target_waypoint[0] - current_position[0]),2) + pow((target_waypoint[1] - current_position[1]),2));
00044             
00045             max_in = sqrt(pow((target[0] - xx *10),2) + pow((target[1] - yy*10),2));
00046             minDist = *robotFrame - generalFunctions::abs_f(*Error*8);
00047             if (*robotFrame < 120)
00048                 *k3 = 0.0;
00049             else
00050                 *k3= generalFunctions::abs_f(generalFunctions::map_f(generalFunctions::abs_f(minDist), 0, max_in, 0.0, lGain));
00051                 *k3 = generalFunctions::constrain_f(*k3, 0, lGain);
00052             
00053             
00054             *wl = -(10 + *speedChange)*(*k3);
00055             *wr = -(10 - *speedChange)*(*k3);                
00056     }
00057     
00058 void pidBearing::findRobotFrameDistance(float target[2], float local[2], float* robotFrame) 
00059     {
00060         *robotFrame= sqrt(pow((target[0] - local[0]),2) + pow((target[1] - local[1]),2));
00061     }