Add z motor
Fix Z gain