Drive - four parts: preparation, correction for accidental movement sideways, detecting which sensors to trust, and driving.

Constructor: takes a point on the x y grid. The origin is the bottom left so all values are positive. preparation: uses the distance formula to find the distance between the points.

finds the angle for the robot to turn to in order to face the destination. runs the turn command to turn towards the destination. resets the encoder values to zero.

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)

slow down the side on the outside of the turn.

trusting the sensors: if one encoder reads a value that is much larger than the other encoder, use the lower value.

otherwise, use the average between the two encoder values. if the lidar reads a value that is too large, stop using it.

driving: sets the motors to the driving speed - the corrections from sideways movement.