Diff: algo.cpp

Revision:

3:68d91895d991

Parent:

0:cee310e0c668

Child:

4:66b2a94a607f

--- a/algo.cpp	Thu Mar 07 21:05:08 2013 +0000
+++ b/algo.cpp	Fri Mar 08 18:58:02 2013 +0000
@@ -7,6 +7,7 @@
 //Output is linearly interpolated
 
 float pid_output = 0;
+bool flag_throttle_brake_plausibility = false;
 
 float turnradiusfunction(float steering)
 {
@@ -48,6 +49,35 @@
 //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;
+           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;
+           }          
+      }
+      
+      if (Dash)                                                         //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)
       {
@@ -108,7 +138,8 @@
         
       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)
       {