2. Command one robot to target1.

3. Once Task 2 is completed, command the robot to target2, with constant velocity 8 cm/s and no angular velocity (open loop control). This can be done by first pointing the robot to a desired direction at its current location, and then calculating the theoretical number of iterations needed to reach target2 at the given speed. The controller is running at 30 Hz (i.e. one iteration corresponds to about 0.033 seconds).

4. Command the robot to target1.

5. Command the robot to target2 with constant 8 cm/s and a feedback strategy where the angular velocity is adjusted (closed loop control) based on the angular deviation from the direction to the target position. Stop the experiment when the robot reaches the target position, within 1 cm and 0.1 radian accuracy. where k is a gain value of 1.

6. Command the robot to target1

Starter code:

%% Simulator Skeleton File - Project 1

% This file provides the bare-bones requirements for interacting with the

% Robotarium. Note that this code won't actually run. You'll have to

% insert your own algorithm! If you want to see some working code, check

% out the 'examples' folder.

close all

%% Get Robotarium object used to communicate with the robots/simulator

N = 1;

r = Robotarium('NumberOfRobots', N, 'ShowFigure', true);

data = [];

% Select the number of iterations for the experiment.

iterations = 3000; % Do not change

% Create a boundary barrier

uni_barrier_certificate = create_uni_barrier_certificate_with_boundary();

% Other important variables

target1 = [-0.5; 0; 0];

target2 = [0.5; 0; 0];

k = 1;

%%%%%%%%%%%%%%%%%%%%%%%% Place Static Variables Here %%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%% Do not modify anything outside this area %%%%%%%%%%%%%%%

% var = 0;

%%%%%%%%%%%%%% Do not modify anything outside this area %%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Iterate for the previously specified number of iterations

for t = 1:iterations

% Retrieve the most recent poses from the Robotarium. The time delay is

% approximately 0.033 seconds

p = r.get_poses();

%%%%%%%%%%%%%%%%%%%%%%%% Place Algorithm Here %%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%% Do not modify anything outside this area %%%%%%%%%%%%%%%

% u = ???;

% You can try with u = [0.1; 0] and [0; 1] first. Observe what happens to

% get a sense of how it works.

% You should think about implementing a finite-state machine. How many

% states are there? What are the transitions? What signals the transition

% from one state to another?

%%%%%%%%%%%%%% Do not modify anything outside this area %%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Send velocities to agents

% Apply the barrier to the valocities

u = uni_barrier_certificate(u, p);

% Set velocities of agents 1,...,N

r.set_velocities(1:N, u); % u is the input, a 2x1 vector for 1 robot

% Send the previously set velocities to the agents. This function must be called!

data = [data [p; u]];

r.step();

end

% Call r.debug() after our experiment is over!

save('data.mat', 'data');

r.debug(); % Do not modify

%%%%%%%%%%%%%%%%%%%%%%%% Place Helper Functions Here %%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%% Do not modify anything outside this area %%%%%%%%%%%%%%%

% function a = foo(b)

%%%%%%%%%%%%%% Do not modify anything outside this area %%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

example of driving robot to point:

 %%%% This example code is for the Robotarium simulator version 1.1 %%%%%% % Drive one robot to a point on the Robotarium arena % Demonstrates the use of the 'PointControlled' setting in the new % simulator % Siddharth Mayya % 10/17/2017 % Get Robotarium object used to communicate with the robots/simulator rb = RobotariumBuilder(); % For this experiment, we need only one robot N = 1; % Initialize the robotarium object, and specify number of robots, how we % want to control the robots, whether we want collision avoidance etc. r = rb.build('NumberOfAgents', N, 'Dynamics', 'PointControlled', ... 'CollisionAvoidance', true, 'SaveData', true, 'ShowFigure', true); % specify the goal (X,Y) coordinates that you want the robot to end up at. goal = [0;0]; % specify the acceptable error margin between the goal location and final % position of the robot. If this number is too small, the experiment may never % end! error_margin = 0.02; % obtain the current pose of the robot. % 'Point Controlled' and 'SingleIntegrator': % x - 2 X 1 vector - contains x, y position of the robot. % 'PoseControlled': % x - 3 X 1 vector - contains x, y position, and orientation (angle with respect to a horizontal axxis) % of the robot x = r.get_states(); r.step(); % while the distance between the robot and the goal is larger than the % margin.... while norm(x(1:2) - goal) >= error_margin % get the current position of the robot x = r.get_states(); % command the robot (which has index 1) to drive to the goal point r.set_inputs(1,goal); % simulate one step! r.step(); end % always call at the end of an experiment r.call_at_scripts_end();