File content as of revision 30:44676e1b38f8:

#include "pidBearing.h"

pidBearing::pidBearing()
    {
        kp = 1;
        kd = 50;
        ki = 0.0;
    }

void pidBearing::setSpeedChange(float* wl, float* wr, float* speedChange, float* Error, 
                                float target[2], 
                                float local[2],
                                float yaw, 
                                float* robotFrame, 
                                float xx, float yy, 
                                float* k3,
                                float lGain)
    {
            bearingToWaypoint = 90 - 180/3.142 * atan2((target[1]-local[1]),(target[0]-local[0])); //angle due X-axis of IMU
            
            if(bearingToWaypoint>=0.0)
                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
            else 
                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
                
            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.
            
            if (offset >=180)
                angleToRotate = offset - 360; //this is to determine the shortest angle to rotate.
            else
                angleToRotate = offset - 0;
                
            if(generalFunctions::abs_f(angleToRotate)>180)
                *Error = 360 - generalFunctions::abs_f(angleToRotate);
            else
                *Error = angleToRotate;
                
            sumError = sumError + *Error;  
            *speedChange = kd * (*Error - prevaError) + kp * (*Error) + ki * (sumError);
            prevaError = *Error;   
            
            *robotFrame= sqrt(pow((target[0] - local[0]),2) + pow((target[1] - local[1]),2));
//            robotFrame_targetDistance= sqrt(pow((target_waypoint[0] - current_position[0]),2) + pow((target_waypoint[1] - current_position[1]),2));
            
            max_in = sqrt(pow((target[0] - xx *10),2) + pow((target[1] - yy*10),2));
            minDist = *robotFrame - generalFunctions::abs_f(*Error*8);
            if (*robotFrame < 120)
                *k3 = 0.0;
            else
                *k3= generalFunctions::abs_f(generalFunctions::map_f(generalFunctions::abs_f(minDist), 0, max_in, 0.0, lGain));
                *k3 = generalFunctions::constrain_f(*k3, 0, lGain);
            
            
            *wl = -(10 + *speedChange)*(*k3);
            *wr = -(10 - *speedChange)*(*k3);                
    }
    
void pidBearing::findRobotFrameDistance(float target[2], float local[2], float* robotFrame) 
    {
        *robotFrame= sqrt(pow((target[0] - local[0]),2) + pow((target[1] - local[1]),2));
    }