File content as of revision 2:eb4101b574bc:

#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;

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 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)
      {
          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;
      }   
}