Polynomial_Controller_Car.cpp

00001 #include "mbed.h"
00002 #include "PwmIn.h"
00003 //Initialize Xbee comms
00004 Serial xbee1(p9,p10);
00005 //Initialize PC comms
00006 Serial pc(USBTX, USBRX);
00007 //Variables for tripping sensors
00008 DigitalInOut Rxsig(p21);
00009 DigitalInOut debugOut(p7);
00010 //Initialize variables coming from MATLAB
00011 int ctrl_flag = 0;
00012 float acc_des = 0;
00013 float steer_des = 0;
00014 char string1[150];
00015 char arg1[100];
00016 char arg2[100];
00017 char arg3[100];
00018 //Initialize pins to receive Sensor voltages
00019 AnalogIn sensor1(p18); //stbd
00020 AnalogIn sensor2 (p19); //mid
00021 AnalogIn sensor3(p20); //port
00022 //Initialize variables for sensor data
00023 float stbd_dist = 0.0, mid_dist = 0.0, port_dist = 0.0; //used by main function
00024 float stbdvolt = 0.0, stbddist = 0.0, midvolt = 0.0, middist = 0.0, portvolt = 0.0, portdist = 0.0;//used by individual functions
00025 float mindist = 0.0; //used by function ClosestRange
00026 float min_dist = 0.0; //used by main function
00027 //Initialize variables for translating sensor data to distances
00028 float stbdslope = 402.4255;
00029 float stbdoffset = 1.5023;
00030 float midslope = 351.825;
00031 float midoffset = 2.33;
00032 float portslope = 383.956;
00033 float portoffset = 1.4791;
00034 //Variables for finding requested car velocity
00035 float ESC_sig = 0.0, vel_req = 0.0, vel_sig = 0.0;
00036 PwmIn acc_in(p27);
00037 float reverse = 1.0;
00038 
00039 float neutral = 1.38;
00040 //float neutral = 1.52;
00041 float forward = 1.98;
00042 
00043 float maxspeed = 1.65/1000;
00044 //Variables for finding requested steering
00045 float steer_sig = 0.0;
00046 float steer_req = 1.5/1000;
00047 PwmIn steer_in(p28);
00048 //Variables for setting car telemetry
00049 PwmOut steer_out(p26);
00050 PwmOut acc_out(p25);
00051 float steer_req_ms =0.0, vel_req_ms=0.0;
00052 //Variables for proportional controller
00053 float c = 9.0; //Distance from hazard at which car should be stopped
00054 float stop_dist = 6.0; //Distance over which the car should come to a stop
00055 float b = c + stop_dist;
00056 float a1 = -(3*b - c)/(b*b*(b - c)*(b*b - 2*b*c + c*c));
00057 float a2 = (2*(2*b*b + 2*b*c - c*c))/(b*b*(b - c)*(b*b - 2*b*c + c*c));
00058 float a3 = (c*(- 8*b*b + b*c + c*c))/(b*b*(b*b*b - 3*b*b*c + 3*b*c*c - c*c*c));
00059 float a4 = (2*(- c*c*c + 2*b*c*c))/(b*(b*b*b - 3*b*b*c + 3*b*c*c - c*c*c));
00060 float pcontroller = 0.0; //used by main function
00061 float ctrl = 0.0; //called by PController function
00062 //Variables for Timing
00063 Timer timer;
00064 float start_time = 0.0, curr_time = 0.0; //timing done in seconds
00065 
00066 //Function for calculating the closest distance from Stbd sensor
00067 float getStbd()
00068 {
00069     stbdvolt = sensor1.read();
00070     stbddist = stbdslope*stbdvolt +stbdoffset;
00071     return stbddist;
00072 }
00073 //Function for calculating the closest distance from Mid sensor
00074 float getMid()
00075 {
00076     midvolt = sensor2.read();
00077     middist = midslope*midvolt +midoffset;
00078     return middist;
00079 }
00080 //Function for calculating the closest distance from Port sensor
00081 float getPort()
00082 {
00083     portvolt = sensor3.read();
00084     portdist = portslope*portvolt +portoffset;
00085     return portdist;
00086 }
00087 //Function for calculating the overall closest hazard
00088 float ClosestRange(float s, float m, float p)
00089 {
00090     if ((p<=m)&&(p<=s)) {
00091         mindist = p;
00092     } else if ((m<=p)&&(m<=s)) {
00093         mindist = m;
00094     } else {
00095         mindist = s;
00096     }
00097     return mindist;
00098 }
00099 
00100 //Function to find the requested car velocity, given the control flag value
00101 float CarVel(int a, float b, float p)
00102 {
00103     if (a ==0) {
00104         //Use Joystick steering and acceleration commands
00105         ESC_sig = acc_in.pulsewidth();
00106         if (ESC_sig > maxspeed) {
00107             ESC_sig = maxspeed; 
00108             }
00109         acc_des = ESC_sig*1000;
00110     }
00111     if(a ==1) {
00112         //Use Matlab steering and acceleration commands
00113         ESC_sig = b/1000;
00114     }
00115 
00116     //vel_sig = p*ESC_sig;
00117 
00118     if (ESC_sig <= neutral/1000) {
00119         vel_sig = ESC_sig;
00120     }
00121     if (ESC_sig > neutral/1000) {
00122         vel_sig = p*(ESC_sig - neutral/1000) + neutral/1000;
00123     }
00124     
00125     if (vel_sig > maxspeed) {
00126         vel_sig = maxspeed; 
00127     }
00128     return vel_sig;
00129 }
00130 
00131 //Function to find the request car steering, given the control flag value
00132 float CarSteer(int c, float d)
00133 {
00134     if(c==0) {
00135         //Use Joystick steering commands
00136         steer_sig = steer_in.pulsewidth();
00137     }
00138     if (c==1) {
00139         //Use Matlab steering commands
00140         steer_sig = d/1000;
00141     }
00142     if ((steer_sig <1.1/1000)&&(steer_sig>0.0000)) {
00143         steer_sig = 1.1/1000;
00144     } else if(steer_sig == 0.0) {
00145         steer_sig = 1.45/1000;
00146     }
00147     if (steer_sig>1.9/1000) {
00148         steer_sig = 1.9/1000;
00149     }
00150     return steer_sig;
00151 }
00152 //Function for calculating the proportional controller value given range information
00153 float PController(float min)
00154 {
00155     ctrl = a1*min*min*min*min + a2*min*min*min + a3*min*min + a4*min;
00156     if (min>=b) {
00157         ctrl = 1;
00158     }
00159     if(min<=c) {
00160         ctrl = 0;
00161     }
00162     if(ctrl>1) {
00163         ctrl = 1;
00164     }
00165     return ctrl;
00166 }
00167 //Main function
00168 int main()
00169 {
00170     pc.baud(921600);
00171     acc_out.period_us(20000);
00172     steer_out.period_us(20000);
00173     xbee1.baud(115200);
00174     timer.start();
00175     start_time = timer.read_us();
00176 
00177     //Trip sonar sequencing
00178     Rxsig = 0;
00179     Rxsig.output();
00180     wait(0.005);
00181     Rxsig = 1;
00182     wait (0.03);
00183     Rxsig = 0;
00184     Rxsig.input();
00185 
00186     int sendDataIdx = 1;
00187     while(1) {
00188         //Get MATLAB Info
00189         if (xbee1.readable()) {
00190             xbee1.scanf("%s",&string1);
00191         }
00192         //Parse string
00193         sscanf(string1,"%[^,\n],%[^,\n],%s",&arg1,&arg2,&arg3);
00194         sscanf(arg1,"%d",&ctrl_flag);
00195         sscanf(arg2,"%f",&acc_des);
00196         sscanf(arg3,"%f",&steer_des);
00197         //Get sensor readings
00198         stbd_dist = getStbd();
00199         mid_dist = getMid();
00200         port_dist = getPort();
00201         //Determine closest object/min distance
00202         min_dist = ClosestRange(stbd_dist,mid_dist,port_dist);
00203         //Calculate Proportional Controller Value
00204         pcontroller = PController(min_dist);
00205         //Get car speed
00206         vel_req = CarVel(ctrl_flag,acc_des,pcontroller);
00207         //Get car steering
00208         steer_req = CarSteer(ctrl_flag,steer_des);
00209         //Set car speed & steering
00210         steer_out.pulsewidth(steer_req);
00211         acc_out.pulsewidth(vel_req);
00212         //Send car telemetry and relevant variables to MATLAB via Xbee
00213         steer_req_ms = 1000*steer_req;
00214         vel_req_ms = 1000*vel_req;
00215         curr_time = timer.read();
00216         //Only send every 10th data point
00217         if (sendDataIdx > 10) {
00218             xbee1.printf("C:%d, Pc:%f, Va:%f, vc:%f, sta:%f, stc:%f, p:%f, m:%f, s:%f, t:%f \n",ctrl_flag,pcontroller, vel_req_ms, acc_des, steer_req_ms, steer_des, port_dist, mid_dist, stbd_dist, curr_time);
00219             sendDataIdx = 1;
00220         } else {
00221             sendDataIdx++;
00222         }
00223         //wait(0.05);
00224         //pc.printf("C:%d, Pc:%f, Va:%f, vc:%f, sta:%f, stc:%f, p:%f, m:%f, s:%f, t:%f \n",ctrl_flag,pcontroller, vel_req_ms, acc_des, steer_req_ms, steer_des, port_dist, mid_dist, stbd_dist, curr_time);
00225     }
00226 }