Prathamesh Joshi / Mbed 2 deprecated EDiffMain

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.

algo.cpp@5:ee37889edcab, 2013-03-09 (annotated)

Committer:
prathamesh1729
Date:
Sat Mar 09 06:10:08 2013 +0000
Revision:
5:ee37889edcab
Parent:
4:66b2a94a607f 
Last commit before end-of-day.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
prathamesh1729 0:cee310e0c668 1 #include "algo.h"
prathamesh1729 0:cee310e0c668 2
prathamesh1729 0:cee310e0c668 3 //Function takes steering value in degrees and returns corresponding turn radius in meters using linear interpolation
prathamesh1729 0:cee310e0c668 4 //Input = Steering in degrees
prathamesh1729 0:cee310e0c668 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
prathamesh1729 0:cee310e0c668 6 //Output array is stored in array
prathamesh1729 0:cee310e0c668 7 //Output is linearly interpolated
prathamesh1729 0:cee310e0c668 8
prathamesh1729 0:cee310e0c668 9 float pid_output = 0;
prathamesh1729 3:68d91895d991 10 bool flag_throttle_brake_plausibility = false;
prathamesh1729 0:cee310e0c668 11
prathamesh1729 0:cee310e0c668 12 float turnradiusfunction(float steering)
prathamesh1729 0:cee310e0c668 13 {
prathamesh1729 0:cee310e0c668 14 float steering_high,steering_range;
prathamesh1729 0:cee310e0c668 15 float radius,radius_low,radius_high,radius_range;
prathamesh1729 0:cee310e0c668 16
prathamesh1729 0:cee310e0c668 17 int x=steering;
prathamesh1729 0:cee310e0c668 18 float y=steering-x;
prathamesh1729 0:cee310e0c668 19
prathamesh1729 0:cee310e0c668 20 if (y<0.3)
prathamesh1729 0:cee310e0c668 21 {
prathamesh1729 0:cee310e0c668 22 radius_low=turn_radius[3*x-1];
prathamesh1729 0:cee310e0c668 23 radius_high=turn_radius[3*x];
prathamesh1729 0:cee310e0c668 24 steering_high=x+0.3;
prathamesh1729 0:cee310e0c668 25 steering_range=0.3;
prathamesh1729 0:cee310e0c668 26 }
prathamesh1729 0:cee310e0c668 27 else if (y<0.6999)
prathamesh1729 0:cee310e0c668 28 {
prathamesh1729 0:cee310e0c668 29 radius_low=turn_radius[3*x];
prathamesh1729 0:cee310e0c668 30 radius_high=turn_radius[3*x+1];
prathamesh1729 0:cee310e0c668 31 steering_high=x+0.7;
prathamesh1729 0:cee310e0c668 32 steering_range=0.4;
prathamesh1729 0:cee310e0c668 33 }
prathamesh1729 0:cee310e0c668 34 else
prathamesh1729 0:cee310e0c668 35 {
prathamesh1729 0:cee310e0c668 36 radius_low=turn_radius[3*x+1];
prathamesh1729 0:cee310e0c668 37 radius_high=turn_radius[3*x+2];
prathamesh1729 0:cee310e0c668 38 steering_high=x+1;
prathamesh1729 0:cee310e0c668 39 steering_range=0.3;
prathamesh1729 0:cee310e0c668 40 }
prathamesh1729 0:cee310e0c668 41
prathamesh1729 0:cee310e0c668 42 radius_range=radius_low-radius_high;
prathamesh1729 0:cee310e0c668 43 radius=radius_high+radius_range*(steering_high-steering)/steering_range;
prathamesh1729 0:cee310e0c668 44 radius = sqrt(pow(radius,2)-pow(rear_wheelbase,2)); //Convert turnradius wrt CG to turnradius wrt center of rear trackwidth
prathamesh1729 0:cee310e0c668 45 return radius;
prathamesh1729 0:cee310e0c668 46 }
prathamesh1729 0:cee310e0c668 47
prathamesh1729 0:cee310e0c668 48 //Function Desired RPM Ratio from turnradius and applies PID control to error between actual RPM ratio and desired RPM ratio.
prathamesh1729 0:cee310e0c668 49 //Further evalutes desired throttle left and right values from PID output and throttle input
prathamesh1729 0:cee310e0c668 50 void pid()
prathamesh1729 0:cee310e0c668 51 {
prathamesh1729 5:ee37889edcab 52 /* Commented for offline testing.
prathamesh1729 3:68d91895d991 53 if (brake > 10 && throttle > 1.5) // What is max input when brake is NOT actuated???
prathamesh1729 3:68d91895d991 54 {
prathamesh1729 3:68d91895d991 55 flag_throttle_brake_plausibility = true;
prathamesh1729 4:66b2a94a607f 56 FMEALowByte |= 0x10; //0001 0000
prathamesh1729 3:68d91895d991 57 throttle_Left = 0;
prathamesh1729 3:68d91895d991 58 throttle_Right = 0;
prathamesh1729 3:68d91895d991 59 return;
prathamesh1729 3:68d91895d991 60 }
prathamesh1729 3:68d91895d991 61
prathamesh1729 3:68d91895d991 62 if (flag_throttle_brake_plausibility)
prathamesh1729 3:68d91895d991 63 {
prathamesh1729 3:68d91895d991 64 if (throttle > 0.7)
prathamesh1729 3:68d91895d991 65 {
prathamesh1729 3:68d91895d991 66 throttle_Left = 0;
prathamesh1729 3:68d91895d991 67 throttle_Right = 0;
prathamesh1729 3:68d91895d991 68 return;
prathamesh1729 3:68d91895d991 69 }
prathamesh1729 3:68d91895d991 70 else
prathamesh1729 3:68d91895d991 71 {
prathamesh1729 3:68d91895d991 72 flag_throttle_brake_plausibility = false;
prathamesh1729 4:66b2a94a607f 73 FMEALowByte &= 0xEF; //1110 1111
prathamesh1729 3:68d91895d991 74 }
prathamesh1729 3:68d91895d991 75 }
prathamesh1729 3:68d91895d991 76
prathamesh1729 4:66b2a94a607f 77 if (((flagsFromICAP & 0x78) && !openloop_driver) || (useBoschSteeringRequest)) //Till driver reads Dash message and takes necessary action - openloop/use_Bosch_steering,do soft shutdown - throttle out = 0
prathamesh1729 3:68d91895d991 78 {
prathamesh1729 3:68d91895d991 79 throttle_Left = 0;
prathamesh1729 3:68d91895d991 80 throttle_Right = 0;
prathamesh1729 3:68d91895d991 81 return;
prathamesh1729 3:68d91895d991 82 }
prathamesh1729 3:68d91895d991 83
prathamesh1729 0:cee310e0c668 84 // if any rpm values leas than 10, make it 10
prathamesh1729 0:cee310e0c668 85 if (rpm_Left <= dead_rpm)
prathamesh1729 0:cee310e0c668 86 {
prathamesh1729 0:cee310e0c668 87 rpm_Left = dead_rpm;
prathamesh1729 0:cee310e0c668 88 }
prathamesh1729 0:cee310e0c668 89
prathamesh1729 0:cee310e0c668 90 if (rpm_Right <= dead_rpm)
prathamesh1729 0:cee310e0c668 91 {
prathamesh1729 0:cee310e0c668 92 rpm_Right = dead_rpm;
prathamesh1729 0:cee310e0c668 93 }
prathamesh1729 5:ee37889edcab 94 */
prathamesh1729 0:cee310e0c668 95
prathamesh1729 0:cee310e0c668 96 // steering angle to turnradius
prathamesh1729 0:cee310e0c668 97 if (abs(steering)<7)
prathamesh1729 0:cee310e0c668 98 {
prathamesh1729 0:cee310e0c668 99 turnradius = 1200; // if steering input is within 7 degrees, turnradius is inf(1200)
prathamesh1729 0:cee310e0c668 100 }
prathamesh1729 0:cee310e0c668 101 else
prathamesh1729 0:cee310e0c668 102 {
prathamesh1729 0:cee310e0c668 103 turnradius = turnradiusfunction(abs(steering));
prathamesh1729 0:cee310e0c668 104 }
prathamesh1729 0:cee310e0c668 105
prathamesh1729 0:cee310e0c668 106 //turnradius to RPM ratio desired
prathamesh1729 0:cee310e0c668 107 if(abs(steering) < dead_steering)
prathamesh1729 0:cee310e0c668 108 {
prathamesh1729 0:cee310e0c668 109 wratio_desired = 1; //if steering is within dead_steering limit, then no e-diff voltage split
prathamesh1729 0:cee310e0c668 110 }
prathamesh1729 0:cee310e0c668 111 else if(steering > 0)
prathamesh1729 0:cee310e0c668 112 {
prathamesh1729 0:cee310e0c668 113 wratio_desired = (turnradius - trackwidth/2)/(turnradius + trackwidth/2);
prathamesh1729 0:cee310e0c668 114 }
prathamesh1729 0:cee310e0c668 115 else
prathamesh1729 0:cee310e0c668 116 {
prathamesh1729 0:cee310e0c668 117 wratio_desired = (turnradius + trackwidth/2)/(turnradius - trackwidth/2);
prathamesh1729 0:cee310e0c668 118 }
prathamesh1729 0:cee310e0c668 119
prathamesh1729 0:cee310e0c668 120 //pid calculations
prathamesh1729 0:cee310e0c668 121
prathamesh1729 0:cee310e0c668 122 if (!openloop_lowrpm && !openloop_driver)
prathamesh1729 0:cee310e0c668 123 {
prathamesh1729 0:cee310e0c668 124 wratio_actual = rpm_Right/rpm_Left;
prathamesh1729 0:cee310e0c668 125
prathamesh1729 0:cee310e0c668 126 error_new = wratio_desired - wratio_actual; //error
prathamesh1729 0:cee310e0c668 127 derivative = (error_new-error_prev)/timeint; //derivative of error
prathamesh1729 0:cee310e0c668 128 if(integral < integral_saturation) //saturate the integral part
prathamesh1729 0:cee310e0c668 129 {
prathamesh1729 0:cee310e0c668 130 integral += error_new*timeint; //integral of error
prathamesh1729 0:cee310e0c668 131 }
prathamesh1729 0:cee310e0c668 132
prathamesh1729 0:cee310e0c668 133 pid_output = kp*error_new + ki*integral + kd*derivative + c; //PID output
prathamesh1729 0:cee310e0c668 134
prathamesh1729 0:cee310e0c668 135 error_prev = error_new;
prathamesh1729 0:cee310e0c668 136 }
prathamesh1729 0:cee310e0c668 137 else
prathamesh1729 0:cee310e0c668 138 {
prathamesh1729 0:cee310e0c668 139 pid_output = 0;
prathamesh1729 0:cee310e0c668 140 }
prathamesh1729 0:cee310e0c668 141 w_ratio = pid_output + wratio_desired; //w ratio desired after feedback
prathamesh1729 0:cee310e0c668 142
prathamesh1729 0:cee310e0c668 143 throttle_Right = 2*throttle/(1+1/w_ratio); // V1, V2 from throttle input and w ratio desired after feedback
prathamesh1729 0:cee310e0c668 144 throttle_Left = 2*throttle/(1+w_ratio);
prathamesh1729 3:68d91895d991 145
prathamesh1729 3:68d91895d991 146
prathamesh1729 0:cee310e0c668 147 //Incase either V1 or V2 desired exceeds full_throttle, saturate it to full_throttle - in this case desired ratio will not be maintained
prathamesh1729 0:cee310e0c668 148 if(throttle_Right > full_throttle)
prathamesh1729 0:cee310e0c668 149 {
prathamesh1729 0:cee310e0c668 150 throttle_Right = full_throttle;
prathamesh1729 0:cee310e0c668 151 }
prathamesh1729 0:cee310e0c668 152
prathamesh1729 0:cee310e0c668 153 if (throttle_Left > full_throttle)
prathamesh1729 0:cee310e0c668 154 {
prathamesh1729 0:cee310e0c668 155 throttle_Left = full_throttle;
prathamesh1729 0:cee310e0c668 156 }
prathamesh1729 0:cee310e0c668 157 }