ArUco
We adjusted the positions of the landmarks from our previous lab so that it reflected the configuration below. This way, instead of seeing two landmarks initially, Cozmo can only see one. We also used a bearing sensor model instead of a distance sensor model because we were turning Cozmo in order for him to orient himself.  In our finite state machine, we referenced the landmark positioning and particle filter class and programmed Cozmo to turn 90 degrees, move forward, and turn 90 degrees again. When we observed the particle filter, we noticed that the display showed a large cluster of particles initially, dispersed when Cozmo turned, and centralized once he moved forward. Images of the particle filter and Cozmo turning in relation to the landmarks are displayed below.   
  

FindR
In order to find the distance between the center of rotation and the origin of Cozmo (R), we used Pythagorean theorem. Our algorithm is as follows: we have access to the robot’s initial coordinates and those after completing a turn. Using these two points, we are able to calculate the difference in x and the difference in y of Cozmo’s turn. We use these two differences to calculate the length of two legs of a right triangle. If we use Pythagorean’s Theorem to calculate the hypotenuse of the triangle, we arrive at the distance between the origin and the center of rotation because the center does not change as Cozmo turns. In order to implement this algorithm, we used a sender and receiver to send the value of the robot’s x and y positions. We then defined global variables to keep track of the initial x and y position of the robot and a list of all of the R values calculated. We calculated and posted the R value in the receiver, appending each value to the list. Another class calculated the average of the R values and printed this average. Our R values varied based on the number of trials we conducted: values ranged from 20.3 -28.7, most often being in the 25-28 range. Some images of the trial are displayed below.     


FindClosestCube
To find the closest cube, we used a sender and receiver. In the sender node, we calculated the distance between each cube and the robot using the distance formula. We then compared each distance and set the value equal to the cube with the shortest distance from the robot. The receiver took the cube ID and computed the angle needed to turn to face the cube. Because the receiver inherited from the Turn node, Cozmo turned after computing this angle. The finite state machine called the transition from sender to receiver. Some images of the trail are displayed below.   
  



TossBall
We first tried out the MoveLift(speed).now() command in simple_cli. We manually observed the distance the ball traveled after each command, gradually increasing the speed. The results are displayed below. 
Speed
	Distance (in.)
	1
	N/A (did not move)
	5
	0.5
	10
	4.9
	15
	7
	20
	3
	400
	5
	Initially, we used a Toss node that inherited from the MoveLift node that adjusted the speed of the toss. However, this seemed redundant, as the Toss node did not add any value to the MoveLift node itself. Thus, we just added a finite state machine to call MoveLift repeatedly with a time delay in between, ranging from speeds of 1 to 20. An image of the trial is displayed below.