| | 1 | Drive - four parts: preparation, correction for accidental movement sideways, detecting which sensors to trust, and driving. |
| | 2 | Constructor: takes a point on the x y grid. The origin is the bottom left so all values are positive. |
| | 3 | preparation: uses the distance formula to find the distance between the points. |
| | 4 | finds the angle for the robot to turn to in order to face the destination. |
| | 5 | runs the turn command to turn towards the destination. |
| | 6 | resets the encoder values to zero. |
| | 7 | correction for sideways movement: if the built in accelerometer detects acceleration in the x axis (forward/backwards is the y axis, left/right is the x axis) |
| | 8 | slow down the side on the outside of the turn. |
| | 9 | trusting the sensors: if one encoder reads a value that is much larger than the other encoder, use the lower value. |
| | 10 | otherwise, use the average between the two encoder values. |
| | 11 | if the lidar reads a value that is too large, stop using it. |
| | 12 | driving: sets the motors to the driving speed - the corrections from sideways movement. |
