Wrong signal generated by using Servo library[Solved, but keep discussion]

This is my small hexapod with 2 DOF legs, it uses an ATmega328 to control the 12 servos of the legs. Another ATmega328 handles the wireless control (nRF24L01) and controls 1 servo (to sweep the ultrasonic sensor). The first ATmega measures the current drawn from the lipo battery via an analog sensor and measures the lipo voltage through a voltage divider. The second ATmega328 controls 3 RGB leds via transistors, measures the temperature of an LM2576 voltage regulator that powers the servos. It runs a PID loop that pwms a mosfet that powers a small fan to keep the regulator cool. Can't remember to which ATmega the ultrasonic sensor is hooked up :slight_smile: The 2 ATmegas talk to each other over hardware serial. I've used almost all the pins on both ATmegas. I now very little about programming and used the standard servo library. It uses a very simple walking gait, no fancy coding.

I'm currently working on a bigger beast with 18 standard sized metal gear servos (MG996R). The servos are controlled by an Arduino Mega 2560. I've spent the last weeks understanding inverse kinematics and managed to write a walking gait only recently. The Mega has no problems controlling 18 servos. And its calculating a lot of floating point math for the inverse kinematics. I made the following video yesterday: