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