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@0:cee310e0c668, 2013-03-07 (annotated)

Committer:
prathamesh1729
Date:
Thu Mar 07 18:22:38 2013 +0000
Revision:
0:cee310e0c668
Child:
3:68d91895d991
CAN with DAQ, SPI from ICAP updated.

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