more photos

main.m

@@ -35,33 +35,32 @@
 while scanline(I2, freespace, 10)
     freespace = freespace + dir;
 end
-%freespace = freespace - dir;
-drivebot(1);
+freespace = freespace + dir*robot_size*2;
+drivebot(2);
 dest = [robot(1), freespace]
 
-figure(2)
+[h w] = size(I2);
+h/2
+target(1)
+shift = sign(h/2 - target(1)) * robot_size * 1.5; 
+
+% everybody likes pictures, right?
+f = figure(2);
 imshow(I2)
 drawnow
 hold on;
 plot(dest(2), dest(1), 'ro');  
+plot(freespace, target(1) + shift, 'ro');
+plot(target(2), target(1), 'ro');
 
-drivetopoint
 
 
-% drive vertical till opening
-dest = [target(1) freespace]
-plot(dest(2), dest(1), 'ro');  
-drivetopoint
 
 
+% ready...set...go!
+drivetopoint
+dest = [target(1) + shift freespace]
+drivetopoint
 dest = target'
-
-plot(target(2), target(1), 'ro');  
-
-% drive horizontal till close enough to target point
-
 drivetopoint
-
-
-% we're done.
 stopbot;

movement/drivebot.m

@@ -52,8 +52,6 @@ function drivebot(lspeed, varargin)
 %end
 lspeed = round(lspeed);
 rspeed = round(rspeed);
-lspeed
-rspeed
 send_command(strcat('D,', num2str(lspeed), ',', num2str(rspeed)));
 read_command; 
 pause(pausetime);

movement/pid.m

@@ -15,11 +15,12 @@ function pid(old, new, destination, dt)
 
 % setup constants based on environment
 if strcmp(environment, 'webots')
-  Kp = 1.0;   Ki = 0.05;  Kd = 0.1;
+  %Kp = 1.0;   Ki = 0.04;  Kd = 0.1;
+  Kp = 2;   Ki = 0.03;     Kd = 1.6;
   V = 20;
 else
-  %Kp = 1;   Ki = 0.01;     Kd = 0.25;
-  Kp = 1;   Ki = 0.01;     Kd = 0.20;
+  Kp = 2;   Ki = 0.03;     Kd = 1.6;
+  %Kp = 1.6;   Ki = 0.01;     Kd = 0.25;
   V = 2;
 end
 

rc

@@ -0,0 +1,53 @@
+function rc()
+
+
+global environment;
+global previous_error;
+global l_integral;
+global r_integral;
+robot = [0 0];
+previous_error = 0;
+l_integral = 0;
+r_integral = 0;
+
+% Stop the robot if it's running
+stopbot;
+
+% Prepare vars depending on environment
+if strcmp(environment, 'real world')
+  [robot_size, transformdims, corners, I0, I2, I3] = realworld();
+else
+  [robot_size, transformdims, corners, I0, I2] = webots();
+  I3 = [];
+end
+
+updaterobot;
+% Get the maze to navigate in
+
+
+old_robot = robot;
+old_robot(1) = old_robot(1) - 10;
+
+% find the target point
+target = getendpoints(I1);
+
+
+f = figure(2);
+imshow(I2)
+drawnow
+hold on;
+plot(dest(2), dest(1), 'ro');  
+plot(freespace, target(1) + shift, 'ro');
+plot(target(2), target(1), 'ro');
+
+set(f, 'ButtonDownFcn', @click);
+    function click(varargin)
+        pt = get(f, 'CurrentPoint'); 
+        drivetopoint
+        stopbot
+    end
+
+
+
+
+end

rc.m

@@ -0,0 +1,53 @@
+function rc()
+
+
+global environment;
+global previous_error;
+global l_integral;
+global r_integral;
+robot = [0 0];
+previous_error = 0;
+l_integral = 0;
+r_integral = 0;
+
+% Stop the robot if it's running
+stopbot;
+environment
+% Prepare vars depending on environment
+if strcmp(environment, 'real world')
+  [robot_size, transformdims, corners, I0, I2, I3] = realworld();
+else
+  [robot_size, transformdims, corners, I0, I2] = webots();
+  I3 = [];
+end
+
+updaterobot;
+% Get the maze to navigate in
+
+
+old_robot = robot;
+old_robot(1) = old_robot(1) - 10;
+
+% find the target point
+target = getendpoints(I1);
+
+
+f = figure(2);
+imshow(I2)
+drawnow
+hold on;
+plot(dest(2), dest(1), 'ro');  
+plot(freespace, target(1) + shift, 'ro');
+plot(target(2), target(1), 'ro');
+
+set(f, 'ButtonDownFcn', @click);
+    function click(varargin)
+        pt = get(f, 'CurrentPoint') 
+        drivetopoint
+        stopbot
+    end
+
+
+
+
+end

setups/realworld.m

@@ -1,6 +1,6 @@
 function [robot_size, transformdims, corners, I0, I2, I3] = realworld()
 
-  robot_size = 20;
+  robot_size = 10;
   I0 = take_pic();
   transformdims = [260, 400];
   % use the ptransform transform