Merge branch 'main' of https://github.com/alouisrogers/MEMs into main

robotPlotter.asv

@@ -8,47 +8,54 @@ dt = 0.1;
 t = 0:dt:20;
 
 %define properties of first robot class (CCW = + convention)
-robotID = robot name
-phi1    = left wheel velocity
-phi2    = right wheel velocity
-alpha1  = left wheel angle with respect to the positive horizontal plane
-alpha2  = left wheel angle with respect to the positive horizontal plane
-beta1
-beta2
-r1      =left wheel radius
-r2      = right wheel radius
-b       = wheelbase length
-theta0   = initial heading
+    % robotID = robot name
+    % phi1    = left wheel velocity
+    % phi2    = right wheel velocity
+    % alpha1  = left wheel angle with respect to the positive horizontal plane
+    % alpha2  = right wheel angle with respect to the positive horizontal plane
+    % beta1   = left wheel angle with respect to the wheelface normal vector
+    % beta2   = right wheel angle with respect to the wheelface normal vector
+    % r1      = left wheel radius
+    % r2      = right wheel radius
+    % b       = wheelbase length
+    % theta0  = initial heading
+
               %(robotID, phi1, phi2, alpha1, alpha2, beta1, beta2, r1, r2, b, theta0)
-robot1 = basicRobot('robot1', 4, 4, pi/2, -pi/2, 0, pi, 0.0015, 0.0025, 0.005, pi/2);
-
+robot1 = basicRobot('robot1', 4, 4, pi/2, -pi/2, 0, pi, 0.0015, 0.0025, 0.009, pi/2);
 
+%Pre-allocate pose storage vectors for plotting
 poseX = zeros(1,length(t));
 poseY = zeros(1,length(t));
 poseTheta = zeros(1,length(t));
 poseTheta(1) = robot1.theta0;
 
+%Begin time stepping
 for i = 2:length(t)+1
+    %access velocity from robot class static methods
     globalVelocity = robot1.getVelocity(robot1);
     xdot = globalVelocity(1);
     ydot = globalVelocity(2);
     omega = globalVelocity(3);
 
+    %access pose from robot class static methods
     poseChange = robot1.getPose(dt, xdot, ydot, omega);     %returns dx, dy, dtheta (change in each var given over the given step)
     poseX(i) = poseX(i-1) + poseChange(1);
     poseY(i) = poseY(i-1) + poseChange(2);
-    poseThetaLocal = poseTheta(i-1) + poseChange(3);
+    poseThetaLocal = poseTheta(i-1) + poseChange(3);        %accumulate robot heading
 
-    poseTheta(i) = wrapTo2Pi(poseThetaLocal);
+    poseTheta(i) = wrapTo2Pi(poseThetaLocal);               %store and export incremental robot heading
 
-    robot1.theta0 = poseThetaLocal; %update robot heading based on most recent timestep calc
+    robot1.theta0 = poseThetaLocal;                         %update robot heading based on most recent accumulation
 
 end
 
 %Calc Trajectory Diameter
 trajDia = abs(max(poseX) - min(poseX));
 
+%Get robot heading in degrees
 poseTheta = wrapTo360(rad2deg(poseTheta));
+
+%Plotting
 figure(1111)
 %title('Robot Position Over ', num2str(t(end)),' Seconds')
 plot(t,poseY(1:end-1), 'r.')
@@ -57,7 +64,7 @@ plot(t,poseX(1:end-1), 'm.')
 xlabel('time (s)')
 ylabel('X and Y Position (m)')
 title('Robot Position vs. Time Over 20 Seconds')
-legend('t vs. x', 't vs. y')
+legend('t vs. y', 't vs. x')
 
 figure(1112)
 plot(poseX,poseY, 'b.')
@@ -72,6 +79,61 @@ title('Theta vs. Time')
 xlabel('Time (s)')
 ylabel('Mapped Heading (0-360 Degrees)')
 
+figure(1114)
+dia = 0.0762;
+n = 6;
+t = 0:n*pi/(length(t)):n*pi;
+r = dia - cos(t)*0.07*dia;
+%r = wrap2Number(r,1);
+
+% order = 4;
+% framelen = 19;
+% sgf = sgolayfilt(r,order,framelen);
+%plot (t,r)
+%hold on
+% plot(sgf,'.-')
+
+zmin = -0.05;
+zmax = 0.05;
+
+[X,Y,Z] = cylinder(r);
+Z = zmin + (zmax-zmin)*Z;
+hs = surf(X,Z,Y);
+set(hs,'FaceColor',[.7 .7 .7],'FaceAlpha',0.5,'EdgeColor', 'none') ;
+hold on
+
+phi = abs((poseX)/(dia)*2*pi);
+xp = r.*cos(phi);
+yp = r.*sin(phi);
+zp = poseY ;
+
+hp = plot3(xp,zp,yp,'-ok') ;
+
+
+% function lon = wrap2Number(lon,number)
+% 
+% positiveInput = (lon > 0);
+% lon = mod(lon, number);
+% lon((lon == 0) & positiveInput) = number;
+% end
+
+% 
+% z2 = y1;
+% phi = (x1-xmin)/(xmax-xmin)*2*pi;
+% x2 = Rc*cos(phi);
+% y2 = Rc*sin(phi);
+% 
+% %// Plot cylinder
+% [xc yc zc] = cylinder(Rc*ones(1,100),100);
+% zc = zminc + (zmaxc-zminc)*zc;
+% surf(xc,yc,zc)
+% shading flat
+% hold on
+% 
+% %// Plot bent spiral
+% plot3(x2,y2,z2, 'k.-');
+
+
 
 
 

