SHARE
TWEET

Untitled

a guest Apr 22nd, 2019 55 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. clear
  2. clc
  3.  
  4. load('torques.mat');            % Ideal torque vector for selected segment  
  5.  
  6. %User Defined Properties
  7. a = arduino('Com3');             % define the Arduino Communication port
  8. xLabel = 'Elapsed Time (s)';    % x-axis label
  9. yLabel = 'Torque(N*m)';         % y-axis label
  10. yMax  = 180;                     % y Maximum Value
  11. yMin  = 0;                       % y minimum Value
  12. min = -180;                     % set y-min
  13. max = 180;                      % set y-max
  14. delay = .4;                     % Refresh rate, determined by target speed
  15.  
  16. %Define Function Variables
  17. time = 0;
  18. data = 0;
  19. data2 = 0;
  20. diff = 0;
  21. nlong = 0;
  22. nlat = 0;
  23. count = 0;
  24.  
  25. % Set up user-input plot
  26. subplot(3, 2, 1)
  27. plotGraph = plot(time,data,'-r' )
  28. hold on                            
  29. title('User Input','FontSize',15);
  30. xlabel(xLabel,'FontSize',15);
  31. ylabel(yLabel,'FontSize',15);
  32. axis([yMin yMax min max]);
  33. grid('on');
  34. tic
  35.  
  36. % Set up ideal-inpit plot
  37. subplot(3, 2, 3)
  38. plotGraph2 = plot(time,data2,'-b' )
  39. hold on                            
  40. title('Simulated Input','FontSize',15);
  41. xlabel(xLabel,'FontSize',15);
  42. ylabel(yLabel,'FontSize',15);
  43. axis([yMin yMax min max]);
  44. grid('on');
  45. tic
  46.  
  47. % Set up difference plot
  48. subplot(3, 2, 5);
  49. plotGraph3 = plot(time,diff,'-g') ;
  50. hold on;                            
  51. title('Difference','FontSize',15);
  52. xlabel(xLabel,'FontSize',15);
  53. ylabel(yLabel,'FontSize',15);
  54. axis([yMin yMax min max]);
  55. grid('on');
  56. tic;
  57.  
  58. % Set up mini-map plot
  59. routename = 'FSGP 2018 MPH.txt';
  60. [xx,yy,elev,dist,lat,long] = loadTrack(routename,2.4);
  61. lapLength = xx(end).*(39.6./12./5280);
  62.  
  63. subplot(3, 2, [2, 4]);
  64. plot(long, lat); title('Full Track');
  65. hold on;
  66. plotGraph4 = plot(nlong, nlat, 'LineWidth', 8);
  67. axis([-98.354 -98.346 40.576 40.582]);
  68. title('Ideal Track Location');
  69. tic;
  70.  
  71.  
  72. % Update plots
  73. while ishandle(plotGraph) %Loop when Plot is Active will run until plot is closed
  74.          deccel = ((a.readVoltage('A0')-4.5601)/-0.5621)*180;
  75.          accel = ((a.readVoltage('A1')-4.6041)/-0.5425)*180;
  76.          dat = accel-deccel; %Data from the arduino
  77.          count = count + 1;    
  78.          time(count) = toc;
  79.          data(count) = dat(1);
  80.          data2(count) = ttt(count);
  81.          diff(count) = data2(count) - data(count);
  82.          nlong(count) = long(count);
  83.          nlat(count) = lat(count);
  84.  
  85.          % Updates exisiting plot instead of replotting
  86.          % Decreases operation time by up to a factor of 20
  87.          set(plotGraph,'XData',time,'YData',data);
  88.          set(plotGraph2,'XData',time,'YData',data2);
  89.          set(plotGraph3,'XData',time,'YData',diff);
  90.          set(plotGraph4,'XData',nlong,'YData',nlat);
  91.          
  92.          pause(delay);
  93. end
  94.  
  95. % Save data for processing in SRSim, display close message
  96. save('Simulation.mat', 'data');
  97. disp('Program closed, output saved to "Simulation.mat"');
  98.  
  99.  
  100.  
  101. function [xx,yy,elev,dist,lat,long,xLong,yLat,dydx,th] = loadTrack(filename,dx)
  102. %% GPS Conversion Constant
  103. latLongDist = 69; %miles between 1 latitude/longitude degree
  104.  
  105. %% Google Earth Text File I/O
  106. i = 1;
  107. fh = fopen(filename);
  108. headers = fgetl(fh); %Extract data headers
  109. line = fgetl(fh);
  110. while ischar(line)
  111.     vals = str2num(line);
  112.     dist(i) = vals(1);
  113.     elev(i) = vals(2);
  114.     lat(i) = vals(3);
  115.     long(i) = vals(4);
  116.    
  117.     line = fgetl(fh);
  118.     i = i + 1;
  119. end
  120. fclose(fh);
  121. y = (elev - min(elev));
  122. x = dist - dist(1);
  123.  
  124. %% Smooth GPS Noise (avoid exploding derivatives)
  125. %ys = y; %don't smooth to save time
  126. ys = smooth(y,0.01,'rlowess');
  127. xx = (x(1):dx:x(end));
  128. yy = (spline(x,ys,xx));
  129. lat = (spline(x,lat,xx));
  130. long = (spline(x,long,xx));
  131. yy = yy - yy(1);
  132. end
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top