Christina Lanier
/
Polynomial_Controller_Car
capstone
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?
User | Revision | Line number | New 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 | } |