robotPlotter.m

@@ -21,7 +21,7 @@
     % theta0  = initial heading
 
               %(robotID, phi1, phi2, alpha1, alpha2, beta1, beta2, r1, r2, b, theta0)
-robot1 = basicRobot('robot1', 4, 4, pi/2, -pi/2, 0, pi, 0.0015, 0.0025, 0.005, pi/2);
+robot1 = basicRobot('robot1', 4, 4, pi/2, -pi/2, 0, pi, 0.0015, 0.0025, 0.009, pi/2);
 
 %Pre-allocate pose storage vectors for plotting
 poseX = zeros(1,length(t));
@@ -64,7 +64,7 @@
 xlabel('time (s)')
 ylabel('X and Y Position (m)')
 title('Robot Position vs. Time Over 20 Seconds')
-legend('t vs. x', 't vs. y')
+legend('t vs. y', 't vs. x')
 
 figure(1112)
 plot(poseX,poseY, 'b.')
@@ -79,6 +79,64 @@
 xlabel('Time (s)')
 ylabel('Mapped Heading (0-360 Degrees)')
 
+figure(1114)
+dia = 0.0762;
+n = 6;
+t = 0:n*pi/(length(t)):n*pi;
+r = dia - cos(t)*0.07*dia;
+%r = wrap2Number(r,1);
+
+% order = 4;
+% framelen = 19;
+% sgf = sgolayfilt(r,order,framelen);
+%plot (t,r)
+%hold on
+% plot(sgf,'.-')
+
+zmin = -0.05;
+zmax = 0.05;
+
+[X,Y,Z] = cylinder(r);
+Z = zmin + (zmax-zmin)*Z;
+hs = surf(X,Z,Y);
+set(hs,'FaceColor',[.7 .7 .7],'FaceAlpha',0.5,'EdgeColor', 'none') ;
+hold on
+
+%To do: 11/14/20: research how to map similar xp, yp, to x, y of cylinder,
+%basically just want traj to change in axial direction, might need new
+%projection method
+phi = abs((poseX)/(dia)*2*pi);
+xp = r.*cos(phi);
+yp = r.*sin(phi);
+zp = poseY ;
+
+hp = plot3(xp,zp,yp,'-ok') ;
+
+
+% function lon = wrap2Number(lon,number)
+% 
+% positiveInput = (lon > 0);
+% lon = mod(lon, number);
+% lon((lon == 0) & positiveInput) = number;
+% end
+
+% 
+% z2 = y1;
+% phi = (x1-xmin)/(xmax-xmin)*2*pi;
+% x2 = Rc*cos(phi);
+% y2 = Rc*sin(phi);
+% 
+% %// Plot cylinder
+% [xc yc zc] = cylinder(Rc*ones(1,100),100);
+% zc = zminc + (zmaxc-zminc)*zc;
+% surf(xc,yc,zc)
+% shading flat
+% hold on
+% 
+% %// Plot bent spiral
+% plot3(x2,y2,z2, 'k.-');
+
+
 
 
 

wrap_test.m

@@ -0,0 +1,29 @@
+%// Data
+xmin = -3;
+xmax = 3; %// this piece will get folded into a cylinder
+Rc = 5; %// cylinder radius
+zmaxc = 5; %// cylinder max z
+zminc = -5; %// cylinder min z
+
+%// Spiral
+t = linspace(0,1,1000);
+r = 1+2*t;
+theta = 2*pi*3*t;
+x1 = r.*cos(theta);
+y1 = r.*sin(theta); %// example spiral. Defined by x1, y1
+
+%// Do the bending
+z2 = y1;
+phi = (x1-xmin)/(xmax-xmin)*2*pi;
+x2 = Rc*cos(phi);
+y2 = Rc*sin(phi);
+
+%// Plot cylinder
+[xc yc zc] = cylinder(Rc*ones(1,100),100);
+zc = zminc + (zmaxc-zminc)*zc;
+surf(xc,yc,zc)
+shading flat
+hold on
+
+%// Plot bent spiral
+plot3(x2,y2,z2, 'k.-');