Design it right to begin with. block out the code flow with state diagrams and flowcharts.

Then design the code.

Code dummy sections for actual hardware interaction, to return known values until you are sure you are handling what they send back correctly.

Code the hardware drivers.

Test

Iterate.

Rest...