Fixed IR sensor readings

Added new IR testing functions.