Divide and conquerProgramming a complicated control system can be a tricky business, especially if it is not your primary job description. Quite often there will be a whole raft of different things the system must do, and getting them all working can be a major challenge.
One good technique is to use is Divide and Conquer (D/C). All that means is that rather than write a huge 5000 line program and try and test and debug it all at once, you should break your program down into much smaller chunks and test one at a time.
Doing it the D/C way will certainly result in writing more code, as each "chunk" you write will need some code to help test it. However, that is a very good investment. For example, suppose I need a function that converts a thermistor reading into ºC. I will write a very simple "test harness" (it may only be 5-6 lines of code) that provides my routine a simulated raw reading and displays the result. Having done that I can be very confident that my temperature calculation is working OK, and know that I can safely use it in my larger program as a tried and proven function.
Using D/C has the added benefit of leading to modular programming. It encourages you to break down your program into functional blocks that can not only be tested individually but designed and specified individually.