# Challenge Statement:

Given a sequence of three input wheel velocities to be used in five second intervals, determine the final position of a fixed point on the robot and its orientation relative to a fixed axis.

# Demo:

1. Use the robot from the previous lab (or build a new one) and make sure that when the motor power level for both left and right motors is set to 100 (maximum power), the robot moves forward at at least 0.15 meters per second. For different kinematic designs make sure that the maximum driving speed of the robot is still at least 0.15 meters per second. If needed a TA may ask you to demonstrate this requirement.
2. Input the three sets of input velocities for both left and right wheels of a differential dirve robot specified by a TA. The values will correspond to a motor power level and will be between -75 and 75.
3. Then, place the robot on the ground and press a button that makes the robot execute the desired sets of motor speeds in five second intervals. For example, if the input set is {{50, 40}, {-30, 40}, {30, 0}}, for the first five seconds, the left and right motor power levels should be 50 and 40 respectively, for the next 5 seconds -30 and 40 and for the last five seconds 30 and 0.
4. Assume that the robot starts in the initial configuration x = 0, y = 0 and θ = 0.
5. After 15 seconds, the robot must stop and report its current position and orientation in meters and degrees respectively.
6. Three tries will be given in order to improve your score if needed. Each try is independent of the other. Progressively easier velocities can be requested, each for a 10 point penalty and only once per try. A request cannot be made on the first try.

# How to:

The following description applies only to a standard differential drive robot.

1. Write a loop that continuously polls the wheel encoders to calculate left and right wheel velocities in terms of ticks per second = degrees per second.
2. Consider the left and right wheel angular velocities in radians per second (,).
=
=

We can convert them to linear velocities (in m/s) by multiplying by the radius of the wheels (R).

From that we can obtain the linear (in m/s) and angular velocity (in rad/sec) for the entire robot.

L (wheel base) : Distance between the point of contact of the two wheels to the ground (in meters)

3. Now that the velocities of the robot have been obtained, it is possible to calculate its position (in meters) and orientation (in radians).

To obtain , we need to solve this non - linear system of differential equations. A numerical integrator like the fourth order Runge - Kutta can be used to approximate the position vector we are looking for. Let t be the time elapsed since we last ran the integration loop (the time step) and n represent the current iteration of the loop.

4. Now that we have position and orientation information with respect to some fixed global coordinate frame, display it and wait for a specified time interval before starting the entire loop again. This is done to ensure that the wheel rotates at least a few degrees so that the encoder readings that are obtained will be accurate.

# Other tips:

1. The PID update interval should be low (5 - 20 ms).
2. The integration loop must also be small (1 - 20 ms).
3. Save this lab. It will be used in the next three labs.
4. Do not use the in-built radiansToDegrees and degreesToRadians functions. They do not handle floating point inputs correctlly.
5. The number inputed as power to the motor does not exactlly correspond to wheel velocity. The relation specified in the RobotC manual however is 1 motor power unit = 10 encoder ticks per second = 10 degrees per second.
6. Do not poll the encoders at extremely small intervals as in such a time step inaccuracies in the encoder ticks (± 1 degree) will cause the velocity estimate to be very inaccurate.
7. There is only one loop in the method described above where wheel velocities are calculate just before running the integration step. It may be beneficial to have them in seperate threads with different update intervals.
8. Play with loop update intervals until error over the fifteen seconds is minimized. Factors like encoder and motor error will add up as we are integrating over time and will cause a drift in position.
9. Having a good position estimate will be crucial in the next few labs so work hard to get this lab working well.
10. Input can be done through a third SERVO motor where encoder clicks correspond to analog input.

Sample starter code: Code fragment written in RobotC.

# Evaluation:

1. Error in final position = ||reported position - actual position|| (out of 70 points) :
i) Error > 5 cm - points
ii) Error 5 cm - 70 points
2. Error in final orientation = |reported orientation in degrees - actual orientation in degrees| (out of 30 points) :
i) Error > 10 degrees - points
ii) Error 10 degrees - 30 points