Added beggining of adjustment code

Added the start of the adjustment code. Currently it only works
when both sensors are usable.
Ajustments were backwards

The adjustments were backwards :(
Cleaned up the main file

Cleaned out old debugging statements, commented code and unused
variables.