Final working code

Good regulation of height.