added startup safety gain scheduling feature

fixed command issues
need to fix gaiting issues