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

algo.cpp@2:eb4101b574bc, 2013-03-06 (annotated)

Committer:
ediff_iitbracing
Date:
Wed Mar 06 19:20:02 2013 +0000
Revision:
2:eb4101b574bc
Parent:
1:fec1d091fa34 
Test publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ediff_iitbracing 0:30ff725706d2 1 #include "algo.h"
ediff_iitbracing 0:30ff725706d2 2
ediff_iitbracing 0:30ff725706d2 3 //Function takes steering value in degrees and returns corresponding turn radius in meters using linear interpolation
ediff_iitbracing 0:30ff725706d2 4 //Input = Steering in degrees
ediff_iitbracing 0:30ff725706d2 5 //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
ediff_iitbracing 0:30ff725706d2 6 //Output array is stored in array
ediff_iitbracing 0:30ff725706d2 7 //Output is linearly interpolated
ediff_iitbracing 0:30ff725706d2 8
ediff_iitbracing 2:eb4101b574bc 9 float pid_output = 0;
ediff_iitbracing 2:eb4101b574bc 10
ediff_iitbracing 0:30ff725706d2 11 float turnradiusfunction(float steering)
ediff_iitbracing 0:30ff725706d2 12 {
ediff_iitbracing 0:30ff725706d2 13 float steering_high,steering_range;
ediff_iitbracing 0:30ff725706d2 14 float radius,radius_low,radius_high,radius_range;
ediff_iitbracing 0:30ff725706d2 15
ediff_iitbracing 0:30ff725706d2 16 int x=steering;
ediff_iitbracing 0:30ff725706d2 17 float y=steering-x;
ediff_iitbracing 0:30ff725706d2 18
ediff_iitbracing 0:30ff725706d2 19 if (y<0.3)
ediff_iitbracing 0:30ff725706d2 20 {
ediff_iitbracing 0:30ff725706d2 21 radius_low=turn_radius[3*x-1];
ediff_iitbracing 0:30ff725706d2 22 radius_high=turn_radius[3*x];
ediff_iitbracing 0:30ff725706d2 23 steering_high=x+0.3;
ediff_iitbracing 0:30ff725706d2 24 steering_range=0.3;
ediff_iitbracing 0:30ff725706d2 25 }
ediff_iitbracing 0:30ff725706d2 26 else if (y<0.6999)
ediff_iitbracing 0:30ff725706d2 27 {
ediff_iitbracing 0:30ff725706d2 28 radius_low=turn_radius[3*x];
ediff_iitbracing 0:30ff725706d2 29 radius_high=turn_radius[3*x+1];
ediff_iitbracing 0:30ff725706d2 30 steering_high=x+0.7;
ediff_iitbracing 0:30ff725706d2 31 steering_range=0.4;
ediff_iitbracing 0:30ff725706d2 32 }
ediff_iitbracing 0:30ff725706d2 33 else
ediff_iitbracing 0:30ff725706d2 34 {
ediff_iitbracing 0:30ff725706d2 35 radius_low=turn_radius[3*x+1];
ediff_iitbracing 0:30ff725706d2 36 radius_high=turn_radius[3*x+2];
ediff_iitbracing 0:30ff725706d2 37 steering_high=x+1;
ediff_iitbracing 0:30ff725706d2 38 steering_range=0.3;
ediff_iitbracing 0:30ff725706d2 39 }
ediff_iitbracing 0:30ff725706d2 40
ediff_iitbracing 0:30ff725706d2 41 radius_range=radius_low-radius_high;
ediff_iitbracing 0:30ff725706d2 42 radius=radius_high+radius_range*(steering_high-steering)/steering_range;
ediff_iitbracing 0:30ff725706d2 43 radius = sqrt(pow(radius,2)-pow(rear_wheelbase,2)); //Convert turnradius wrt CG to turnradius wrt center of rear trackwidth
ediff_iitbracing 0:30ff725706d2 44 return radius;
ediff_iitbracing 0:30ff725706d2 45 }
ediff_iitbracing 0:30ff725706d2 46
ediff_iitbracing 0:30ff725706d2 47 //Function Desired RPM Ratio from turnradius and applies PID control to error between actual RPM ratio and desired RPM ratio.
ediff_iitbracing 0:30ff725706d2 48 //Further evalutes desired throttle left and right values from PID output and throttle input
ediff_iitbracing 0:30ff725706d2 49 void pid()
ediff_iitbracing 0:30ff725706d2 50 {
ediff_iitbracing 2:eb4101b574bc 51 // if any rpm values leas than 10, make it 10
ediff_iitbracing 2:eb4101b574bc 52 if (rpm_Left <= dead_rpm)
ediff_iitbracing 0:30ff725706d2 53 {
ediff_iitbracing 0:30ff725706d2 54 rpm_Left = dead_rpm;
ediff_iitbracing 0:30ff725706d2 55 }
ediff_iitbracing 0:30ff725706d2 56
ediff_iitbracing 2:eb4101b574bc 57 if (rpm_Right <= dead_rpm)
ediff_iitbracing 0:30ff725706d2 58 {
ediff_iitbracing 0:30ff725706d2 59 rpm_Right = dead_rpm;
ediff_iitbracing 0:30ff725706d2 60 }
ediff_iitbracing 0:30ff725706d2 61
ediff_iitbracing 0:30ff725706d2 62 // steering angle to turnradius
ediff_iitbracing 0:30ff725706d2 63 if (abs(steering)<7)
ediff_iitbracing 0:30ff725706d2 64 {
ediff_iitbracing 0:30ff725706d2 65 turnradius = 1200; // if steering input is within 7 degrees, turnradius is inf(1200)
ediff_iitbracing 0:30ff725706d2 66 }
ediff_iitbracing 0:30ff725706d2 67 else
ediff_iitbracing 0:30ff725706d2 68 {
ediff_iitbracing 0:30ff725706d2 69 turnradius = turnradiusfunction(abs(steering));
ediff_iitbracing 0:30ff725706d2 70 }
ediff_iitbracing 0:30ff725706d2 71
ediff_iitbracing 0:30ff725706d2 72 //turnradius to RPM ratio desired
ediff_iitbracing 0:30ff725706d2 73 if(abs(steering) < dead_steering)
ediff_iitbracing 0:30ff725706d2 74 {
ediff_iitbracing 0:30ff725706d2 75 wratio_desired = 1; //if steering is within dead_steering limit, then no e-diff voltage split
ediff_iitbracing 0:30ff725706d2 76 }
ediff_iitbracing 0:30ff725706d2 77 else if(steering > 0)
ediff_iitbracing 0:30ff725706d2 78 {
ediff_iitbracing 0:30ff725706d2 79 wratio_desired = (turnradius - trackwidth/2)/(turnradius + trackwidth/2);
ediff_iitbracing 0:30ff725706d2 80 }
ediff_iitbracing 0:30ff725706d2 81 else
ediff_iitbracing 0:30ff725706d2 82 {
ediff_iitbracing 0:30ff725706d2 83 wratio_desired = (turnradius + trackwidth/2)/(turnradius - trackwidth/2);
ediff_iitbracing 0:30ff725706d2 84 }
ediff_iitbracing 0:30ff725706d2 85
ediff_iitbracing 0:30ff725706d2 86 //pid calculations
ediff_iitbracing 0:30ff725706d2 87
ediff_iitbracing 2:eb4101b574bc 88 if (!openloop)
ediff_iitbracing 0:30ff725706d2 89 {
ediff_iitbracing 2:eb4101b574bc 90 wratio_actual = rpm_Right/rpm_Left;
ediff_iitbracing 2:eb4101b574bc 91
ediff_iitbracing 2:eb4101b574bc 92 error_new = wratio_desired - wratio_actual; //error
ediff_iitbracing 2:eb4101b574bc 93 derivative = (error_new-error_prev)/timeint; //derivative of error
ediff_iitbracing 2:eb4101b574bc 94 if(integral < integral_saturation) //saturate the integral part
ediff_iitbracing 2:eb4101b574bc 95 {
ediff_iitbracing 2:eb4101b574bc 96 integral += error_new*timeint; //integral of error
ediff_iitbracing 2:eb4101b574bc 97 }
ediff_iitbracing 2:eb4101b574bc 98
ediff_iitbracing 2:eb4101b574bc 99 pid_output = kp*error_new + ki*integral + kd*derivative + c; //PID output
ediff_iitbracing 2:eb4101b574bc 100
ediff_iitbracing 2:eb4101b574bc 101 error_prev = error_new;
ediff_iitbracing 2:eb4101b574bc 102 }
ediff_iitbracing 2:eb4101b574bc 103 else
ediff_iitbracing 2:eb4101b574bc 104 {
ediff_iitbracing 2:eb4101b574bc 105 pid_output = 0;
ediff_iitbracing 2:eb4101b574bc 106 }
ediff_iitbracing 0:30ff725706d2 107 w_ratio = pid_output + wratio_desired; //w ratio desired after feedback
ediff_iitbracing 2:eb4101b574bc 108
ediff_iitbracing 0:30ff725706d2 109 throttle_Right = 2*throttle/(1+1/w_ratio); // V1, V2 from throttle input and w ratio desired after feedback
ediff_iitbracing 0:30ff725706d2 110 throttle_Left = 2*throttle/(1+w_ratio);
ediff_iitbracing 0:30ff725706d2 111
ediff_iitbracing 0:30ff725706d2 112 //Incase either V1 or V2 desired exceeds full_throttle, saturate it to full_throttle - in this case desired ratio will not be maintained
ediff_iitbracing 0:30ff725706d2 113 if(throttle_Right > full_throttle)
ediff_iitbracing 0:30ff725706d2 114 {
ediff_iitbracing 0:30ff725706d2 115 throttle_Right = full_throttle;
ediff_iitbracing 0:30ff725706d2 116 }
ediff_iitbracing 0:30ff725706d2 117
ediff_iitbracing 0:30ff725706d2 118 if (throttle_Left > full_throttle)
ediff_iitbracing 0:30ff725706d2 119 {
ediff_iitbracing 0:30ff725706d2 120 throttle_Left = full_throttle;
ediff_iitbracing 0:30ff725706d2 121 }
ediff_iitbracing 0:30ff725706d2 122 }