Added a hardware serial version + testing

There was a problem when using software serial + the servo library. Both
using interrupts -> sometimes when receiving a character the motors
twitched because the servo timing got interrupted by the incoming
character. When using the hardware serial this doesnt occur. To be able
to program the copter when using HWSerial the rx on the arduino has to
be disconnected from the RF-receiver. Added a hardware switch on the
copter to be able to program the copter without having to open the
copter and manually disconnect the RF-receiver.
Also added some testing code to optimize the balancing algorithm.