Christina Lanier
/
Polynomial_Controller_Car
capstone
Polynomial_Controller_Car.cpp
- Committer:
- LanierUSNA16
- Date:
- 2016-05-03
- Revision:
- 0:d1910328124a
File content as of revision 0:d1910328124a:
#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); } }