Diff: algo.cpp

Revision:

0:cee310e0c668

Child:

3:68d91895d991

diff -r 000000000000 -r cee310e0c668 algo.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/algo.cpp	Thu Mar 07 18:22:38 2013 +0000
@@ -0,0 +1,122 @@
+#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_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;
+      }   
+}
\ No newline at end of file