Update AS5040 library test program

Simplified