Christina Lanier / Mbed 2 deprecated Polynomial_Controller_Car

capstone

Dependencies:   PwmIn mbed

Polynomial_Controller_Car.cpp@0:d1910328124a, 2016-05-03 (annotated)

Committer:
LanierUSNA16
Date:
Tue May 03 15:46:32 2016 +0000
Revision:
0:d1910328124a
final code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LanierUSNA16 0:d1910328124a 1 #include "mbed.h"
LanierUSNA16 0:d1910328124a 2 #include "PwmIn.h"
LanierUSNA16 0:d1910328124a 3 //Initialize Xbee comms
LanierUSNA16 0:d1910328124a 4 Serial xbee1(p9,p10);
LanierUSNA16 0:d1910328124a 5 //Initialize PC comms
LanierUSNA16 0:d1910328124a 6 Serial pc(USBTX, USBRX);
LanierUSNA16 0:d1910328124a 7 //Variables for tripping sensors
LanierUSNA16 0:d1910328124a 8 DigitalInOut Rxsig(p21);
LanierUSNA16 0:d1910328124a 9 DigitalInOut debugOut(p7);
LanierUSNA16 0:d1910328124a 10 //Initialize variables coming from MATLAB
LanierUSNA16 0:d1910328124a 11 int ctrl_flag = 0;
LanierUSNA16 0:d1910328124a 12 float acc_des = 0;
LanierUSNA16 0:d1910328124a 13 float steer_des = 0;
LanierUSNA16 0:d1910328124a 14 char string1[150];
LanierUSNA16 0:d1910328124a 15 char arg1[100];
LanierUSNA16 0:d1910328124a 16 char arg2[100];
LanierUSNA16 0:d1910328124a 17 char arg3[100];
LanierUSNA16 0:d1910328124a 18 //Initialize pins to receive Sensor voltages
LanierUSNA16 0:d1910328124a 19 AnalogIn sensor1(p18); //stbd
LanierUSNA16 0:d1910328124a 20 AnalogIn sensor2 (p19); //mid
LanierUSNA16 0:d1910328124a 21 AnalogIn sensor3(p20); //port
LanierUSNA16 0:d1910328124a 22 //Initialize variables for sensor data
LanierUSNA16 0:d1910328124a 23 float stbd_dist = 0.0, mid_dist = 0.0, port_dist = 0.0; //used by main function
LanierUSNA16 0:d1910328124a 24 float stbdvolt = 0.0, stbddist = 0.0, midvolt = 0.0, middist = 0.0, portvolt = 0.0, portdist = 0.0;//used by individual functions
LanierUSNA16 0:d1910328124a 25 float mindist = 0.0; //used by function ClosestRange
LanierUSNA16 0:d1910328124a 26 float min_dist = 0.0; //used by main function
LanierUSNA16 0:d1910328124a 27 //Initialize variables for translating sensor data to distances
LanierUSNA16 0:d1910328124a 28 float stbdslope = 402.4255;
LanierUSNA16 0:d1910328124a 29 float stbdoffset = 1.5023;
LanierUSNA16 0:d1910328124a 30 float midslope = 351.825;
LanierUSNA16 0:d1910328124a 31 float midoffset = 2.33;
LanierUSNA16 0:d1910328124a 32 float portslope = 383.956;
LanierUSNA16 0:d1910328124a 33 float portoffset = 1.4791;
LanierUSNA16 0:d1910328124a 34 //Variables for finding requested car velocity
LanierUSNA16 0:d1910328124a 35 float ESC_sig = 0.0, vel_req = 0.0, vel_sig = 0.0;
LanierUSNA16 0:d1910328124a 36 PwmIn acc_in(p27);
LanierUSNA16 0:d1910328124a 37 float reverse = 1.0;
LanierUSNA16 0:d1910328124a 38
LanierUSNA16 0:d1910328124a 39 float neutral = 1.38;
LanierUSNA16 0:d1910328124a 40 //float neutral = 1.52;
LanierUSNA16 0:d1910328124a 41 float forward = 1.98;
LanierUSNA16 0:d1910328124a 42
LanierUSNA16 0:d1910328124a 43 float maxspeed = 1.65/1000;
LanierUSNA16 0:d1910328124a 44 //Variables for finding requested steering
LanierUSNA16 0:d1910328124a 45 float steer_sig = 0.0;
LanierUSNA16 0:d1910328124a 46 float steer_req = 1.5/1000;
LanierUSNA16 0:d1910328124a 47 PwmIn steer_in(p28);
LanierUSNA16 0:d1910328124a 48 //Variables for setting car telemetry
LanierUSNA16 0:d1910328124a 49 PwmOut steer_out(p26);
LanierUSNA16 0:d1910328124a 50 PwmOut acc_out(p25);
LanierUSNA16 0:d1910328124a 51 float steer_req_ms =0.0, vel_req_ms=0.0;
LanierUSNA16 0:d1910328124a 52 //Variables for proportional controller
LanierUSNA16 0:d1910328124a 53 float c = 9.0; //Distance from hazard at which car should be stopped
LanierUSNA16 0:d1910328124a 54 float stop_dist = 6.0; //Distance over which the car should come to a stop
LanierUSNA16 0:d1910328124a 55 float b = c + stop_dist;
LanierUSNA16 0:d1910328124a 56 float a1 = -(3*b - c)/(b*b*(b - c)*(b*b - 2*b*c + c*c));
LanierUSNA16 0:d1910328124a 57 float a2 = (2*(2*b*b + 2*b*c - c*c))/(b*b*(b - c)*(b*b - 2*b*c + c*c));
LanierUSNA16 0:d1910328124a 58 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));
LanierUSNA16 0:d1910328124a 59 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));
LanierUSNA16 0:d1910328124a 60 float pcontroller = 0.0; //used by main function
LanierUSNA16 0:d1910328124a 61 float ctrl = 0.0; //called by PController function
LanierUSNA16 0:d1910328124a 62 //Variables for Timing
LanierUSNA16 0:d1910328124a 63 Timer timer;
LanierUSNA16 0:d1910328124a 64 float start_time = 0.0, curr_time = 0.0; //timing done in seconds
LanierUSNA16 0:d1910328124a 65
LanierUSNA16 0:d1910328124a 66 //Function for calculating the closest distance from Stbd sensor
LanierUSNA16 0:d1910328124a 67 float getStbd()
LanierUSNA16 0:d1910328124a 68 {
LanierUSNA16 0:d1910328124a 69 stbdvolt = sensor1.read();
LanierUSNA16 0:d1910328124a 70 stbddist = stbdslope*stbdvolt +stbdoffset;
LanierUSNA16 0:d1910328124a 71 return stbddist;
LanierUSNA16 0:d1910328124a 72 }
LanierUSNA16 0:d1910328124a 73 //Function for calculating the closest distance from Mid sensor
LanierUSNA16 0:d1910328124a 74 float getMid()
LanierUSNA16 0:d1910328124a 75 {
LanierUSNA16 0:d1910328124a 76 midvolt = sensor2.read();
LanierUSNA16 0:d1910328124a 77 middist = midslope*midvolt +midoffset;
LanierUSNA16 0:d1910328124a 78 return middist;
LanierUSNA16 0:d1910328124a 79 }
LanierUSNA16 0:d1910328124a 80 //Function for calculating the closest distance from Port sensor
LanierUSNA16 0:d1910328124a 81 float getPort()
LanierUSNA16 0:d1910328124a 82 {
LanierUSNA16 0:d1910328124a 83 portvolt = sensor3.read();
LanierUSNA16 0:d1910328124a 84 portdist = portslope*portvolt +portoffset;
LanierUSNA16 0:d1910328124a 85 return portdist;
LanierUSNA16 0:d1910328124a 86 }
LanierUSNA16 0:d1910328124a 87 //Function for calculating the overall closest hazard
LanierUSNA16 0:d1910328124a 88 float ClosestRange(float s, float m, float p)
LanierUSNA16 0:d1910328124a 89 {
LanierUSNA16 0:d1910328124a 90 if ((p<=m)&&(p<=s)) {
LanierUSNA16 0:d1910328124a 91 mindist = p;
LanierUSNA16 0:d1910328124a 92 } else if ((m<=p)&&(m<=s)) {
LanierUSNA16 0:d1910328124a 93 mindist = m;
LanierUSNA16 0:d1910328124a 94 } else {
LanierUSNA16 0:d1910328124a 95 mindist = s;
LanierUSNA16 0:d1910328124a 96 }
LanierUSNA16 0:d1910328124a 97 return mindist;
LanierUSNA16 0:d1910328124a 98 }
LanierUSNA16 0:d1910328124a 99
LanierUSNA16 0:d1910328124a 100 //Function to find the requested car velocity, given the control flag value
LanierUSNA16 0:d1910328124a 101 float CarVel(int a, float b, float p)
LanierUSNA16 0:d1910328124a 102 {
LanierUSNA16 0:d1910328124a 103 if (a ==0) {
LanierUSNA16 0:d1910328124a 104 //Use Joystick steering and acceleration commands
LanierUSNA16 0:d1910328124a 105 ESC_sig = acc_in.pulsewidth();
LanierUSNA16 0:d1910328124a 106 if (ESC_sig > maxspeed) {
LanierUSNA16 0:d1910328124a 107 ESC_sig = maxspeed;
LanierUSNA16 0:d1910328124a 108 }
LanierUSNA16 0:d1910328124a 109 acc_des = ESC_sig*1000;
LanierUSNA16 0:d1910328124a 110 }
LanierUSNA16 0:d1910328124a 111 if(a ==1) {
LanierUSNA16 0:d1910328124a 112 //Use Matlab steering and acceleration commands
LanierUSNA16 0:d1910328124a 113 ESC_sig = b/1000;
LanierUSNA16 0:d1910328124a 114 }
LanierUSNA16 0:d1910328124a 115
LanierUSNA16 0:d1910328124a 116 //vel_sig = p*ESC_sig;
LanierUSNA16 0:d1910328124a 117
LanierUSNA16 0:d1910328124a 118 if (ESC_sig <= neutral/1000) {
LanierUSNA16 0:d1910328124a 119 vel_sig = ESC_sig;
LanierUSNA16 0:d1910328124a 120 }
LanierUSNA16 0:d1910328124a 121 if (ESC_sig > neutral/1000) {
LanierUSNA16 0:d1910328124a 122 vel_sig = p*(ESC_sig - neutral/1000) + neutral/1000;
LanierUSNA16 0:d1910328124a 123 }
LanierUSNA16 0:d1910328124a 124
LanierUSNA16 0:d1910328124a 125 if (vel_sig > maxspeed) {
LanierUSNA16 0:d1910328124a 126 vel_sig = maxspeed;
LanierUSNA16 0:d1910328124a 127 }
LanierUSNA16 0:d1910328124a 128 return vel_sig;
LanierUSNA16 0:d1910328124a 129 }
LanierUSNA16 0:d1910328124a 130
LanierUSNA16 0:d1910328124a 131 //Function to find the request car steering, given the control flag value
LanierUSNA16 0:d1910328124a 132 float CarSteer(int c, float d)
LanierUSNA16 0:d1910328124a 133 {
LanierUSNA16 0:d1910328124a 134 if(c==0) {
LanierUSNA16 0:d1910328124a 135 //Use Joystick steering commands
LanierUSNA16 0:d1910328124a 136 steer_sig = steer_in.pulsewidth();
LanierUSNA16 0:d1910328124a 137 }
LanierUSNA16 0:d1910328124a 138 if (c==1) {
LanierUSNA16 0:d1910328124a 139 //Use Matlab steering commands
LanierUSNA16 0:d1910328124a 140 steer_sig = d/1000;
LanierUSNA16 0:d1910328124a 141 }
LanierUSNA16 0:d1910328124a 142 if ((steer_sig <1.1/1000)&&(steer_sig>0.0000)) {
LanierUSNA16 0:d1910328124a 143 steer_sig = 1.1/1000;
LanierUSNA16 0:d1910328124a 144 } else if(steer_sig == 0.0) {
LanierUSNA16 0:d1910328124a 145 steer_sig = 1.45/1000;
LanierUSNA16 0:d1910328124a 146 }
LanierUSNA16 0:d1910328124a 147 if (steer_sig>1.9/1000) {
LanierUSNA16 0:d1910328124a 148 steer_sig = 1.9/1000;
LanierUSNA16 0:d1910328124a 149 }
LanierUSNA16 0:d1910328124a 150 return steer_sig;
LanierUSNA16 0:d1910328124a 151 }
LanierUSNA16 0:d1910328124a 152 //Function for calculating the proportional controller value given range information
LanierUSNA16 0:d1910328124a 153 float PController(float min)
LanierUSNA16 0:d1910328124a 154 {
LanierUSNA16 0:d1910328124a 155 ctrl = a1*min*min*min*min + a2*min*min*min + a3*min*min + a4*min;
LanierUSNA16 0:d1910328124a 156 if (min>=b) {
LanierUSNA16 0:d1910328124a 157 ctrl = 1;
LanierUSNA16 0:d1910328124a 158 }
LanierUSNA16 0:d1910328124a 159 if(min<=c) {
LanierUSNA16 0:d1910328124a 160 ctrl = 0;
LanierUSNA16 0:d1910328124a 161 }
LanierUSNA16 0:d1910328124a 162 if(ctrl>1) {
LanierUSNA16 0:d1910328124a 163 ctrl = 1;
LanierUSNA16 0:d1910328124a 164 }
LanierUSNA16 0:d1910328124a 165 return ctrl;
LanierUSNA16 0:d1910328124a 166 }
LanierUSNA16 0:d1910328124a 167 //Main function
LanierUSNA16 0:d1910328124a 168 int main()
LanierUSNA16 0:d1910328124a 169 {
LanierUSNA16 0:d1910328124a 170 pc.baud(921600);
LanierUSNA16 0:d1910328124a 171 acc_out.period_us(20000);
LanierUSNA16 0:d1910328124a 172 steer_out.period_us(20000);
LanierUSNA16 0:d1910328124a 173 xbee1.baud(115200);
LanierUSNA16 0:d1910328124a 174 timer.start();
LanierUSNA16 0:d1910328124a 175 start_time = timer.read_us();
LanierUSNA16 0:d1910328124a 176
LanierUSNA16 0:d1910328124a 177 //Trip sonar sequencing
LanierUSNA16 0:d1910328124a 178 Rxsig = 0;
LanierUSNA16 0:d1910328124a 179 Rxsig.output();
LanierUSNA16 0:d1910328124a 180 wait(0.005);
LanierUSNA16 0:d1910328124a 181 Rxsig = 1;
LanierUSNA16 0:d1910328124a 182 wait (0.03);
LanierUSNA16 0:d1910328124a 183 Rxsig = 0;
LanierUSNA16 0:d1910328124a 184 Rxsig.input();
LanierUSNA16 0:d1910328124a 185
LanierUSNA16 0:d1910328124a 186 int sendDataIdx = 1;
LanierUSNA16 0:d1910328124a 187 while(1) {
LanierUSNA16 0:d1910328124a 188 //Get MATLAB Info
LanierUSNA16 0:d1910328124a 189 if (xbee1.readable()) {
LanierUSNA16 0:d1910328124a 190 xbee1.scanf("%s",&string1);
LanierUSNA16 0:d1910328124a 191 }
LanierUSNA16 0:d1910328124a 192 //Parse string
LanierUSNA16 0:d1910328124a 193 sscanf(string1,"%[^,\n],%[^,\n],%s",&arg1,&arg2,&arg3);
LanierUSNA16 0:d1910328124a 194 sscanf(arg1,"%d",&ctrl_flag);
LanierUSNA16 0:d1910328124a 195 sscanf(arg2,"%f",&acc_des);
LanierUSNA16 0:d1910328124a 196 sscanf(arg3,"%f",&steer_des);
LanierUSNA16 0:d1910328124a 197 //Get sensor readings
LanierUSNA16 0:d1910328124a 198 stbd_dist = getStbd();
LanierUSNA16 0:d1910328124a 199 mid_dist = getMid();
LanierUSNA16 0:d1910328124a 200 port_dist = getPort();
LanierUSNA16 0:d1910328124a 201 //Determine closest object/min distance
LanierUSNA16 0:d1910328124a 202 min_dist = ClosestRange(stbd_dist,mid_dist,port_dist);
LanierUSNA16 0:d1910328124a 203 //Calculate Proportional Controller Value
LanierUSNA16 0:d1910328124a 204 pcontroller = PController(min_dist);
LanierUSNA16 0:d1910328124a 205 //Get car speed
LanierUSNA16 0:d1910328124a 206 vel_req = CarVel(ctrl_flag,acc_des,pcontroller);
LanierUSNA16 0:d1910328124a 207 //Get car steering
LanierUSNA16 0:d1910328124a 208 steer_req = CarSteer(ctrl_flag,steer_des);
LanierUSNA16 0:d1910328124a 209 //Set car speed & steering
LanierUSNA16 0:d1910328124a 210 steer_out.pulsewidth(steer_req);
LanierUSNA16 0:d1910328124a 211 acc_out.pulsewidth(vel_req);
LanierUSNA16 0:d1910328124a 212 //Send car telemetry and relevant variables to MATLAB via Xbee
LanierUSNA16 0:d1910328124a 213 steer_req_ms = 1000*steer_req;
LanierUSNA16 0:d1910328124a 214 vel_req_ms = 1000*vel_req;
LanierUSNA16 0:d1910328124a 215 curr_time = timer.read();
LanierUSNA16 0:d1910328124a 216 //Only send every 10th data point
LanierUSNA16 0:d1910328124a 217 if (sendDataIdx > 10) {
LanierUSNA16 0:d1910328124a 218 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);
LanierUSNA16 0:d1910328124a 219 sendDataIdx = 1;
LanierUSNA16 0:d1910328124a 220 } else {
LanierUSNA16 0:d1910328124a 221 sendDataIdx++;
LanierUSNA16 0:d1910328124a 222 }
LanierUSNA16 0:d1910328124a 223 //wait(0.05);
LanierUSNA16 0:d1910328124a 224 //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);
LanierUSNA16 0:d1910328124a 225 }
LanierUSNA16 0:d1910328124a 226 }