Diff: Polynomial_Controller_Car.cpp

Revision:

0:d1910328124a

--- /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);
+    }
+}