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@4:66b2a94a607f, 2013-03-08 (annotated)

Committer:
prathamesh1729
Date:
Fri Mar 08 21:54:08 2013 +0000
Revision:
4:66b2a94a607f
Parent:
3:68d91895d991
Child:
5:ee37889edcab
Changes before Offline Testing

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 3:68d91895d991 52 if (brake > 10 && throttle > 1.5) // What is max input when brake is NOT actuated???
prathamesh1729 3:68d91895d991 53 {
prathamesh1729 3:68d91895d991 54 flag_throttle_brake_plausibility = true;
prathamesh1729 4:66b2a94a607f 55 FMEALowByte |= 0x10; //0001 0000
prathamesh1729 3:68d91895d991 56 throttle_Left = 0;
prathamesh1729 3:68d91895d991 57 throttle_Right = 0;
prathamesh1729 3:68d91895d991 58 return;
prathamesh1729 3:68d91895d991 59 }
prathamesh1729 3:68d91895d991 60
prathamesh1729 3:68d91895d991 61 if (flag_throttle_brake_plausibility)
prathamesh1729 3:68d91895d991 62 {
prathamesh1729 3:68d91895d991 63 if (throttle > 0.7)
prathamesh1729 3:68d91895d991 64 {
prathamesh1729 3:68d91895d991 65 throttle_Left = 0;
prathamesh1729 3:68d91895d991 66 throttle_Right = 0;
prathamesh1729 3:68d91895d991 67 return;
prathamesh1729 3:68d91895d991 68 }
prathamesh1729 3:68d91895d991 69 else
prathamesh1729 3:68d91895d991 70 {
prathamesh1729 3:68d91895d991 71 flag_throttle_brake_plausibility = false;
prathamesh1729 4:66b2a94a607f 72 FMEALowByte &= 0xEF; //1110 1111
prathamesh1729 3:68d91895d991 73 }
prathamesh1729 3:68d91895d991 74 }
prathamesh1729 3:68d91895d991 75
prathamesh1729 4:66b2a94a607f 76 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 77 {
prathamesh1729 3:68d91895d991 78 throttle_Left = 0;
prathamesh1729 3:68d91895d991 79 throttle_Right = 0;
prathamesh1729 3:68d91895d991 80 return;
prathamesh1729 3:68d91895d991 81 }
prathamesh1729 3:68d91895d991 82
prathamesh1729 0:cee310e0c668 83 // if any rpm values leas than 10, make it 10
prathamesh1729 0:cee310e0c668 84 if (rpm_Left <= dead_rpm)
prathamesh1729 0:cee310e0c668 85 {
prathamesh1729 0:cee310e0c668 86 rpm_Left = dead_rpm;
prathamesh1729 0:cee310e0c668 87 }
prathamesh1729 0:cee310e0c668 88
prathamesh1729 0:cee310e0c668 89 if (rpm_Right <= dead_rpm)
prathamesh1729 0:cee310e0c668 90 {
prathamesh1729 0:cee310e0c668 91 rpm_Right = dead_rpm;
prathamesh1729 0:cee310e0c668 92 }
prathamesh1729 0:cee310e0c668 93
prathamesh1729 0:cee310e0c668 94 // steering angle to turnradius
prathamesh1729 0:cee310e0c668 95 if (abs(steering)<7)
prathamesh1729 0:cee310e0c668 96 {
prathamesh1729 0:cee310e0c668 97 turnradius = 1200; // if steering input is within 7 degrees, turnradius is inf(1200)
prathamesh1729 0:cee310e0c668 98 }
prathamesh1729 0:cee310e0c668 99 else
prathamesh1729 0:cee310e0c668 100 {
prathamesh1729 0:cee310e0c668 101 turnradius = turnradiusfunction(abs(steering));
prathamesh1729 0:cee310e0c668 102 }
prathamesh1729 0:cee310e0c668 103
prathamesh1729 0:cee310e0c668 104 //turnradius to RPM ratio desired
prathamesh1729 0:cee310e0c668 105 if(abs(steering) < dead_steering)
prathamesh1729 0:cee310e0c668 106 {
prathamesh1729 0:cee310e0c668 107 wratio_desired = 1; //if steering is within dead_steering limit, then no e-diff voltage split
prathamesh1729 0:cee310e0c668 108 }
prathamesh1729 0:cee310e0c668 109 else if(steering > 0)
prathamesh1729 0:cee310e0c668 110 {
prathamesh1729 0:cee310e0c668 111 wratio_desired = (turnradius - trackwidth/2)/(turnradius + trackwidth/2);
prathamesh1729 0:cee310e0c668 112 }
prathamesh1729 0:cee310e0c668 113 else
prathamesh1729 0:cee310e0c668 114 {
prathamesh1729 0:cee310e0c668 115 wratio_desired = (turnradius + trackwidth/2)/(turnradius - trackwidth/2);
prathamesh1729 0:cee310e0c668 116 }
prathamesh1729 0:cee310e0c668 117
prathamesh1729 0:cee310e0c668 118 //pid calculations
prathamesh1729 0:cee310e0c668 119
prathamesh1729 0:cee310e0c668 120 if (!openloop_lowrpm && !openloop_driver)
prathamesh1729 0:cee310e0c668 121 {
prathamesh1729 0:cee310e0c668 122 wratio_actual = rpm_Right/rpm_Left;
prathamesh1729 0:cee310e0c668 123
prathamesh1729 0:cee310e0c668 124 error_new = wratio_desired - wratio_actual; //error
prathamesh1729 0:cee310e0c668 125 derivative = (error_new-error_prev)/timeint; //derivative of error
prathamesh1729 0:cee310e0c668 126 if(integral < integral_saturation) //saturate the integral part
prathamesh1729 0:cee310e0c668 127 {
prathamesh1729 0:cee310e0c668 128 integral += error_new*timeint; //integral of error
prathamesh1729 0:cee310e0c668 129 }
prathamesh1729 0:cee310e0c668 130
prathamesh1729 0:cee310e0c668 131 pid_output = kp*error_new + ki*integral + kd*derivative + c; //PID output
prathamesh1729 0:cee310e0c668 132
prathamesh1729 0:cee310e0c668 133 error_prev = error_new;
prathamesh1729 0:cee310e0c668 134 }
prathamesh1729 0:cee310e0c668 135 else
prathamesh1729 0:cee310e0c668 136 {
prathamesh1729 0:cee310e0c668 137 pid_output = 0;
prathamesh1729 0:cee310e0c668 138 }
prathamesh1729 0:cee310e0c668 139 w_ratio = pid_output + wratio_desired; //w ratio desired after feedback
prathamesh1729 0:cee310e0c668 140
prathamesh1729 0:cee310e0c668 141 throttle_Right = 2*throttle/(1+1/w_ratio); // V1, V2 from throttle input and w ratio desired after feedback
prathamesh1729 0:cee310e0c668 142 throttle_Left = 2*throttle/(1+w_ratio);
prathamesh1729 3:68d91895d991 143
prathamesh1729 3:68d91895d991 144
prathamesh1729 0:cee310e0c668 145 //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 146 if(throttle_Right > full_throttle)
prathamesh1729 0:cee310e0c668 147 {
prathamesh1729 0:cee310e0c668 148 throttle_Right = full_throttle;
prathamesh1729 0:cee310e0c668 149 }
prathamesh1729 0:cee310e0c668 150
prathamesh1729 0:cee310e0c668 151 if (throttle_Left > full_throttle)
prathamesh1729 0:cee310e0c668 152 {
prathamesh1729 0:cee310e0c668 153 throttle_Left = full_throttle;
prathamesh1729 0:cee310e0c668 154 }
prathamesh1729 0:cee310e0c668 155 }