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@0:30ff725706d2, 2013-03-05 (annotated)

Committer:
ediff_iitbracing
Date:
Tue Mar 05 01:19:30 2013 +0000
Revision:
0:30ff725706d2
Child:
1:fec1d091fa34
05-03-2013, 06:51 - Cleaned up code a little. Added some comments.

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