Christina Lanier
/
Polynomial_Controller_Car
capstone
Diff: Polynomial_Controller_Car.cpp
- Revision:
- 0:d1910328124a
diff -r 000000000000 -r d1910328124a Polynomial_Controller_Car.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Polynomial_Controller_Car.cpp Tue May 03 15:46:32 2016 +0000 @@ -0,0 +1,226 @@ +#include "mbed.h" +#include "PwmIn.h" +//Initialize Xbee comms +Serial xbee1(p9,p10); +//Initialize PC comms +Serial pc(USBTX, USBRX); +//Variables for tripping sensors +DigitalInOut Rxsig(p21); +DigitalInOut debugOut(p7); +//Initialize variables coming from MATLAB +int ctrl_flag = 0; +float acc_des = 0; +float steer_des = 0; +char string1[150]; +char arg1[100]; +char arg2[100]; +char arg3[100]; +//Initialize pins to receive Sensor voltages +AnalogIn sensor1(p18); //stbd +AnalogIn sensor2 (p19); //mid +AnalogIn sensor3(p20); //port +//Initialize variables for sensor data +float stbd_dist = 0.0, mid_dist = 0.0, port_dist = 0.0; //used by main function +float stbdvolt = 0.0, stbddist = 0.0, midvolt = 0.0, middist = 0.0, portvolt = 0.0, portdist = 0.0;//used by individual functions +float mindist = 0.0; //used by function ClosestRange +float min_dist = 0.0; //used by main function +//Initialize variables for translating sensor data to distances +float stbdslope = 402.4255; +float stbdoffset = 1.5023; +float midslope = 351.825; +float midoffset = 2.33; +float portslope = 383.956; +float portoffset = 1.4791; +//Variables for finding requested car velocity +float ESC_sig = 0.0, vel_req = 0.0, vel_sig = 0.0; +PwmIn acc_in(p27); +float reverse = 1.0; + +float neutral = 1.38; +//float neutral = 1.52; +float forward = 1.98; + +float maxspeed = 1.65/1000; +//Variables for finding requested steering +float steer_sig = 0.0; +float steer_req = 1.5/1000; +PwmIn steer_in(p28); +//Variables for setting car telemetry +PwmOut steer_out(p26); +PwmOut acc_out(p25); +float steer_req_ms =0.0, vel_req_ms=0.0; +//Variables for proportional controller +float c = 9.0; //Distance from hazard at which car should be stopped +float stop_dist = 6.0; //Distance over which the car should come to a stop +float b = c + stop_dist; +float a1 = -(3*b - c)/(b*b*(b - c)*(b*b - 2*b*c + c*c)); +float a2 = (2*(2*b*b + 2*b*c - c*c))/(b*b*(b - c)*(b*b - 2*b*c + c*c)); +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)); +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)); +float pcontroller = 0.0; //used by main function +float ctrl = 0.0; //called by PController function +//Variables for Timing +Timer timer; +float start_time = 0.0, curr_time = 0.0; //timing done in seconds + +//Function for calculating the closest distance from Stbd sensor +float getStbd() +{ + stbdvolt = sensor1.read(); + stbddist = stbdslope*stbdvolt +stbdoffset; + return stbddist; +} +//Function for calculating the closest distance from Mid sensor +float getMid() +{ + midvolt = sensor2.read(); + middist = midslope*midvolt +midoffset; + return middist; +} +//Function for calculating the closest distance from Port sensor +float getPort() +{ + portvolt = sensor3.read(); + portdist = portslope*portvolt +portoffset; + return portdist; +} +//Function for calculating the overall closest hazard +float ClosestRange(float s, float m, float p) +{ + if ((p<=m)&&(p<=s)) { + mindist = p; + } else if ((m<=p)&&(m<=s)) { + mindist = m; + } else { + mindist = s; + } + return mindist; +} + +//Function to find the requested car velocity, given the control flag value +float CarVel(int a, float b, float p) +{ + if (a ==0) { + //Use Joystick steering and acceleration commands + ESC_sig = acc_in.pulsewidth(); + if (ESC_sig > maxspeed) { + ESC_sig = maxspeed; + } + acc_des = ESC_sig*1000; + } + if(a ==1) { + //Use Matlab steering and acceleration commands + ESC_sig = b/1000; + } + + //vel_sig = p*ESC_sig; + + if (ESC_sig <= neutral/1000) { + vel_sig = ESC_sig; + } + if (ESC_sig > neutral/1000) { + vel_sig = p*(ESC_sig - neutral/1000) + neutral/1000; + } + + if (vel_sig > maxspeed) { + vel_sig = maxspeed; + } + return vel_sig; +} + +//Function to find the request car steering, given the control flag value +float CarSteer(int c, float d) +{ + if(c==0) { + //Use Joystick steering commands + steer_sig = steer_in.pulsewidth(); + } + if (c==1) { + //Use Matlab steering commands + steer_sig = d/1000; + } + if ((steer_sig <1.1/1000)&&(steer_sig>0.0000)) { + steer_sig = 1.1/1000; + } else if(steer_sig == 0.0) { + steer_sig = 1.45/1000; + } + if (steer_sig>1.9/1000) { + steer_sig = 1.9/1000; + } + return steer_sig; +} +//Function for calculating the proportional controller value given range information +float PController(float min) +{ + ctrl = a1*min*min*min*min + a2*min*min*min + a3*min*min + a4*min; + if (min>=b) { + ctrl = 1; + } + if(min<=c) { + ctrl = 0; + } + if(ctrl>1) { + ctrl = 1; + } + return ctrl; +} +//Main function +int main() +{ + pc.baud(921600); + acc_out.period_us(20000); + steer_out.period_us(20000); + xbee1.baud(115200); + timer.start(); + start_time = timer.read_us(); + + //Trip sonar sequencing + Rxsig = 0; + Rxsig.output(); + wait(0.005); + Rxsig = 1; + wait (0.03); + Rxsig = 0; + Rxsig.input(); + + int sendDataIdx = 1; + while(1) { + //Get MATLAB Info + if (xbee1.readable()) { + xbee1.scanf("%s",&string1); + } + //Parse string + sscanf(string1,"%[^,\n],%[^,\n],%s",&arg1,&arg2,&arg3); + sscanf(arg1,"%d",&ctrl_flag); + sscanf(arg2,"%f",&acc_des); + sscanf(arg3,"%f",&steer_des); + //Get sensor readings + stbd_dist = getStbd(); + mid_dist = getMid(); + port_dist = getPort(); + //Determine closest object/min distance + min_dist = ClosestRange(stbd_dist,mid_dist,port_dist); + //Calculate Proportional Controller Value + pcontroller = PController(min_dist); + //Get car speed + vel_req = CarVel(ctrl_flag,acc_des,pcontroller); + //Get car steering + steer_req = CarSteer(ctrl_flag,steer_des); + //Set car speed & steering + steer_out.pulsewidth(steer_req); + acc_out.pulsewidth(vel_req); + //Send car telemetry and relevant variables to MATLAB via Xbee + steer_req_ms = 1000*steer_req; + vel_req_ms = 1000*vel_req; + curr_time = timer.read(); + //Only send every 10th data point + if (sendDataIdx > 10) { + 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); + sendDataIdx = 1; + } else { + sendDataIdx++; + } + //wait(0.05); + //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); + } +}