File content as of revision 4:66b2a94a607f:

#include "algo.h"

//Function takes steering value in degrees and returns corresponding turn radius in meters using linear interpolation
//Input = Steering in degrees
//Input is of the form of x , x.3 , x.7 where x is an integer so there is no need of storing this array
//Output array is stored in array
//Output is linearly interpolated

float pid_output = 0;
bool flag_throttle_brake_plausibility = false;

float turnradiusfunction(float steering)
{
    float steering_high,steering_range;
    float radius,radius_low,radius_high,radius_range;

    int x=steering;
    float y=steering-x;
    
    if (y<0.3)
    {
        radius_low=turn_radius[3*x-1];
        radius_high=turn_radius[3*x];
        steering_high=x+0.3;
        steering_range=0.3;
    }
    else if (y<0.6999)
    {
        radius_low=turn_radius[3*x];
        radius_high=turn_radius[3*x+1];
        steering_high=x+0.7;
        steering_range=0.4;
    }
    else 
    {
        radius_low=turn_radius[3*x+1];
        radius_high=turn_radius[3*x+2];
        steering_high=x+1;
        steering_range=0.3;
    }

      radius_range=radius_low-radius_high;
      radius=radius_high+radius_range*(steering_high-steering)/steering_range;
      radius = sqrt(pow(radius,2)-pow(rear_wheelbase,2));                                       //Convert turnradius wrt CG to turnradius wrt center of rear trackwidth      
      return radius; 
}

//Function Desired RPM Ratio from turnradius and applies PID control to error between actual RPM ratio and desired RPM ratio.
//Further evalutes desired throttle left and right values from PID output and throttle input
void pid()
{   
      if (brake > 10 && throttle > 1.5)                                                               // What is max input when brake is NOT actuated???
      {
           flag_throttle_brake_plausibility = true;
           FMEALowByte |= 0x10; //0001 0000
           throttle_Left = 0;
           throttle_Right = 0;           
           return;
      }
      
      if (flag_throttle_brake_plausibility)
      {
           if (throttle > 0.7)
           {
                throttle_Left = 0;
                throttle_Right = 0;           
                return;
           }
           else
           {
                flag_throttle_brake_plausibility = false;
                FMEALowByte &= 0xEF; //1110 1111
           }          
      }
      
      if (((flagsFromICAP & 0x78) && !openloop_driver) || (useBoschSteeringRequest))                                                         //Till driver reads Dash message and takes necessary action - openloop/use_Bosch_steering,do soft shutdown - throttle out = 0
      {
            throttle_Left = 0;
            throttle_Right = 0;
            return;
      }
      
      // if any rpm values leas than 10, make it 10
      if (rpm_Left <= dead_rpm)
      {
          rpm_Left = dead_rpm;
      }
    
      if (rpm_Right <= dead_rpm)
      {
          rpm_Right = dead_rpm;
      }    
   
      // steering angle to turnradius
      if (abs(steering)<7)
      {                                                
        turnradius = 1200;                                                       // if steering input is within 7 degrees, turnradius is inf(1200)
      }
      else
      {      
        turnradius = turnradiusfunction(abs(steering));
      }
   
      //turnradius to RPM ratio desired    
      if(abs(steering) < dead_steering)
      {
          wratio_desired = 1;                                                   //if steering is within dead_steering limit, then no e-diff voltage split
      }
      else if(steering > 0)
      {
          wratio_desired = (turnradius - trackwidth/2)/(turnradius + trackwidth/2);
      }
      else 
      {
          wratio_desired = (turnradius + trackwidth/2)/(turnradius - trackwidth/2);
      }    
    
      //pid calculations
      
      if (!openloop_lowrpm && !openloop_driver)
      {
          wratio_actual = rpm_Right/rpm_Left;
          
          error_new = wratio_desired - wratio_actual;                                  //error
          derivative = (error_new-error_prev)/timeint;                                 //derivative of error            
          if(integral < integral_saturation)                                           //saturate the integral part 
          {
              integral += error_new*timeint;                                           //integral of error
          }   
          
          pid_output =  kp*error_new + ki*integral + kd*derivative + c;               //PID output
     
          error_prev = error_new;
      }
      else
      {
          pid_output = 0;
      }
      w_ratio = pid_output + wratio_desired;                                        //w ratio desired after feedback
        
      throttle_Right = 2*throttle/(1+1/w_ratio);                                    // V1, V2 from throttle input and w ratio desired after feedback
      throttle_Left = 2*throttle/(1+w_ratio);
       
      
      //Incase either V1 or V2 desired exceeds full_throttle, saturate it to full_throttle - in this case desired ratio will not be maintained
      if(throttle_Right > full_throttle)
      {
          throttle_Right = full_throttle;
      } 
   
      if (throttle_Left > full_throttle)
      {
          throttle_Left = full_throttle;
      }   
}