function graph_function(a, u, xprev, yprev, xval, yval) F1 = 'D2'; B1 = 'D4'; F2 = 'D7'; B2 = 'D8'; F3 = 'D11'; B3 = 'D12'; E1 = 'D3'; E2 = 'D6'; E3 = 'D10'; %Fwd and Bck PWMVoltage Range: 0 - 5 %Minimum rotation voltage ~0.8 distance = sqrt((xval-xprev)^2+(yval-yprev)^2) n = ceil(distance/100) xline = linspace(xprev,xval,n); yline = linspace(yprev,yval,n); Abase = []; Bbase = []; Cbase = []; Awire = []; Bwire = []; Cwire = []; for i = 1:1:n Abase = [Abase, floor(sqrt((650+xline(i))^2+(238+yline(i))^2))]; Bbase = [Bbase, floor(sqrt((550-xline(i))^2+(1938-yline(i))^2))]; Cbase = [Cbase, floor(sqrt((1850-xline(i))^2+(238+yline(i))^2))]; Awire = [Awire, floor(sqrt((Abase(i))^2+(900)^2))]; Bwire = [Bwire, floor(sqrt((Bbase(i))^2+(900)^2))]; Cwire = [Cwire, floor(sqrt((Cbase(i))^2+(900)^2))]; end Aspeed = []; Bspeed = []; Cspeed = []; %========ADJUST FOR PROPER SCALING OF CHANGE IN================= %==============DISTANCE TO SPEED================================ timeconstant = 30.0; %Adjust for speed differences for i = 1:1:n-1 Aspeed = [Aspeed, (Awire(i+1)-Awire(i))/timeconstant]; Bspeed = [Bspeed, (Bwire(i+1)-Bwire(i))/timeconstant]; Cspeed = [Cspeed, (Cwire(i+1)-Cwire(i))/timeconstant]; end for i = 1:1:n-1 if Aspeed(i) > 5 Aspeed(i) = 5; end if Aspeed(i) < -5 Bspeed(i) = -5; end if Bspeed(i) > 5 Bspeed(i) = 5; end if Bspeed(i) < -5 Bspeed(i) = -5; end if Cspeed(i) < -5 Cspeed(i) = -5; end if Cspeed(i) > 5 Cspeed(i) = 5; end end tic; height = readDistance(u)*38 for i = 1:1:(n-1) if(Aspeed(i) > 0) writePWMVoltage(a, E1, 0.6*Aspeed(i)); %Loosen writeDigitalPin(a, F1, 1); writeDigitalPin(a, B1, 0); else writePWMVoltage(a, E1, -(0.84)*Aspeed(i)); %Tighten writeDigitalPin(a, F1, 0); writeDigitalPin(a, B1, 1); end if(Bspeed(i) > 0) writePWMVoltage(a, E2, (0.8)*Bspeed(i)); %Loosen writeDigitalPin(a, F2, 1); writeDigitalPin(a, B2, 0); else writePWMVoltage(a, E2, (0.9)*-Bspeed(i)); %Tighten writeDigitalPin(a, F2, 0); writeDigitalPin(a, B2, 1); end if(Cspeed(i) > 0) writePWMVoltage(a, E3, (0.82)*Cspeed(i)); %Loosen writeDigitalPin(a, F3, 1); writeDigitalPin(a, B3, 0); else writePWMVoltage(a, E3, (1)*-Cspeed(i)); %Tighten writeDigitalPin(a, F3, 0); writeDigitalPin(a, B3, 1); end curr_height = readDistance(u)*38; end toc writeDigitalPin(a, F1, 0); writeDigitalPin(a, B1, 0); writeDigitalPin(a, F2, 0); writeDigitalPin(a, B2, 0); writeDigitalPin(a, F3, 0); writeDigitalPin(a, B3, 0); clear a clear u end