File content as of revision 1:fec1d091fa34:

//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;
float c=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,ratio1;

//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, %f\n",rpm_Left,time_elapsed_Left);
            displayLeftrpm = false;
        }
        /*
        if (displayLeftdiff)
        {
            printf("CurrentDiffLeftMissingTeethTimeElapsed %d \n",CurrentDiffLeftMissingTeethTimeElapsed);
            displayLeftdiff = false;
        }
        */
        if (displayRightrpm)
        {
            printf("time_elapsed_Right %f, %f\n",rpm_Right,time_elapsed_Right);
            displayRightrpm = false;
        }
        /*
        if (displayRightdiff)
        {
            printf("CurrentDiffRightMissingTeethTimeElapsed %d \n",CurrentDiffRightMissingTeethTimeElapsed);
            displayRightdiff = false;
        } 
        */ 
        if (displayDash)
        {
            printf("\n \n HOLE MISSING OPENLOOP %f \n \n", ratio1);
            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_us();                        //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
            {
                ratio1 = ratio;
                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_us();                         //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;
    steering=220*steeringread-110;                                                //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 = throttle_Left*1000/5;
    throttle_Left_pwmout.pulsewidth_us(throttle_Left_pulsewidth);
    throttle_Left_pwmout.period_us(1000);
    
    //output pwm for Right wheel
    throttle_Right_pulsewidth = throttle_Right*1000/5;
    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;
}