Update AS5040.cpp

Functional program with error reporting