Handling error during forward motion

[QuentinTorg]

During straight accel and decel, rangefinder and gyro data will be used to account for encoder error, while the accelerometer can be used to compensate for slip.

Gyro data should be collected and converted into terms of rotation instead of rotational velocity. A rotation that is different from the desired rotation will be a rotational error, and not a positional error. Adding a value X to one encoder and -X to the other encoder will create a rotation of the robot without changing its overall position/velocity/acceleration. A PID loop will be used with this error to calculate how much offset should be written to the encoders. Before this happens though, the error will be manipulated by the rangefinder data.

Rangefinders will keep the robot centered between the maze walls if they are present. Rangefinder data will affect the gyro data before the gyro data is used to adjust the encoder positions. If the rangefinders sense that the robot is closer to one side than the other, then they will impose an intentional error back to the gyro error that has already been collected. This collective error is what will be sent through the PID loop to determine how much the encoders should be adjusted

Accelerometer data will be used to sense slip during acceleration. If the accelerometer senses less distance traveled than the encoders say was traveled, then the accelerometer data will set both encoders back to the actual position, and reinstantiate the motionCalc class to fix the motion profile.

At many open wall junctions, or corners, the rangefinders will see a wall for a split second and then it will disappear. This can be used to reset the encoders to the actual position instead of just the expected position that they and the accelerometers have measured. Should this happen, the motionCalc class will be reinstantiated based on the newest data, and the accelerometer distance will be adjusted to match the same data.

[ghost]

Gyro and range data have been combined in 5d575d3, so that issue is resolved. Created separate issues #44 and #45 for wall transitions and slip correction.