1

William Krier and Jacob Sonshein ENGR

1410_050 P4_016 11 April 2014
Problem Statement: Program a robot to do a left or right inward spiral
% Variables:
% L - Infinite Loop counter
% distance - distance registered by sensor on Lth loop
% p - power of motors
% m_varB - multifunctional property command variable of motor B
% m_varC - multifunctional property command variable of motor C
% turn -
% Spiral -
% Rev -
% Reset
clear
clear all
clc
COM_CloseNXT('all') % closes any present NXT programs
COM_SetDefaultNXT(COM_OpenNXT()) % Opens NXT program
OpenUltrasonic(SENSOR_4) % opens switch to port of the sensor
pause(1.0) % lets sensor load to full functionaliity before gathering data
% We have a very sensitive sensor reading at an angles
% while true % static sensor testing
% distance = GetUltrasonic( SENSOR_4);
% disp(distance)
% pause(0.5)
% end
% Compensated by drift using a remainder function to give a certain motor an
% extra nudge a percentage of the time just in case. The variable L was
% used to as a loop counter while second input of the remainder function
% acted as an adjustable divisor to the loop number. If the remainder
% equaled zero, in our case, the left motor's power incremented by 1
% allowing the robot to get back on the straight path every 3rd loop.
% However as the robot spirals inward there is less distance available for
% the robot to compensate. A possible solution is to use the Rev variable
% to increase the strength of the compensation as the revolutions increase.
D=menu('Which direction is the robot to turn? ','Left','Right');
while D==0
D=menu('Which direction is the robot to turn? ','Left','Right');
end
L=1; % Loop Number 1
Turn=0 % Initials turn count to 0 on for first revolution
Rev=0 % Initials revolution count
Spiral=35 % initial distance the ultrasonice sensor will reas to decide whether to turn
2
while Rev<4 % the robot will stop at the center after 4 revolutions
distance = GetUltrasonic( SENSOR_4 ); % gathers distance for sensor
p=35; % power set to _ _
if D==1 % if the robot is to turn left
if distance>=Spiral % needs at least a _ _ cm gap for forward motion
m_varB = NXTMotor(MOTOR_B); % defines a variable to motor B(right)
m_varB.Power = p; % gives motor B a power
m_varB.TachoLimit = 0; % axle for B keeps turning
m_varC = NXTMotor(MOTOR_C); % defines a variable to motor C(left)
if rem(L,3)==0 % a certain percentage of the time
m_varC.Power = p+Rev*1+1; % compensates power/maintain regular power
% compensation strengthens after each revolution at a
% chosen factor
else
m_varC.Power = p; % maintain regular power/compensates power
end
% 2nd input of rem & addend to p can be adjusted for improved compensation
m_varC.TachoLimit = 0; % axle for C keeps turing
m_varB.SendToNXT() % sends B commands to B motor
m_varC.SendToNXT() % sends C commands to C motor
elseif distance<Spiral % obstacle within _ _ cm
m_forwardB = NXTMotor(MOTOR_B); % gives a variable to motor B
m_forwardC = NXTMotor(MOTOR_C); % gives a variable to motor C
m_forwardB.Power = 0; % stops B motor
m_forwardC.Power = 0; % stops C motor
m_forwardB.SendToNXT() % executes stopping command for B
m_forwardC.SendToNXT() % executes stopping command for C
m_varB = NXTMotor(MOTOR_B); % gives a variable to motor B
m_varB.Power=p; % regular power
m_varB.TachoLimit=180; % B axle does a half turn for a 90 deg turn
m_varC = NXTMotor(MOTOR_C); % gives a variable to motor C
m_varC.Power=-p; % left motor reverses to allow robot to turn left
m_varC.TachoLimit=169; % C axle almost does a half turn
% lack of differential requires nonequal limits
m_varB.SendToNXT() % executes commands for B
m_varC.SendToNXT() % executes commands for C
m_varB.WaitFor() % Limit>0; lets command complete before additional
m_varC.WaitFor()
Turn=Turn+1 % increases turn count after a turn
end
elseif D==2 % if the robot is to turn right
if distance>=Spiral % needs at least a _ _ cm gap for forward motion
m_varB = NXTMotor(MOTOR_B); % defines a variable to motor B(right)
m_varB.Power = p; % gives motor B a power
m_varB.TachoLimit = 0; % axle for B keeps turning
3
m_varC = NXTMotor(MOTOR_C); % defines a variable to motor C(left)
if rem(L,3)==0 % a certain percentage of the time
m_varC.Power = p+Rev*1+1; % compensates power/maintain regular power
% compensation strengthens after each revolution at a
% chosen factor
else
m_varC.Power = p; % maintain regular power/compensates power
end
% 2nd input of rem & addend to p can be adjusted for improved compensation
m_varC.TachoLimit = 0; % axle for C keeps turing
m_varB.SendToNXT() % sends B commands to B motor
m_varC.SendToNXT() % sends C commands to C motor
elseif distance<Spiral % obstacle within _ _ cm
m_forwardB = NXTMotor(MOTOR_B); % gives a variable to motor B
m_forwardC = NXTMotor(MOTOR_C); % gives a variable to motor C
m_forwardB.Power = 0; % stops B motor
m_forwardC.Power = 0; % stops C motor
m_forwardB.SendToNXT() % executes stopping command for B
m_forwardC.SendToNXT() % executes stopping command for C
m_varB = NXTMotor(MOTOR_B); % gives a variable to motor B
m_varB.Power=-p; % left motor reverses to allow robot to turn left
m_varB.TachoLimit=169; % B axle almost does a half turn
m_varC = NXTMotor(MOTOR_C); % gives a variable to motor C
m_varC.Power=p; % regular power
m_varC.TachoLimit=180; % C axle does a half turn for a 90 deg turn
% lack of differential requires nonequal tacholimits
m_varB.SendToNXT() % executes commands for B
m_varC.SendToNXT() % executes commands for C
m_varB.WaitFor() % Limit>0; lets command complete before additional
m_varC.WaitFor()
Turn=Turn+1 % increases turn count after a turn
end
end
pause(0.5) % lets ultrasonic sensor virtually stop sensing while turning
if Turn==4 % after 4 turns have been executed
Spiral=Spiral+12 % increase the distance to the wall to which the robot turns
Turn=0 % reset turn count
Rev=Rev+1 % one revolution is executed after 4 turns
end
L=L+1; % increments loop counter
end
Published with MATLAB R2013b