ediff iitb / Mbed 2 deprecated HIL_FMEA_oursteering

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:fec1d091fa34, 2013-03-06 (annotated)

Committer:
ediff_iitbracing
Date:
Wed Mar 06 13:58:03 2013 +0000
Revision:
1:fec1d091fa34
Parent:
0:30ff725706d2
Child:
2:eb4101b574bc
06-03-2013, 19:29 BackUp

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ediff_iitbracing 0:30ff725706d2 1 //HIL+fmea
ediff_iitbracing 0:30ff725706d2 2 //Last updated 04/03/2013
ediff_iitbracing 0:30ff725706d2 3 #include "algo.h"
ediff_iitbracing 0:30ff725706d2 4
ediff_iitbracing 0:30ff725706d2 5 // - - - Parameters - - -
ediff_iitbracing 0:30ff725706d2 6
ediff_iitbracing 0:30ff725706d2 7 //car parameters
ediff_iitbracing 0:30ff725706d2 8 const float trackwidth = 49*0.0254;
ediff_iitbracing 0:30ff725706d2 9 const float wheelradius = 0.254;
ediff_iitbracing 0:30ff725706d2 10 const float rear_wheelbase = 0.686816; //rear weightbias*wheelbase
ediff_iitbracing 0:30ff725706d2 11
ediff_iitbracing 0:30ff725706d2 12 //constant parameters
ediff_iitbracing 0:30ff725706d2 13 const float full_throttle=5;
ediff_iitbracing 0:30ff725706d2 14 const float dead_steering=7;
ediff_iitbracing 0:30ff725706d2 15 const float integral_saturation=0.5;
ediff_iitbracing 0:30ff725706d2 16 const float dead_rpm = 10;
ediff_iitbracing 0:30ff725706d2 17
ediff_iitbracing 0:30ff725706d2 18 // - - - Inputs - - -
ediff_iitbracing 0:30ff725706d2 19
ediff_iitbracing 0:30ff725706d2 20 //steering and throttle inputs
ediff_iitbracing 0:30ff725706d2 21 float steering; //input steering wheel angle in degrees
ediff_iitbracing 0:30ff725706d2 22 float throttle; //throttle input from pedal
ediff_iitbracing 0:30ff725706d2 23
ediff_iitbracing 0:30ff725706d2 24 // - - - Outputs - - -
ediff_iitbracing 0:30ff725706d2 25
ediff_iitbracing 0:30ff725706d2 26 // throttle output values
ediff_iitbracing 0:30ff725706d2 27 float throttle_Left, throttle_Right, throttle_Left_pulsewidth, throttle_Right_pulsewidth;
ediff_iitbracing 0:30ff725706d2 28
ediff_iitbracing 0:30ff725706d2 29 // pwm out functions (pins 21 and 22)
ediff_iitbracing 0:30ff725706d2 30 PwmOut throttle_Left_pwmout(throttle_Left_pwm_pin);
ediff_iitbracing 0:30ff725706d2 31 PwmOut throttle_Right_pwmout(throttle_Right_pwm_pin);
ediff_iitbracing 0:30ff725706d2 32
ediff_iitbracing 0:30ff725706d2 33 // - - - For Basic Algo and Steering to turnradius function - - -
ediff_iitbracing 0:30ff725706d2 34
ediff_iitbracing 0:30ff725706d2 35 //Ticker for algo interrupt
ediff_iitbracing 0:30ff725706d2 36 Ticker for_algo;
ediff_iitbracing 0:30ff725706d2 37
ediff_iitbracing 0:30ff725706d2 38 //algo function, PID funtion
ediff_iitbracing 0:30ff725706d2 39 void ediff_func();
ediff_iitbracing 0:30ff725706d2 40 void pid(void);
ediff_iitbracing 0:30ff725706d2 41
ediff_iitbracing 0:30ff725706d2 42 //delta_Left, delta_Right, turnradius and wratio_desired calculated realtime from delta (delta_Left and delta_Right are Left and Right tyre angles)
ediff_iitbracing 0:30ff725706d2 43 float delta_Left; //Left wheel Steer angle
ediff_iitbracing 0:30ff725706d2 44 float delta_Right; //Right wheel Steer angle
ediff_iitbracing 0:30ff725706d2 45 float turnradius;
ediff_iitbracing 0:30ff725706d2 46 float wratio_desired; //Right/Left
ediff_iitbracing 0:30ff725706d2 47
ediff_iitbracing 0:30ff725706d2 48 //PID constants
ediff_iitbracing 0:30ff725706d2 49 float kp=0;
ediff_iitbracing 0:30ff725706d2 50 float kd=0;
ediff_iitbracing 0:30ff725706d2 51 float ki=0;
ediff_iitbracing 1:fec1d091fa34 52 float c=0;
ediff_iitbracing 0:30ff725706d2 53
ediff_iitbracing 0:30ff725706d2 54 //For PID
ediff_iitbracing 0:30ff725706d2 55 const float timeint = 0.1;
ediff_iitbracing 0:30ff725706d2 56 float integral;
ediff_iitbracing 0:30ff725706d2 57 float derivative;
ediff_iitbracing 0:30ff725706d2 58 float error_prev = 0; //error at t0
ediff_iitbracing 0:30ff725706d2 59 float error_new; //error at t
ediff_iitbracing 0:30ff725706d2 60 float wratio_actual;
ediff_iitbracing 0:30ff725706d2 61 float w_ratio; //ratio of controller voltages - Right/Left
ediff_iitbracing 0:30ff725706d2 62
ediff_iitbracing 0:30ff725706d2 63 //virtual sensors
ediff_iitbracing 0:30ff725706d2 64 float steering2, throttle2, yaw_rate;
ediff_iitbracing 0:30ff725706d2 65 float yaw_rate_desired;
ediff_iitbracing 0:30ff725706d2 66
ediff_iitbracing 0:30ff725706d2 67 // - - - For RPM Measurement - - -
ediff_iitbracing 0:30ff725706d2 68
ediff_iitbracing 0:30ff725706d2 69 //rpm counts
ediff_iitbracing 0:30ff725706d2 70 volatile int cnt_Left = 0, cnt_Right = 0;
ediff_iitbracing 0:30ff725706d2 71
ediff_iitbracing 0:30ff725706d2 72 //Left and Right rear RPMs feedback-input
ediff_iitbracing 0:30ff725706d2 73 float rpm_Left;
ediff_iitbracing 0:30ff725706d2 74 float rpm_Right;
ediff_iitbracing 0:30ff725706d2 75
ediff_iitbracing 0:30ff725706d2 76 //RPM feedback pins
ediff_iitbracing 0:30ff725706d2 77 AnalogIn steeringread(p16);
ediff_iitbracing 0:30ff725706d2 78 AnalogIn throttleread(p17);
ediff_iitbracing 0:30ff725706d2 79
ediff_iitbracing 0:30ff725706d2 80 // take interrupts (pins 25 and 23)
ediff_iitbracing 0:30ff725706d2 81 InterruptIn wheel_Left_intr(wheel_Left_pin);
ediff_iitbracing 0:30ff725706d2 82 InterruptIn wheel_Right_intr(wheel_Right_pin);
ediff_iitbracing 0:30ff725706d2 83
ediff_iitbracing 0:30ff725706d2 84 //RPM count function
ediff_iitbracing 0:30ff725706d2 85 void wheel_Left_interrupt(void);
ediff_iitbracing 0:30ff725706d2 86 void wheel_Right_interrupt(void);
ediff_iitbracing 0:30ff725706d2 87
ediff_iitbracing 0:30ff725706d2 88 //Timer for RPM measurement
ediff_iitbracing 0:30ff725706d2 89 Timer rpm_timer;
ediff_iitbracing 0:30ff725706d2 90
ediff_iitbracing 0:30ff725706d2 91 //Ticker (for RPM measurement Timer Reset) and corresponding Reset Function
ediff_iitbracing 0:30ff725706d2 92 Ticker ForTimerReset;
ediff_iitbracing 0:30ff725706d2 93 void TimerReset();
ediff_iitbracing 0:30ff725706d2 94
ediff_iitbracing 0:30ff725706d2 95 //RPM Measurement Timer Readings for Current/Prev pulse of Left/Right Wheel Sensor
ediff_iitbracing 0:30ff725706d2 96 volatile int Current_time_Left = 0,Prev_time_Left = 0;
ediff_iitbracing 0:30ff725706d2 97 volatile int Current_time_Right = 0,Prev_time_Right = 0;
ediff_iitbracing 0:30ff725706d2 98
ediff_iitbracing 0:30ff725706d2 99 //Difference between Timer Readings for consecutive pulses i.e. time for 1 revolution
ediff_iitbracing 0:30ff725706d2 100 volatile float time_elapsed_Left;
ediff_iitbracing 0:30ff725706d2 101 volatile float time_elapsed_Right;
ediff_iitbracing 0:30ff725706d2 102
ediff_iitbracing 0:30ff725706d2 103 // - - - FOR FMEA - - -
ediff_iitbracing 0:30ff725706d2 104
ediff_iitbracing 0:30ff725706d2 105 //FMEA output
ediff_iitbracing 0:30ff725706d2 106 bool shutdown= false; //Shutdown Car
ediff_iitbracing 0:30ff725706d2 107 bool Dash = false; //Display Message on Dash - give option to driver to switch to openloop
ediff_iitbracing 0:30ff725706d2 108
ediff_iitbracing 0:30ff725706d2 109 //Display Indicators for FMEA testing
ediff_iitbracing 0:30ff725706d2 110 volatile bool displayLeftrpm = false;
ediff_iitbracing 0:30ff725706d2 111 volatile bool displayDash = false;
ediff_iitbracing 0:30ff725706d2 112 volatile bool displayLeftdiff = false;
ediff_iitbracing 0:30ff725706d2 113 volatile bool displayRightrpm = false;
ediff_iitbracing 0:30ff725706d2 114 volatile bool displayRightdiff = false;
ediff_iitbracing 0:30ff725706d2 115
ediff_iitbracing 0:30ff725706d2 116 //For Missing Teeth RPM FMEA
ediff_iitbracing 0:30ff725706d2 117
ediff_iitbracing 0:30ff725706d2 118 //Pulse Time measurement variables
ediff_iitbracing 0:30ff725706d2 119 volatile int CurrentLeftMissingTeethTimeElapsed=0, PrevLeftMissingTeethTimeElapsed = 0,
ediff_iitbracing 0:30ff725706d2 120 CurrentDiffLeftMissingTeethTimeElapsed = 0,PrevDiffLeftMissingTeethTimeElapsed = 0;
ediff_iitbracing 0:30ff725706d2 121
ediff_iitbracing 0:30ff725706d2 122 volatile int CurrentRightMissingTeethTimeElapsed=0, PrevRightMissingTeethTimeElapsed = 0,
ediff_iitbracing 0:30ff725706d2 123 CurrentDiffRightMissingTeethTimeElapsed = 0,PrevDiffRightMissingTeethTimeElapsed = 0;
ediff_iitbracing 0:30ff725706d2 124
ediff_iitbracing 0:30ff725706d2 125 /*
ediff_iitbracing 0:30ff725706d2 126
ediff_iitbracing 0:30ff725706d2 127 (Current/Prev)(Left/Right)MissingTeethTimeElapsed - Timer reading for current/previous pulse of Left/Right RPM pulses
ediff_iitbracing 0:30ff725706d2 128 (Current/Prev)Diff(Left/Right)MissingTeethTimeElapsed - Difference between timer readings of consecutive pulse
ediff_iitbracing 0:30ff725706d2 129 i.e. width of current/prev pulse of of Left/Right RPM pulses
ediff_iitbracing 0:30ff725706d2 130 */
ediff_iitbracing 0:30ff725706d2 131
ediff_iitbracing 0:30ff725706d2 132 //Flags for starting Left/Right Missing Teeth RPM FMEA
ediff_iitbracing 0:30ff725706d2 133 volatile bool MissingTeethCheckRight = false,MissingTeethCheckLeft = true;
ediff_iitbracing 0:30ff725706d2 134 volatile bool ToggleMissingTeethCheck = false;
ediff_iitbracing 0:30ff725706d2 135
ediff_iitbracing 0:30ff725706d2 136 //Missing Teeth Count
ediff_iitbracing 0:30ff725706d2 137 int countLeftMissingTeeth = 0,countRightMissingTeeth = 0;
ediff_iitbracing 0:30ff725706d2 138
ediff_iitbracing 0:30ff725706d2 139 //Ratio between times of consecutive pulses - to check for missing teeth - (will be >=2 in case of missing teeth)
ediff_iitbracing 1:fec1d091fa34 140 float ratio,ratio1;
ediff_iitbracing 0:30ff725706d2 141
ediff_iitbracing 0:30ff725706d2 142 //Missing Teeth FMEA Call Function and corresponding Ticker
ediff_iitbracing 0:30ff725706d2 143
ediff_iitbracing 0:30ff725706d2 144 Ticker ForFmeaMissingTeeth;
ediff_iitbracing 0:30ff725706d2 145 void FmeaMissingTeeth();
ediff_iitbracing 0:30ff725706d2 146
ediff_iitbracing 0:30ff725706d2 147 // - - - Function Definitions - - -
ediff_iitbracing 0:30ff725706d2 148
ediff_iitbracing 0:30ff725706d2 149 int main()
ediff_iitbracing 0:30ff725706d2 150 {
ediff_iitbracing 0:30ff725706d2 151 for_algo.attach_us(&ediff_func,(algoperiod)); //call ediff func every "algoperiod" sec
ediff_iitbracing 0:30ff725706d2 152 ForFmeaMissingTeeth.attach(&FmeaMissingTeeth, 5.0); //call Missing Teeth RPM FMEA every 5 sec
ediff_iitbracing 0:30ff725706d2 153 ForTimerReset.attach(&TimerReset,100); //Reset RPM measurement timer every 100 sec
ediff_iitbracing 0:30ff725706d2 154
ediff_iitbracing 0:30ff725706d2 155 //RPM attach functions on rise value
ediff_iitbracing 0:30ff725706d2 156 wheel_Left_intr.rise(&wheel_Left_interrupt);
ediff_iitbracing 0:30ff725706d2 157 wheel_Right_intr.rise(&wheel_Right_interrupt);
ediff_iitbracing 0:30ff725706d2 158
ediff_iitbracing 0:30ff725706d2 159 //start the wheel rom timers
ediff_iitbracing 0:30ff725706d2 160 rpm_timer.start();
ediff_iitbracing 0:30ff725706d2 161
ediff_iitbracing 0:30ff725706d2 162 while(1)
ediff_iitbracing 0:30ff725706d2 163 {
ediff_iitbracing 0:30ff725706d2 164 if (displayLeftrpm)
ediff_iitbracing 0:30ff725706d2 165 {
ediff_iitbracing 1:fec1d091fa34 166 printf("time_elapsed_Left %f, %f\n",rpm_Left,time_elapsed_Left);
ediff_iitbracing 0:30ff725706d2 167 displayLeftrpm = false;
ediff_iitbracing 0:30ff725706d2 168 }
ediff_iitbracing 1:fec1d091fa34 169 /*
ediff_iitbracing 0:30ff725706d2 170 if (displayLeftdiff)
ediff_iitbracing 0:30ff725706d2 171 {
ediff_iitbracing 0:30ff725706d2 172 printf("CurrentDiffLeftMissingTeethTimeElapsed %d \n",CurrentDiffLeftMissingTeethTimeElapsed);
ediff_iitbracing 0:30ff725706d2 173 displayLeftdiff = false;
ediff_iitbracing 0:30ff725706d2 174 }
ediff_iitbracing 1:fec1d091fa34 175 */
ediff_iitbracing 0:30ff725706d2 176 if (displayRightrpm)
ediff_iitbracing 0:30ff725706d2 177 {
ediff_iitbracing 1:fec1d091fa34 178 printf("time_elapsed_Right %f, %f\n",rpm_Right,time_elapsed_Right);
ediff_iitbracing 0:30ff725706d2 179 displayRightrpm = false;
ediff_iitbracing 0:30ff725706d2 180 }
ediff_iitbracing 1:fec1d091fa34 181 /*
ediff_iitbracing 0:30ff725706d2 182 if (displayRightdiff)
ediff_iitbracing 0:30ff725706d2 183 {
ediff_iitbracing 0:30ff725706d2 184 printf("CurrentDiffRightMissingTeethTimeElapsed %d \n",CurrentDiffRightMissingTeethTimeElapsed);
ediff_iitbracing 0:30ff725706d2 185 displayRightdiff = false;
ediff_iitbracing 1:fec1d091fa34 186 }
ediff_iitbracing 1:fec1d091fa34 187 */
ediff_iitbracing 0:30ff725706d2 188 if (displayDash)
ediff_iitbracing 0:30ff725706d2 189 {
ediff_iitbracing 1:fec1d091fa34 190 printf("\n \n HOLE MISSING OPENLOOP %f \n \n", ratio1);
ediff_iitbracing 0:30ff725706d2 191 displayDash = false;
ediff_iitbracing 0:30ff725706d2 192 }
ediff_iitbracing 0:30ff725706d2 193 }
ediff_iitbracing 0:30ff725706d2 194 }
ediff_iitbracing 0:30ff725706d2 195
ediff_iitbracing 0:30ff725706d2 196 //Interrupt Function for measuring Left Wheel RPM and checking Left Sensor Disk Missing Teeth FMEA
ediff_iitbracing 0:30ff725706d2 197 void wheel_Left_interrupt()
ediff_iitbracing 0:30ff725706d2 198 {
ediff_iitbracing 0:30ff725706d2 199 cnt_Left++; //Left RPM pulse count
ediff_iitbracing 0:30ff725706d2 200
ediff_iitbracing 0:30ff725706d2 201 if(cnt_Left >= 12) //Measure time between 12 holes => 1 revolution
ediff_iitbracing 0:30ff725706d2 202 {
ediff_iitbracing 1:fec1d091fa34 203 Current_time_Left = rpm_timer.read_us(); //Read Current Timer Reading
ediff_iitbracing 0:30ff725706d2 204 time_elapsed_Left = Current_time_Left - Prev_time_Left; //Difference between Current and precious Timer readings - time for 1 revolution
ediff_iitbracing 0:30ff725706d2 205 Prev_time_Left = Current_time_Left; // Set Current Reading as previous reading for next iteration
ediff_iitbracing 0:30ff725706d2 206 cnt_Left = 0; //Reset Count
ediff_iitbracing 0:30ff725706d2 207 rpm_Left = 1 / ( time_elapsed_Left / (1000000*60) ); //Evaluate RPM from time for 1 revolution
ediff_iitbracing 0:30ff725706d2 208 displayLeftrpm = true; //Display RPM - for testing purposes only
ediff_iitbracing 0:30ff725706d2 209 }
ediff_iitbracing 0:30ff725706d2 210
ediff_iitbracing 0:30ff725706d2 211 if (MissingTeethCheckLeft) //Called every 10 sec
ediff_iitbracing 0:30ff725706d2 212 {
ediff_iitbracing 0:30ff725706d2 213 countLeftMissingTeeth++;
ediff_iitbracing 0:30ff725706d2 214 if (countLeftMissingTeeth == 1) //No comparisons for first tooth ( 12th is compared with first)
ediff_iitbracing 0:30ff725706d2 215 {
ediff_iitbracing 0:30ff725706d2 216 PrevLeftMissingTeethTimeElapsed = rpm_timer.read_ms(); // Set Current Timer Reading as previous reading for next iteration
ediff_iitbracing 0:30ff725706d2 217 }
ediff_iitbracing 0:30ff725706d2 218 else
ediff_iitbracing 0:30ff725706d2 219 {
ediff_iitbracing 0:30ff725706d2 220 CurrentLeftMissingTeethTimeElapsed = rpm_timer.read_ms(); //Read Current Timer Reading
ediff_iitbracing 0:30ff725706d2 221
ediff_iitbracing 0:30ff725706d2 222 CurrentDiffLeftMissingTeethTimeElapsed = CurrentLeftMissingTeethTimeElapsed - PrevLeftMissingTeethTimeElapsed;
ediff_iitbracing 0:30ff725706d2 223 //Difference between Current and precious Timer readings - current pulse time
ediff_iitbracing 0:30ff725706d2 224 PrevLeftMissingTeethTimeElapsed = CurrentLeftMissingTeethTimeElapsed;
ediff_iitbracing 0:30ff725706d2 225 //Set Current Timer Reading as previous reading for next iteration
ediff_iitbracing 0:30ff725706d2 226 ratio = ((float)CurrentDiffLeftMissingTeethTimeElapsed/PrevDiffLeftMissingTeethTimeElapsed);
ediff_iitbracing 0:30ff725706d2 227 //Ratio between consecutive pulses
ediff_iitbracing 0:30ff725706d2 228
ediff_iitbracing 0:30ff725706d2 229 if (ratio > 1.5 || ratio < 1/1.5) //If teeth are missing ratio will be >=2
ediff_iitbracing 0:30ff725706d2 230 {
ediff_iitbracing 1:fec1d091fa34 231 ratio1 = ratio;
ediff_iitbracing 0:30ff725706d2 232 Dash = true;
ediff_iitbracing 0:30ff725706d2 233 displayDash = true;
ediff_iitbracing 0:30ff725706d2 234 }
ediff_iitbracing 0:30ff725706d2 235
ediff_iitbracing 0:30ff725706d2 236 PrevDiffLeftMissingTeethTimeElapsed = CurrentDiffLeftMissingTeethTimeElapsed;
ediff_iitbracing 0:30ff725706d2 237 //Set Current pulse time as previous pulse time for next pulse
ediff_iitbracing 0:30ff725706d2 238 displayLeftdiff = true;
ediff_iitbracing 0:30ff725706d2 239 }
ediff_iitbracing 0:30ff725706d2 240
ediff_iitbracing 0:30ff725706d2 241 if (countLeftMissingTeeth > 13) //Stop once all (12) teeth have been checked
ediff_iitbracing 0:30ff725706d2 242 {
ediff_iitbracing 0:30ff725706d2 243 MissingTeethCheckLeft = false;
ediff_iitbracing 0:30ff725706d2 244 countLeftMissingTeeth = 0;
ediff_iitbracing 0:30ff725706d2 245 }
ediff_iitbracing 0:30ff725706d2 246 }
ediff_iitbracing 0:30ff725706d2 247 }
ediff_iitbracing 0:30ff725706d2 248
ediff_iitbracing 0:30ff725706d2 249 //Interrupt Function for measuring Right Wheel RPM and checking Right Sensor Disk Missing Teeth FMEA
ediff_iitbracing 0:30ff725706d2 250 void wheel_Right_interrupt()
ediff_iitbracing 0:30ff725706d2 251 {
ediff_iitbracing 0:30ff725706d2 252 cnt_Right++; //Right RPM pulse count
ediff_iitbracing 0:30ff725706d2 253
ediff_iitbracing 0:30ff725706d2 254 if(cnt_Right >= 12) //Measure time between 12 holes => 1 revolution
ediff_iitbracing 0:30ff725706d2 255 {
ediff_iitbracing 1:fec1d091fa34 256 Current_time_Right = rpm_timer.read_us(); //Read Current Timer Reading
ediff_iitbracing 0:30ff725706d2 257 time_elapsed_Right = Current_time_Right - Prev_time_Right; //Difference between Current and precious Timer readings - time for 1 revolution
ediff_iitbracing 0:30ff725706d2 258 Prev_time_Right = Current_time_Right; // Set Current Reading as previous reading for next iteration
ediff_iitbracing 0:30ff725706d2 259 cnt_Right = 0; //Reset Count
ediff_iitbracing 0:30ff725706d2 260 rpm_Right = 1 / ( time_elapsed_Right / (1000000*60) ); //Evaluate RPM from time for 1 revolution
ediff_iitbracing 0:30ff725706d2 261 displayRightrpm = true; //Evaluate RPM from time for 1 revolution
ediff_iitbracing 0:30ff725706d2 262 }
ediff_iitbracing 0:30ff725706d2 263
ediff_iitbracing 0:30ff725706d2 264 if (MissingTeethCheckRight) //Called every 10 sec
ediff_iitbracing 0:30ff725706d2 265 {
ediff_iitbracing 0:30ff725706d2 266 countRightMissingTeeth++;
ediff_iitbracing 0:30ff725706d2 267 if (countRightMissingTeeth == 1) //No comparisons for first tooth ( 12th is compared with first)
ediff_iitbracing 0:30ff725706d2 268 {
ediff_iitbracing 0:30ff725706d2 269 PrevRightMissingTeethTimeElapsed = rpm_timer.read_ms(); //Set Current Timer Reading as previous reading for next iteration
ediff_iitbracing 0:30ff725706d2 270 }
ediff_iitbracing 0:30ff725706d2 271 else
ediff_iitbracing 0:30ff725706d2 272 {
ediff_iitbracing 0:30ff725706d2 273 CurrentRightMissingTeethTimeElapsed = rpm_timer.read_ms(); //Read Current Timer Reading
ediff_iitbracing 0:30ff725706d2 274 CurrentDiffRightMissingTeethTimeElapsed = CurrentRightMissingTeethTimeElapsed - PrevRightMissingTeethTimeElapsed;
ediff_iitbracing 0:30ff725706d2 275 //Difference between Current and precious Timer readings - current pulse time
ediff_iitbracing 0:30ff725706d2 276 PrevRightMissingTeethTimeElapsed = CurrentRightMissingTeethTimeElapsed;
ediff_iitbracing 0:30ff725706d2 277 //Set Current Timer Reading as previous reading for next iteration
ediff_iitbracing 0:30ff725706d2 278
ediff_iitbracing 0:30ff725706d2 279 ratio = ((float)CurrentDiffRightMissingTeethTimeElapsed/PrevDiffRightMissingTeethTimeElapsed);
ediff_iitbracing 0:30ff725706d2 280 //Ratio between consecutive pulses
ediff_iitbracing 0:30ff725706d2 281
ediff_iitbracing 0:30ff725706d2 282 if (ratio > 1.5 || ratio < 1/1.5) //If teeth are missing ratio will be >=2
ediff_iitbracing 0:30ff725706d2 283 {
ediff_iitbracing 0:30ff725706d2 284 Dash = true;
ediff_iitbracing 0:30ff725706d2 285 displayDash = true;
ediff_iitbracing 0:30ff725706d2 286 }
ediff_iitbracing 0:30ff725706d2 287
ediff_iitbracing 0:30ff725706d2 288 PrevDiffRightMissingTeethTimeElapsed = CurrentDiffRightMissingTeethTimeElapsed;
ediff_iitbracing 0:30ff725706d2 289 //Set Current pulse time as previous pulse time for next pulse
ediff_iitbracing 0:30ff725706d2 290 displayRightdiff = true;
ediff_iitbracing 0:30ff725706d2 291 }
ediff_iitbracing 0:30ff725706d2 292
ediff_iitbracing 0:30ff725706d2 293 if (countRightMissingTeeth > 13) //Stop once all (12) teeth have been checked
ediff_iitbracing 0:30ff725706d2 294 {
ediff_iitbracing 0:30ff725706d2 295 MissingTeethCheckRight = false;
ediff_iitbracing 0:30ff725706d2 296 countRightMissingTeeth = 0;
ediff_iitbracing 0:30ff725706d2 297 }
ediff_iitbracing 0:30ff725706d2 298 }
ediff_iitbracing 0:30ff725706d2 299 }
ediff_iitbracing 0:30ff725706d2 300
ediff_iitbracing 0:30ff725706d2 301 //Function reads stearing and throttle input, calls PID Function and gives Throttle Left and Right PWM outputs
ediff_iitbracing 0:30ff725706d2 302 void ediff_func()
ediff_iitbracing 0:30ff725706d2 303 {
ediff_iitbracing 0:30ff725706d2 304 steeringread.read();
ediff_iitbracing 0:30ff725706d2 305 throttleread.read(); //read throttle, steering
ediff_iitbracing 0:30ff725706d2 306
ediff_iitbracing 1:fec1d091fa34 307 throttle=throttleread*5;
ediff_iitbracing 1:fec1d091fa34 308 steering=220*steeringread-110; //calibration : throttle in 1 to 5, stering -110 to 110 degrees steering=220*steeringread-110;
ediff_iitbracing 0:30ff725706d2 309
ediff_iitbracing 0:30ff725706d2 310 pid(); // call pid function which gives v1, v2 output
ediff_iitbracing 0:30ff725706d2 311
ediff_iitbracing 0:30ff725706d2 312 //output pwm for Left wheel
ediff_iitbracing 1:fec1d091fa34 313 throttle_Left_pulsewidth = throttle_Left*1000/5;
ediff_iitbracing 0:30ff725706d2 314 throttle_Left_pwmout.pulsewidth_us(throttle_Left_pulsewidth);
ediff_iitbracing 0:30ff725706d2 315 throttle_Left_pwmout.period_us(1000);
ediff_iitbracing 0:30ff725706d2 316
ediff_iitbracing 0:30ff725706d2 317 //output pwm for Right wheel
ediff_iitbracing 1:fec1d091fa34 318 throttle_Right_pulsewidth = throttle_Right*1000/5;
ediff_iitbracing 0:30ff725706d2 319 throttle_Right_pwmout.pulsewidth_us(throttle_Right_pulsewidth);
ediff_iitbracing 0:30ff725706d2 320 throttle_Right_pwmout.period_us(1000);
ediff_iitbracing 0:30ff725706d2 321 }
ediff_iitbracing 0:30ff725706d2 322
ediff_iitbracing 0:30ff725706d2 323 //Resets timer for RPM measurement every 100 sec
ediff_iitbracing 0:30ff725706d2 324 void TimerReset()
ediff_iitbracing 0:30ff725706d2 325 {
ediff_iitbracing 0:30ff725706d2 326 rpm_timer.stop();
ediff_iitbracing 0:30ff725706d2 327 rpm_timer.reset();
ediff_iitbracing 0:30ff725706d2 328 rpm_timer.start();
ediff_iitbracing 0:30ff725706d2 329 }
ediff_iitbracing 0:30ff725706d2 330
ediff_iitbracing 0:30ff725706d2 331 // Sets Flag for starting Left/Right Missing Teeth FMEA after every 5 sec, alternatively
ediff_iitbracing 0:30ff725706d2 332 void FmeaMissingTeeth()
ediff_iitbracing 0:30ff725706d2 333 {
ediff_iitbracing 0:30ff725706d2 334 if (ToggleMissingTeethCheck)
ediff_iitbracing 0:30ff725706d2 335 {
ediff_iitbracing 0:30ff725706d2 336 MissingTeethCheckRight = true;
ediff_iitbracing 0:30ff725706d2 337 }
ediff_iitbracing 0:30ff725706d2 338 else
ediff_iitbracing 0:30ff725706d2 339 {
ediff_iitbracing 0:30ff725706d2 340 MissingTeethCheckLeft = true;
ediff_iitbracing 0:30ff725706d2 341 }
ediff_iitbracing 0:30ff725706d2 342 ToggleMissingTeethCheck = ~ToggleMissingTeethCheck;
ediff_iitbracing 0:30ff725706d2 343 }
ediff_iitbracing 0:30ff725706d2 344