Diff: main.cpp

Revision:

0:30ff725706d2

Child:

1:fec1d091fa34

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 05 01:19:30 2013 +0000
@@ -0,0 +1,338 @@
+//HIL+fmea
+//Last updated 04/03/2013
+#include "algo.h"
+
+// - - - Parameters - - -
+
+//car parameters
+const float trackwidth = 49*0.0254;
+const float wheelradius = 0.254;
+const float rear_wheelbase = 0.686816;                                   //rear weightbias*wheelbase
+
+//constant parameters
+const float full_throttle=5;
+const float dead_steering=7;
+const float integral_saturation=0.5;
+const float dead_rpm = 10;
+
+// - - - Inputs - - -
+
+//steering and throttle inputs
+float steering;                                                         //input steering wheel angle in degrees
+float throttle;                                                         //throttle input from pedal
+
+// - - - Outputs - - -
+
+// throttle output values
+float throttle_Left, throttle_Right, throttle_Left_pulsewidth, throttle_Right_pulsewidth;
+
+// pwm out functions (pins 21 and 22)
+PwmOut throttle_Left_pwmout(throttle_Left_pwm_pin);
+PwmOut throttle_Right_pwmout(throttle_Right_pwm_pin);
+
+// - - - For Basic Algo and Steering to turnradius function - - -
+
+//Ticker for algo interrupt
+Ticker for_algo;
+
+//algo function, PID funtion
+void ediff_func();
+void pid(void);
+
+//delta_Left, delta_Right, turnradius and wratio_desired calculated realtime from delta (delta_Left and delta_Right are Left and Right tyre angles)
+float delta_Left;                                                       //Left wheel Steer angle
+float delta_Right;                                                      //Right wheel Steer angle
+float turnradius;
+float wratio_desired;                                                   //Right/Left
+
+//PID constants
+float kp=0;
+float kd=0;
+float ki=0;
+
+//For PID
+const float timeint = 0.1;
+float integral;
+float derivative;
+float error_prev = 0;     //error at t0
+float error_new;          //error at t
+float wratio_actual;
+float w_ratio;            //ratio of controller voltages - Right/Left
+
+//virtual sensors
+float steering2, throttle2, yaw_rate;
+float yaw_rate_desired;
+
+// - - - For RPM Measurement - - -
+
+//rpm counts
+volatile int cnt_Left = 0, cnt_Right = 0;
+
+//Left and Right rear RPMs feedback-input 
+float rpm_Left;
+float rpm_Right;
+
+//RPM feedback pins
+AnalogIn steeringread(p16);
+AnalogIn throttleread(p17);
+
+// take interrupts (pins 25 and 23)
+InterruptIn wheel_Left_intr(wheel_Left_pin);
+InterruptIn wheel_Right_intr(wheel_Right_pin);
+
+//RPM count function
+void wheel_Left_interrupt(void);
+void wheel_Right_interrupt(void);
+
+//Timer for RPM measurement
+Timer rpm_timer;
+
+//Ticker (for RPM measurement Timer Reset) and corresponding Reset Function
+Ticker ForTimerReset;
+void TimerReset();
+
+//RPM Measurement Timer Readings for Current/Prev pulse of Left/Right Wheel Sensor
+volatile int Current_time_Left = 0,Prev_time_Left = 0;
+volatile int Current_time_Right = 0,Prev_time_Right = 0;
+
+//Difference between Timer Readings for consecutive pulses i.e. time for 1 revolution 
+volatile float time_elapsed_Left;
+volatile float time_elapsed_Right;
+
+// - - - FOR FMEA - - - 
+
+//FMEA output
+bool shutdown= false;                                                           //Shutdown Car
+bool Dash = false;                                                              //Display Message on Dash - give option to driver to switch to openloop
+
+//Display Indicators for FMEA testing
+volatile bool displayLeftrpm = false;
+volatile bool displayDash = false;
+volatile bool displayLeftdiff = false;
+volatile bool displayRightrpm = false;
+volatile bool displayRightdiff = false;
+
+//For Missing Teeth RPM FMEA
+
+//Pulse Time measurement variables
+volatile int CurrentLeftMissingTeethTimeElapsed=0, PrevLeftMissingTeethTimeElapsed = 0, 
+CurrentDiffLeftMissingTeethTimeElapsed = 0,PrevDiffLeftMissingTeethTimeElapsed = 0;
+
+volatile int CurrentRightMissingTeethTimeElapsed=0, PrevRightMissingTeethTimeElapsed = 0, 
+CurrentDiffRightMissingTeethTimeElapsed = 0,PrevDiffRightMissingTeethTimeElapsed = 0;
+
+/*
+
+(Current/Prev)(Left/Right)MissingTeethTimeElapsed       -   Timer reading for current/previous pulse of Left/Right RPM pulses
+(Current/Prev)Diff(Left/Right)MissingTeethTimeElapsed   -   Difference between timer readings of consecutive pulse 
+                                                            i.e. width of current/prev pulse of of Left/Right RPM pulses  
+*/
+
+//Flags for starting Left/Right Missing Teeth RPM FMEA
+volatile bool MissingTeethCheckRight = false,MissingTeethCheckLeft = true;
+volatile bool ToggleMissingTeethCheck = false;
+
+//Missing Teeth Count
+int countLeftMissingTeeth = 0,countRightMissingTeeth = 0; 
+
+//Ratio between times of consecutive pulses - to check for missing teeth - (will be >=2 in case of missing teeth)
+float ratio;
+
+//Missing Teeth FMEA Call Function and corresponding Ticker
+
+Ticker ForFmeaMissingTeeth; 
+void FmeaMissingTeeth();
+
+// - - - Function Definitions - - -
+
+int main()
+{   
+    for_algo.attach_us(&ediff_func,(algoperiod));                   //call ediff func every "algoperiod" sec  
+    ForFmeaMissingTeeth.attach(&FmeaMissingTeeth, 5.0);             //call Missing Teeth RPM FMEA every 5 sec
+    ForTimerReset.attach(&TimerReset,100);                          //Reset RPM measurement timer every 100 sec
+     
+    //RPM attach functions on rise value
+    wheel_Left_intr.rise(&wheel_Left_interrupt);
+    wheel_Right_intr.rise(&wheel_Right_interrupt);
+    
+    //start the wheel rom timers
+    rpm_timer.start();
+   
+    while(1)
+    {
+        if (displayLeftrpm)
+        {
+            printf("time_elapsed_Left %f \n",time_elapsed_Left);
+            displayLeftrpm = false;
+        }
+        if (displayLeftdiff)
+        {
+            printf("CurrentDiffLeftMissingTeethTimeElapsed %d \n",CurrentDiffLeftMissingTeethTimeElapsed);
+            displayLeftdiff = false;
+        }
+        if (displayRightrpm)
+        {
+            printf("time_elapsed_Right %f \n",time_elapsed_Right);
+            displayRightrpm = false;
+        }
+        if (displayRightdiff)
+        {
+            printf("CurrentDiffRightMissingTeethTimeElapsed %d \n",CurrentDiffRightMissingTeethTimeElapsed);
+            displayRightdiff = false;
+        }  
+        if (displayDash)
+        {
+            printf("Hole is missing - move to open loop %f\n", ratio);
+            displayDash = false;
+        }
+    }
+}
+
+//Interrupt Function for measuring Left Wheel RPM and checking Left Sensor Disk Missing Teeth FMEA
+void wheel_Left_interrupt()
+{
+    cnt_Left++;                                                         //Left RPM pulse count
+ 
+    if(cnt_Left >= 12)                                                  //Measure time between 12 holes => 1 revolution
+    { 
+        Current_time_Left = rpm_timer.read_ms();                        //Read Current Timer Reading
+        time_elapsed_Left = Current_time_Left - Prev_time_Left;         //Difference between Current and precious Timer readings - time for 1 revolution
+        Prev_time_Left = Current_time_Left;                             // Set Current Reading as previous reading for next iteration
+        cnt_Left = 0;                                                   //Reset Count
+        rpm_Left = 1 / ( time_elapsed_Left  / (1000000*60) );           //Evaluate RPM from time for 1 revolution
+        displayLeftrpm = true;                                          //Display RPM - for testing purposes only
+    }
+    
+    if (MissingTeethCheckLeft)                                          //Called every 10 sec
+    {
+         countLeftMissingTeeth++;        
+         if (countLeftMissingTeeth == 1)                                //No comparisons for first tooth ( 12th is compared with first)
+         {
+            PrevLeftMissingTeethTimeElapsed = rpm_timer.read_ms();      // Set Current Timer Reading as previous reading for next iteration
+         }
+         else  
+         {
+            CurrentLeftMissingTeethTimeElapsed = rpm_timer.read_ms();    //Read Current Timer Reading
+            
+            CurrentDiffLeftMissingTeethTimeElapsed = CurrentLeftMissingTeethTimeElapsed - PrevLeftMissingTeethTimeElapsed;  
+                                                                         //Difference between Current and precious Timer readings - current pulse time
+            PrevLeftMissingTeethTimeElapsed = CurrentLeftMissingTeethTimeElapsed;                          
+                                                                         //Set Current Timer Reading as previous reading for next iteration
+            ratio = ((float)CurrentDiffLeftMissingTeethTimeElapsed/PrevDiffLeftMissingTeethTimeElapsed);    
+                                                                         //Ratio between consecutive pulses
+            
+            if (ratio > 1.5 || ratio < 1/1.5)                            //If teeth are missing ratio will be >=2
+            {
+                Dash = true;
+                displayDash = true;
+            }
+            
+            PrevDiffLeftMissingTeethTimeElapsed = CurrentDiffLeftMissingTeethTimeElapsed;                 
+                                                                         //Set Current pulse time as previous pulse time for next pulse
+            displayLeftdiff = true;
+         }
+        
+        if (countLeftMissingTeeth > 13)                                  //Stop once all (12) teeth have been checked
+        {
+            MissingTeethCheckLeft = false;
+            countLeftMissingTeeth = 0;
+        }
+    }
+}
+
+//Interrupt Function for measuring Right Wheel RPM and checking Right Sensor Disk Missing Teeth FMEA
+void wheel_Right_interrupt()
+{
+    cnt_Right++;                                                          //Right RPM pulse count
+ 
+    if(cnt_Right >= 12)                                                   //Measure time between 12 holes => 1 revolution
+    { 
+        Current_time_Right = rpm_timer.read_ms();                         //Read Current Timer Reading
+        time_elapsed_Right = Current_time_Right - Prev_time_Right;        //Difference between Current and precious Timer readings - time for 1 revolution
+        Prev_time_Right = Current_time_Right;                             // Set Current Reading as previous reading for next iteration
+        cnt_Right = 0;                                                    //Reset Count
+        rpm_Right = 1 / ( time_elapsed_Right  / (1000000*60) );           //Evaluate RPM from time for 1 revolution
+        displayRightrpm = true;                                           //Evaluate RPM from time for 1 revolution
+     }
+    
+    if (MissingTeethCheckRight)                                           //Called every 10 sec
+    {
+         countRightMissingTeeth++;        
+         if (countRightMissingTeeth == 1)                                 //No comparisons for first tooth ( 12th is compared with first)
+         {
+            PrevRightMissingTeethTimeElapsed = rpm_timer.read_ms();      //Set Current Timer Reading as previous reading for next iteration
+         }
+         else  
+         {
+            CurrentRightMissingTeethTimeElapsed = rpm_timer.read_ms();    //Read Current Timer Reading  
+            CurrentDiffRightMissingTeethTimeElapsed = CurrentRightMissingTeethTimeElapsed - PrevRightMissingTeethTimeElapsed;  
+                                                                          //Difference between Current and precious Timer readings - current pulse time
+            PrevRightMissingTeethTimeElapsed = CurrentRightMissingTeethTimeElapsed;
+                                                                          //Set Current Timer Reading as previous reading for next iteration
+                                                                          
+            ratio = ((float)CurrentDiffRightMissingTeethTimeElapsed/PrevDiffRightMissingTeethTimeElapsed);
+                                                                          //Ratio between consecutive pulses
+            
+            if (ratio > 1.5 || ratio < 1/1.5)                             //If teeth are missing ratio will be >=2
+            {
+                Dash = true;
+                displayDash = true;
+            }
+            
+            PrevDiffRightMissingTeethTimeElapsed = CurrentDiffRightMissingTeethTimeElapsed;
+                                                                         //Set Current pulse time as previous pulse time for next pulse
+            displayRightdiff = true;
+         }
+         
+        if (countRightMissingTeeth > 13)                                 //Stop once all (12) teeth have been checked
+        {
+            MissingTeethCheckRight = false;
+            countRightMissingTeeth = 0;
+        }
+    }
+}
+
+//Function reads stearing and throttle input, calls PID Function and gives Throttle Left and Right PWM outputs
+void ediff_func()    
+{    
+    steeringread.read();
+    throttleread.read();                                                //read throttle, steering
+    
+    throttle=throttleread*5;                                            //calibration : throttle in 1 to 5, stering -110 to 110 degrees
+    steering=220*steeringread-110;
+
+    pid();                                                              // call pid function which gives v1, v2 output
+    
+    //output pwm for Left wheel   
+    throttle_Left_pulsewidth = rpm_Left;
+    throttle_Left_pwmout.pulsewidth_us(throttle_Left_pulsewidth);
+    throttle_Left_pwmout.period_us(1000);
+    
+    //output pwm for Right wheel
+    throttle_Right_pulsewidth = rpm_Right;
+    throttle_Right_pwmout.pulsewidth_us(throttle_Right_pulsewidth);
+    throttle_Right_pwmout.period_us(1000);    
+}
+
+//Resets timer for RPM measurement every 100 sec
+void TimerReset()
+{
+    rpm_timer.stop();
+    rpm_timer.reset();
+    rpm_timer.start();
+}
+
+// Sets Flag for starting Left/Right Missing Teeth FMEA after every 5 sec, alternatively
+void FmeaMissingTeeth()
+{
+    if (ToggleMissingTeethCheck)
+    {
+        MissingTeethCheckRight = true;
+    }    
+    else
+    {
+        MissingTeethCheckLeft = true;
+    }    
+    ToggleMissingTeethCheck = ~ToggleMissingTeethCheck;
+}
+